1use crate::geometry::Point;
32
33#[derive(Clone, Copy, Debug, Default, PartialEq)]
35pub struct Velocity {
36 pub x: f32,
38 pub y: f32,
40}
41
42impl Velocity {
43 pub const ZERO: Velocity = Velocity { x: 0.0, y: 0.0 };
45
46 pub fn new(x: f32, y: f32) -> Self {
48 Self { x, y }
49 }
50
51 pub fn magnitude(self) -> f32 {
53 self.x.hypot(self.y)
54 }
55
56 pub fn clamp_magnitude(self, min: f32, max: f32) -> Velocity {
61 debug_assert!(min >= 0.0 && max >= min);
62 let magnitude = self.magnitude();
63 if magnitude == 0.0 {
64 return self;
65 }
66 let target = magnitude.clamp(min, max);
67 if target == magnitude {
68 return self;
69 }
70 let scale = target / magnitude;
71 Velocity::new(self.x * scale, self.y * scale)
72 }
73}
74
75#[derive(Clone, Copy, Debug, PartialEq)]
77pub struct VelocityEstimate {
78 pub velocity: Velocity,
80 pub confidence: f32,
83 pub duration: f32,
85 pub offset: (f32, f32),
89}
90
91impl VelocityEstimate {
92 pub const STILL: VelocityEstimate = VelocityEstimate {
94 velocity: Velocity::ZERO,
95 confidence: 1.0,
96 duration: 0.0,
97 offset: (0.0, 0.0),
98 };
99}
100
101const HISTORY: usize = 20;
104const HORIZON_MS: f32 = 100.0;
106const STOPPED_MS: f32 = 40.0;
109const DELIVERY_GAPS: usize = 3;
111const MAX_DELIVERY_MS: f32 = 50.0;
120const MIN_SAMPLES: usize = 3;
122
123pub const BOUNCING_FLING_WEIGHTS: [f32; 3] = [0.6, 0.35, 0.05];
126pub const DESKTOP_FLING_WEIGHTS: [f32; 3] = [0.15, 0.65, 0.2];
128
129#[derive(Clone, Copy, Debug, PartialEq)]
131pub enum VelocityStrategy {
132 Regression,
136 RecentAverage([f32; 3]),
140}
141
142#[derive(Clone, Copy, Debug)]
143struct Sample {
144 point: Point,
145 time: f32,
147}
148
149#[derive(Clone, Debug)]
154pub struct VelocityTracker {
155 strategy: VelocityStrategy,
156 samples: [Option<Sample>; HISTORY],
157 index: usize,
159}
160
161impl Default for VelocityTracker {
162 fn default() -> Self {
165 Self::new(VelocityStrategy::Regression)
166 }
167}
168
169impl VelocityTracker {
170 pub fn new(strategy: VelocityStrategy) -> Self {
172 Self {
173 strategy,
174 samples: [None; HISTORY],
175 index: 0,
176 }
177 }
178
179 pub fn strategy(&self) -> VelocityStrategy {
181 self.strategy
182 }
183
184 pub fn add_position(&mut self, time: f32, point: Point) {
186 self.index = (self.index + 1) % HISTORY;
187 self.samples[self.index] = Some(Sample { point, time });
188 }
189
190 pub fn clear(&mut self) {
192 self.samples = [None; HISTORY];
193 self.index = 0;
194 }
195
196 pub fn estimate(&self, now: f32) -> Option<VelocityEstimate> {
199 let newest = self.samples[self.index]?;
200 if (now - newest.time) * 1000.0 - self.delivery_ms() > STOPPED_MS {
204 return Some(VelocityEstimate::STILL);
205 }
206 match self.strategy {
207 VelocityStrategy::Regression => Some(self.regress(newest)),
208 VelocityStrategy::RecentAverage(weights) => Some(self.recent_average(newest, weights)),
209 }
210 }
211
212 pub fn velocity(&self, now: f32) -> Velocity {
215 self.estimate(now)
216 .map(|e| e.velocity)
217 .unwrap_or(Velocity::ZERO)
218 }
219
220 fn regress(&self, newest: Sample) -> VelocityEstimate {
223 let mut ages = [0.0f64; HISTORY];
226 let mut xs = [0.0f64; HISTORY];
227 let mut ys = [0.0f64; HISTORY];
228 let mut count = 0;
229 let mut oldest = newest;
230 let mut previous = newest;
231 let mut index = self.index;
232 let delivery = self.delivery_ms();
235
236 while count < HISTORY {
237 let Some(sample) = self.samples[index] else {
238 break;
239 };
240 let age = (newest.time - sample.time) * 1000.0;
241 let gap = ((sample.time - previous.time) * 1000.0).abs();
242 previous = sample;
243 if age > HORIZON_MS + delivery || gap - delivery > STOPPED_MS {
245 break;
246 }
247 oldest = sample;
248 ages[count] = -age as f64;
249 xs[count] = sample.point.x as f64;
250 ys[count] = sample.point.y as f64;
251 count += 1;
252 index = if index == 0 { HISTORY } else { index } - 1;
253 }
254
255 let duration = newest.time - oldest.time;
256 let offset = (
257 newest.point.x - oldest.point.x,
258 newest.point.y - oldest.point.y,
259 );
260 if count >= MIN_SAMPLES {
261 let x_fit = PolynomialFit::solve(&ages[..count], &xs[..count], 2);
262 let y_fit = PolynomialFit::solve(&ages[..count], &ys[..count], 2);
263 if let (Some(x_fit), Some(y_fit)) = (x_fit, y_fit) {
264 return VelocityEstimate {
265 velocity: Velocity::new(
267 (x_fit.coefficients[1] * 1000.0) as f32,
268 (y_fit.coefficients[1] * 1000.0) as f32,
269 ),
270 confidence: (x_fit.confidence * y_fit.confidence) as f32,
271 duration,
272 offset,
273 };
274 }
275 }
276 VelocityEstimate {
279 velocity: Velocity::ZERO,
280 confidence: 1.0,
281 duration,
282 offset,
283 }
284 }
285
286 fn delivery_ms(&self) -> f32 {
292 let mut gaps = [0.0f32; DELIVERY_GAPS];
293 let mut count = 0;
294 let mut index = self.index;
295 while count < DELIVERY_GAPS {
296 let before = if index == 0 { HISTORY } else { index } - 1;
297 let (Some(newer), Some(older)) = (self.samples[index], self.samples[before]) else {
298 break;
299 };
300 gaps[count] = ((newer.time - older.time) * 1000.0).max(0.0);
301 count += 1;
302 index = before;
303 }
304 let gaps = &mut gaps[..count];
305 gaps.sort_by(f32::total_cmp);
306 let pace = match gaps.len() {
307 0 => 0.0,
308 DELIVERY_GAPS => gaps[DELIVERY_GAPS / 2],
309 _ => gaps[0],
310 };
311 pace.min(MAX_DELIVERY_MS)
312 }
313
314 fn pair_velocity(&self, offset: isize) -> Velocity {
316 let size = HISTORY as isize;
317 let end = self.samples[((self.index as isize + offset).rem_euclid(size)) as usize];
318 let start = self.samples[((self.index as isize + offset - 1).rem_euclid(size)) as usize];
319 let (Some(end), Some(start)) = (end, start) else {
320 return Velocity::ZERO;
321 };
322 let dt = end.time - start.time;
323 if dt <= 0.0 {
324 return Velocity::ZERO;
325 }
326 Velocity::new(
327 (end.point.x - start.point.x) / dt,
328 (end.point.y - start.point.y) / dt,
329 )
330 }
331
332 fn recent_average(&self, newest: Sample, weights: [f32; 3]) -> VelocityEstimate {
334 let mut velocity = Velocity::ZERO;
335 for (i, weight) in weights.iter().enumerate() {
336 let pair = self.pair_velocity(i as isize - (weights.len() as isize - 1));
337 velocity.x += pair.x * weight;
338 velocity.y += pair.y * weight;
339 }
340 let mut oldest = newest;
343 for i in 1..=HISTORY {
344 if let Some(sample) = self.samples[(self.index + i) % HISTORY] {
345 oldest = sample;
346 break;
347 }
348 }
349 VelocityEstimate {
350 velocity,
351 confidence: 1.0,
352 duration: newest.time - oldest.time,
353 offset: (
354 newest.point.x - oldest.point.x,
355 newest.point.y - oldest.point.y,
356 ),
357 }
358 }
359}
360
361#[derive(Clone, Copy, Debug, PartialEq)]
365pub struct PolynomialFit {
366 pub coefficients: [f64; 3],
368 pub confidence: f64,
371}
372
373const PRECISION_TOLERANCE: f64 = 1e-10;
375
376impl PolynomialFit {
377 pub fn solve(x: &[f64], y: &[f64], degree: usize) -> Option<PolynomialFit> {
387 debug_assert_eq!(x.len(), y.len());
388 debug_assert!(degree <= 2);
389 let m = x.len();
390 let n = degree + 1;
391 if degree > m {
392 return None; }
394
395 let mut a = vec![0.0f64; n * m];
397 for h in 0..m {
398 a[h] = 1.0;
399 for i in 1..n {
400 a[i * m + h] = a[(i - 1) * m + h] * x[h];
401 }
402 }
403
404 let mut q = vec![0.0f64; n * m];
406 let mut r = vec![0.0f64; n * n];
407 for j in 0..n {
408 for h in 0..m {
409 q[j * m + h] = a[j * m + h];
410 }
411 for i in 0..j {
412 let dot: f64 = (0..m).map(|h| q[j * m + h] * q[i * m + h]).sum();
413 for h in 0..m {
414 q[j * m + h] -= dot * q[i * m + h];
415 }
416 }
417 let norm = (0..m)
418 .map(|h| q[j * m + h] * q[j * m + h])
419 .sum::<f64>()
420 .sqrt();
421 if norm < PRECISION_TOLERANCE {
422 return None; }
424 let inverse = 1.0 / norm;
425 for h in 0..m {
426 q[j * m + h] *= inverse;
427 }
428 for i in 0..n {
429 r[j * n + i] = if i < j {
430 0.0
431 } else {
432 (0..m).map(|h| q[j * m + h] * a[i * m + h]).sum()
433 };
434 }
435 }
436
437 let mut coefficients = [0.0f64; 3];
439 for i in (0..n).rev() {
440 let mut c: f64 = (0..m).map(|h| q[i * m + h] * y[h]).sum();
441 for j in (i + 1..n).rev() {
442 c -= r[i * n + j] * coefficients[j];
443 }
444 coefficients[i] = c / r[i * n + i];
445 }
446
447 let mean = y.iter().sum::<f64>() / m as f64;
449 let mut squared_error = 0.0;
450 let mut squared_total = 0.0;
451 for h in 0..m {
452 let mut term = 1.0;
453 let mut error = y[h] - coefficients[0];
454 for c in coefficients.iter().take(n).skip(1) {
455 term *= x[h];
456 error -= term * c;
457 }
458 squared_error += error * error;
459 let deviation = y[h] - mean;
460 squared_total += deviation * deviation;
461 }
462 let confidence = if squared_total <= PRECISION_TOLERANCE {
463 1.0
464 } else {
465 1.0 - (squared_error / squared_total)
466 };
467
468 Some(PolynomialFit {
469 coefficients,
470 confidence,
471 })
472 }
473}
474
475#[cfg(test)]
476mod tests {
477 use super::*;
478
479 fn steady(tracker: &mut VelocityTracker, speed: f32, samples: usize) -> f32 {
481 let step = 0.008;
482 let mut t = 0.0;
483 for i in 0..samples {
484 t = i as f32 * step;
485 tracker.add_position(t, Point::new(speed * t, 2.0 * speed * t));
486 }
487 t
488 }
489
490 fn feed(tracker: &mut VelocityTracker, deltas: &[f32]) -> f32 {
498 let step = 0.008;
499 let mut t = 0.0;
500 let mut x = 0.0;
501 tracker.add_position(t, Point::new(x, 0.0));
502 for delta in deltas {
503 t += step;
504 x += delta;
505 tracker.add_position(t, Point::new(x, 0.0));
506 }
507 t
508 }
509
510 fn throw_then_lift(tracker: &mut VelocityTracker, slow: usize) -> f32 {
513 let mut deltas = vec![8.0f32; 7];
514 deltas.extend(std::iter::repeat_n(1.0f32, slow));
515 feed(tracker, &deltas)
516 }
517
518 #[test]
519 fn a_steady_drag_is_read_exactly() {
520 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
521 let now = steady(&mut tracker, 600.0, 10);
522 let estimate = tracker.estimate(now).expect("samples were added");
523 assert!(
524 (estimate.velocity.x - 600.0).abs() < 1.0,
525 "x = {}",
526 estimate.velocity.x
527 );
528 assert!(
529 (estimate.velocity.y - 1200.0).abs() < 2.0,
530 "y = {}",
531 estimate.velocity.y
532 );
533 assert!(estimate.confidence > 0.99, "a line fits a line");
534 assert!(estimate.offset.0 > 0.0 && estimate.duration > 0.0);
535 }
536
537 #[test]
538 fn the_regression_survives_a_last_instant_slowdown() {
539 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
540 let now = throw_then_lift(&mut tracker, 2);
541 let estimate = tracker.estimate(now).expect("samples");
542 assert!(
545 estimate.velocity.x > 400.0,
546 "the throw should survive the lift-off, got {}",
547 estimate.velocity.x
548 );
549 }
550
551 #[test]
552 fn a_finger_that_stopped_before_lifting_flings_nothing() {
553 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
554 let last = steady(&mut tracker, 900.0, 8);
555 let estimate = tracker.estimate(last + 0.1).expect("samples");
557 assert_eq!(estimate.velocity, Velocity::ZERO);
558 assert_eq!(estimate, VelocityEstimate::STILL);
559 }
560
561 fn paced(tracker: &mut VelocityTracker, speed: f32, step: f32, samples: usize) -> f32 {
564 let mut t = 0.0;
565 for i in 0..samples {
566 t = i as f32 * step;
567 tracker.add_position(t, Point::new(speed * t, 0.0));
568 }
569 t
570 }
571
572 #[test]
579 fn a_throw_delivered_a_frame_at_a_time_is_still_a_throw() {
580 for strategy in [
581 VelocityStrategy::Regression,
582 VelocityStrategy::RecentAverage(BOUNCING_FLING_WEIGHTS),
583 ] {
584 for step in [0.037, 0.045, 0.051] {
585 let mut tracker = VelocityTracker::new(strategy);
586 let last = paced(&mut tracker, 2000.0, step, 6);
587 let estimate = tracker.estimate(last + 0.041).expect("samples");
588 assert!(
589 (estimate.velocity.x - 2000.0).abs() < 40.0,
590 "{strategy:?} every {step} s: got {}",
591 estimate.velocity.x
592 );
593 }
594 }
595 }
596
597 #[test]
600 fn a_short_throw_at_slow_frames_is_still_a_throw() {
601 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
602 let last = paced(&mut tracker, 2000.0, 0.037, 3);
603 let estimate = tracker.estimate(last + 0.041).expect("samples");
604 assert!(
605 (estimate.velocity.x - 2000.0).abs() < 40.0,
606 "got {}",
607 estimate.velocity.x
608 );
609 }
610
611 #[test]
614 fn slow_frames_do_not_hide_a_finger_that_stopped() {
615 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
616 let last = paced(&mut tracker, 2000.0, 0.037, 6);
617 let estimate = tracker.estimate(last + 0.12).expect("samples");
618 assert_eq!(estimate, VelocityEstimate::STILL);
619 let mut crawling = VelocityTracker::new(VelocityStrategy::Regression);
621 let last = paced(&mut crawling, 2000.0, 0.2, 6);
622 assert_eq!(
623 crawling.estimate(last + 0.1).expect("samples"),
624 VelocityEstimate::STILL
625 );
626 }
627
628 #[test]
631 fn one_short_gap_does_not_change_the_pace() {
632 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
633 for (t, x) in [
634 (0.0, 0.0),
635 (0.037, 74.0),
636 (0.074, 148.0),
637 (0.111, 222.0),
638 (0.116, 232.0),
639 (0.153, 306.0),
640 ] {
641 tracker.add_position(t, Point::new(x, 0.0));
642 }
643 let estimate = tracker.estimate(0.153 + 0.05).expect("samples");
646 assert!(estimate.velocity.x > 1500.0, "got {}", estimate.velocity.x);
647 }
648
649 #[test]
653 fn a_pause_just_before_the_release_excuses_nothing() {
654 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
655 let mut t = 0.0;
656 for i in 0..6 {
657 t = i as f32 * 0.008;
658 tracker.add_position(t, Point::new(i as f32 * 16.0, 0.0));
659 }
660 let stopped = t + 0.06;
661 tracker.add_position(stopped, Point::new(80.0, 0.0));
662 let estimate = tracker.estimate(stopped + 0.005).expect("samples");
663 assert!(
664 estimate.velocity.x < 400.0,
665 "the throw before the pause leaked across it: {}",
666 estimate.velocity.x
667 );
668 assert!(
670 estimate.offset.0.abs() < 1.0 && estimate.duration < 0.01,
671 "the history ran back across the pause: {estimate:?}"
672 );
673 }
674
675 #[test]
676 fn samples_older_than_the_horizon_are_not_part_of_the_motion() {
677 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
678 for i in 0..6 {
681 tracker.add_position(i as f32 * 0.008, Point::new(i as f32 * 16.0, 0.0));
682 }
683 let base = 1.0;
684 for i in 0..6 {
685 tracker.add_position(
686 base + i as f32 * 0.008,
687 Point::new(1000.0 + i as f32 * 1.6, 0.0),
688 );
689 }
690 let now = base + 5.0 * 0.008;
691 let estimate = tracker.estimate(now).expect("samples");
692 assert!(
693 estimate.velocity.x < 400.0,
694 "the old fast gesture leaked in: {}",
695 estimate.velocity.x
696 );
697 assert!(estimate.duration < 0.1, "span = {}", estimate.duration);
698 }
699
700 #[test]
701 fn too_few_samples_give_travel_but_no_speed() {
702 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
703 tracker.add_position(0.0, Point::new(0.0, 0.0));
704 tracker.add_position(0.008, Point::new(10.0, 0.0));
705 let estimate = tracker.estimate(0.008).expect("samples");
706 assert_eq!(
707 estimate.velocity,
708 Velocity::ZERO,
709 "2 samples cannot regress"
710 );
711 assert_eq!(estimate.offset.0, 10.0, "but the travel is still known");
712 }
713
714 #[test]
715 fn an_empty_tracker_has_nothing_to_say() {
716 let tracker = VelocityTracker::new(VelocityStrategy::Regression);
717 assert!(tracker.estimate(0.0).is_none());
718 assert_eq!(tracker.velocity(0.0), Velocity::ZERO);
719 }
720
721 #[test]
722 fn clearing_starts_the_gesture_over() {
723 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
724 let now = steady(&mut tracker, 600.0, 10);
725 tracker.clear();
726 assert!(tracker.estimate(now).is_none());
727 }
728
729 #[test]
730 fn the_recent_average_leans_on_the_older_samples() {
731 let mut tracker =
734 VelocityTracker::new(VelocityStrategy::RecentAverage(BOUNCING_FLING_WEIGHTS));
735 let now = throw_then_lift(&mut tracker, 1);
736 let estimate = tracker.estimate(now).expect("samples");
737 assert!(
740 (estimate.velocity.x - 956.0).abs() < 20.0,
741 "got {}",
742 estimate.velocity.x
743 );
744 }
745
746 #[test]
747 fn the_recent_average_needs_no_history_to_answer() {
748 let mut tracker =
749 VelocityTracker::new(VelocityStrategy::RecentAverage(DESKTOP_FLING_WEIGHTS));
750 tracker.add_position(0.0, Point::new(0.0, 0.0));
751 tracker.add_position(0.008, Point::new(8.0, 0.0));
752 let estimate = tracker.estimate(0.008).expect("samples");
753 assert!(
755 (estimate.velocity.x - 200.0).abs() < 1.0,
756 "got {}",
757 estimate.velocity.x
758 );
759 }
760
761 #[test]
762 fn the_ring_buffer_wraps_without_losing_the_motion() {
763 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
764 let now = steady(&mut tracker, 500.0, HISTORY * 3);
766 let estimate = tracker.estimate(now).expect("samples");
767 assert!(
768 (estimate.velocity.x - 500.0).abs() < 2.0,
769 "got {}",
770 estimate.velocity.x
771 );
772 }
773
774 #[test]
775 fn a_quadratic_is_recovered_exactly() {
776 let x: Vec<f64> = (0..8).map(|i| i as f64 * 0.5).collect();
778 let y: Vec<f64> = x.iter().map(|x| 3.0 + 2.0 * x + 4.0 * x * x).collect();
779 let fit = PolynomialFit::solve(&x, &y, 2).expect("a well-posed system");
780 assert!((fit.coefficients[0] - 3.0).abs() < 1e-6, "{:?}", fit);
781 assert!((fit.coefficients[1] - 2.0).abs() < 1e-6, "{:?}", fit);
782 assert!((fit.coefficients[2] - 4.0).abs() < 1e-6, "{:?}", fit);
783 assert!((fit.confidence - 1.0).abs() < 1e-9);
784 }
785
786 #[test]
787 fn a_degenerate_system_has_no_fit() {
788 let x = [1.0, 1.0, 1.0, 1.0];
790 let y = [0.0, 1.0, 2.0, 3.0];
791 assert!(PolynomialFit::solve(&x, &y, 2).is_none());
792 }
793
794 #[test]
795 fn confidence_falls_when_the_points_do_not_follow_the_curve() {
796 let x: Vec<f64> = (0..10).map(|i| i as f64).collect();
797 let clean: Vec<f64> = x.iter().map(|x| 2.0 * x).collect();
798 let noisy: Vec<f64> = x
799 .iter()
800 .enumerate()
801 .map(|(i, x)| 2.0 * x + if i % 2 == 0 { 40.0 } else { -40.0 })
802 .collect();
803 let a = PolynomialFit::solve(&x, &clean, 2).unwrap();
804 let b = PolynomialFit::solve(&x, &noisy, 2).unwrap();
805 assert!(
806 a.confidence > b.confidence,
807 "{} vs {}",
808 a.confidence,
809 b.confidence
810 );
811 assert!(b.confidence < 0.9);
812 }
813
814 #[test]
815 fn a_velocity_can_be_brought_into_range() {
816 let v = Velocity::new(3000.0, 4000.0); assert!((v.magnitude() - 5000.0).abs() < 1e-3);
818 let capped = v.clamp_magnitude(0.0, 1000.0);
819 assert!((capped.magnitude() - 1000.0).abs() < 1e-2);
820 assert!((capped.x / capped.y - v.x / v.y).abs() < 1e-4);
822 assert_eq!(v.clamp_magnitude(0.0, 8000.0), v);
824 assert_eq!(Velocity::ZERO.clamp_magnitude(50.0, 100.0), Velocity::ZERO);
826 }
827}