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 {
163 Self::platform_default()
164 }
165}
166
167impl VelocityTracker {
168 pub fn new(strategy: VelocityStrategy) -> Self {
170 Self {
171 strategy,
172 samples: [None; HISTORY],
173 index: 0,
174 }
175 }
176
177 pub fn platform_default() -> Self {
182 #[cfg(target_os = "ios")]
183 let strategy = VelocityStrategy::RecentAverage(BOUNCING_FLING_WEIGHTS);
184 #[cfg(target_os = "macos")]
185 let strategy = VelocityStrategy::RecentAverage(DESKTOP_FLING_WEIGHTS);
186 #[cfg(not(any(target_os = "ios", target_os = "macos")))]
187 let strategy = VelocityStrategy::Regression;
188 Self::new(strategy)
189 }
190
191 pub fn strategy(&self) -> VelocityStrategy {
193 self.strategy
194 }
195
196 pub fn add_position(&mut self, time: f32, point: Point) {
198 self.index = (self.index + 1) % HISTORY;
199 self.samples[self.index] = Some(Sample { point, time });
200 }
201
202 pub fn clear(&mut self) {
204 self.samples = [None; HISTORY];
205 self.index = 0;
206 }
207
208 pub fn estimate(&self, now: f32) -> Option<VelocityEstimate> {
211 let newest = self.samples[self.index]?;
212 if (now - newest.time) * 1000.0 - self.delivery_ms() > STOPPED_MS {
216 return Some(VelocityEstimate::STILL);
217 }
218 match self.strategy {
219 VelocityStrategy::Regression => Some(self.regress(newest)),
220 VelocityStrategy::RecentAverage(weights) => Some(self.recent_average(newest, weights)),
221 }
222 }
223
224 pub fn velocity(&self, now: f32) -> Velocity {
227 self.estimate(now)
228 .map(|e| e.velocity)
229 .unwrap_or(Velocity::ZERO)
230 }
231
232 fn regress(&self, newest: Sample) -> VelocityEstimate {
235 let mut ages = [0.0f64; HISTORY];
238 let mut xs = [0.0f64; HISTORY];
239 let mut ys = [0.0f64; HISTORY];
240 let mut count = 0;
241 let mut oldest = newest;
242 let mut previous = newest;
243 let mut index = self.index;
244 let delivery = self.delivery_ms();
247
248 while count < HISTORY {
249 let Some(sample) = self.samples[index] else {
250 break;
251 };
252 let age = (newest.time - sample.time) * 1000.0;
253 let gap = ((sample.time - previous.time) * 1000.0).abs();
254 previous = sample;
255 if age > HORIZON_MS + delivery || gap - delivery > STOPPED_MS {
257 break;
258 }
259 oldest = sample;
260 ages[count] = -age as f64;
261 xs[count] = sample.point.x as f64;
262 ys[count] = sample.point.y as f64;
263 count += 1;
264 index = if index == 0 { HISTORY } else { index } - 1;
265 }
266
267 let duration = newest.time - oldest.time;
268 let offset = (
269 newest.point.x - oldest.point.x,
270 newest.point.y - oldest.point.y,
271 );
272 if count >= MIN_SAMPLES {
273 let x_fit = PolynomialFit::solve(&ages[..count], &xs[..count], 2);
274 let y_fit = PolynomialFit::solve(&ages[..count], &ys[..count], 2);
275 if let (Some(x_fit), Some(y_fit)) = (x_fit, y_fit) {
276 return VelocityEstimate {
277 velocity: Velocity::new(
279 (x_fit.coefficients[1] * 1000.0) as f32,
280 (y_fit.coefficients[1] * 1000.0) as f32,
281 ),
282 confidence: (x_fit.confidence * y_fit.confidence) as f32,
283 duration,
284 offset,
285 };
286 }
287 }
288 VelocityEstimate {
291 velocity: Velocity::ZERO,
292 confidence: 1.0,
293 duration,
294 offset,
295 }
296 }
297
298 fn delivery_ms(&self) -> f32 {
304 let mut gaps = [0.0f32; DELIVERY_GAPS];
305 let mut count = 0;
306 let mut index = self.index;
307 while count < DELIVERY_GAPS {
308 let before = if index == 0 { HISTORY } else { index } - 1;
309 let (Some(newer), Some(older)) = (self.samples[index], self.samples[before]) else {
310 break;
311 };
312 gaps[count] = ((newer.time - older.time) * 1000.0).max(0.0);
313 count += 1;
314 index = before;
315 }
316 let gaps = &mut gaps[..count];
317 gaps.sort_by(f32::total_cmp);
318 let pace = match gaps.len() {
319 0 => 0.0,
320 DELIVERY_GAPS => gaps[DELIVERY_GAPS / 2],
321 _ => gaps[0],
322 };
323 pace.min(MAX_DELIVERY_MS)
324 }
325
326 fn pair_velocity(&self, offset: isize) -> Velocity {
328 let size = HISTORY as isize;
329 let end = self.samples[((self.index as isize + offset).rem_euclid(size)) as usize];
330 let start = self.samples[((self.index as isize + offset - 1).rem_euclid(size)) as usize];
331 let (Some(end), Some(start)) = (end, start) else {
332 return Velocity::ZERO;
333 };
334 let dt = end.time - start.time;
335 if dt <= 0.0 {
336 return Velocity::ZERO;
337 }
338 Velocity::new(
339 (end.point.x - start.point.x) / dt,
340 (end.point.y - start.point.y) / dt,
341 )
342 }
343
344 fn recent_average(&self, newest: Sample, weights: [f32; 3]) -> VelocityEstimate {
346 let mut velocity = Velocity::ZERO;
347 for (i, weight) in weights.iter().enumerate() {
348 let pair = self.pair_velocity(i as isize - (weights.len() as isize - 1));
349 velocity.x += pair.x * weight;
350 velocity.y += pair.y * weight;
351 }
352 let mut oldest = newest;
355 for i in 1..=HISTORY {
356 if let Some(sample) = self.samples[(self.index + i) % HISTORY] {
357 oldest = sample;
358 break;
359 }
360 }
361 VelocityEstimate {
362 velocity,
363 confidence: 1.0,
364 duration: newest.time - oldest.time,
365 offset: (
366 newest.point.x - oldest.point.x,
367 newest.point.y - oldest.point.y,
368 ),
369 }
370 }
371}
372
373#[derive(Clone, Copy, Debug, PartialEq)]
377pub struct PolynomialFit {
378 pub coefficients: [f64; 3],
380 pub confidence: f64,
383}
384
385const PRECISION_TOLERANCE: f64 = 1e-10;
387
388impl PolynomialFit {
389 pub fn solve(x: &[f64], y: &[f64], degree: usize) -> Option<PolynomialFit> {
399 debug_assert_eq!(x.len(), y.len());
400 debug_assert!(degree <= 2);
401 let m = x.len();
402 let n = degree + 1;
403 if degree > m {
404 return None; }
406
407 let mut a = vec![0.0f64; n * m];
409 for h in 0..m {
410 a[h] = 1.0;
411 for i in 1..n {
412 a[i * m + h] = a[(i - 1) * m + h] * x[h];
413 }
414 }
415
416 let mut q = vec![0.0f64; n * m];
418 let mut r = vec![0.0f64; n * n];
419 for j in 0..n {
420 for h in 0..m {
421 q[j * m + h] = a[j * m + h];
422 }
423 for i in 0..j {
424 let dot: f64 = (0..m).map(|h| q[j * m + h] * q[i * m + h]).sum();
425 for h in 0..m {
426 q[j * m + h] -= dot * q[i * m + h];
427 }
428 }
429 let norm = (0..m)
430 .map(|h| q[j * m + h] * q[j * m + h])
431 .sum::<f64>()
432 .sqrt();
433 if norm < PRECISION_TOLERANCE {
434 return None; }
436 let inverse = 1.0 / norm;
437 for h in 0..m {
438 q[j * m + h] *= inverse;
439 }
440 for i in 0..n {
441 r[j * n + i] = if i < j {
442 0.0
443 } else {
444 (0..m).map(|h| q[j * m + h] * a[i * m + h]).sum()
445 };
446 }
447 }
448
449 let mut coefficients = [0.0f64; 3];
451 for i in (0..n).rev() {
452 let mut c: f64 = (0..m).map(|h| q[i * m + h] * y[h]).sum();
453 for j in (i + 1..n).rev() {
454 c -= r[i * n + j] * coefficients[j];
455 }
456 coefficients[i] = c / r[i * n + i];
457 }
458
459 let mean = y.iter().sum::<f64>() / m as f64;
461 let mut squared_error = 0.0;
462 let mut squared_total = 0.0;
463 for h in 0..m {
464 let mut term = 1.0;
465 let mut error = y[h] - coefficients[0];
466 for c in coefficients.iter().take(n).skip(1) {
467 term *= x[h];
468 error -= term * c;
469 }
470 squared_error += error * error;
471 let deviation = y[h] - mean;
472 squared_total += deviation * deviation;
473 }
474 let confidence = if squared_total <= PRECISION_TOLERANCE {
475 1.0
476 } else {
477 1.0 - (squared_error / squared_total)
478 };
479
480 Some(PolynomialFit {
481 coefficients,
482 confidence,
483 })
484 }
485}
486
487#[cfg(test)]
488mod tests {
489 use super::*;
490
491 fn steady(tracker: &mut VelocityTracker, speed: f32, samples: usize) -> f32 {
493 let step = 0.008;
494 let mut t = 0.0;
495 for i in 0..samples {
496 t = i as f32 * step;
497 tracker.add_position(t, Point::new(speed * t, 2.0 * speed * t));
498 }
499 t
500 }
501
502 fn feed(tracker: &mut VelocityTracker, deltas: &[f32]) -> f32 {
510 let step = 0.008;
511 let mut t = 0.0;
512 let mut x = 0.0;
513 tracker.add_position(t, Point::new(x, 0.0));
514 for delta in deltas {
515 t += step;
516 x += delta;
517 tracker.add_position(t, Point::new(x, 0.0));
518 }
519 t
520 }
521
522 fn throw_then_lift(tracker: &mut VelocityTracker, slow: usize) -> f32 {
525 let mut deltas = vec![8.0f32; 7];
526 deltas.extend(std::iter::repeat_n(1.0f32, slow));
527 feed(tracker, &deltas)
528 }
529
530 #[test]
531 fn a_steady_drag_is_read_exactly() {
532 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
533 let now = steady(&mut tracker, 600.0, 10);
534 let estimate = tracker.estimate(now).expect("samples were added");
535 assert!(
536 (estimate.velocity.x - 600.0).abs() < 1.0,
537 "x = {}",
538 estimate.velocity.x
539 );
540 assert!(
541 (estimate.velocity.y - 1200.0).abs() < 2.0,
542 "y = {}",
543 estimate.velocity.y
544 );
545 assert!(estimate.confidence > 0.99, "a line fits a line");
546 assert!(estimate.offset.0 > 0.0 && estimate.duration > 0.0);
547 }
548
549 #[test]
550 fn the_regression_survives_a_last_instant_slowdown() {
551 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
552 let now = throw_then_lift(&mut tracker, 2);
553 let estimate = tracker.estimate(now).expect("samples");
554 assert!(
557 estimate.velocity.x > 400.0,
558 "the throw should survive the lift-off, got {}",
559 estimate.velocity.x
560 );
561 }
562
563 #[test]
564 fn a_finger_that_stopped_before_lifting_flings_nothing() {
565 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
566 let last = steady(&mut tracker, 900.0, 8);
567 let estimate = tracker.estimate(last + 0.1).expect("samples");
569 assert_eq!(estimate.velocity, Velocity::ZERO);
570 assert_eq!(estimate, VelocityEstimate::STILL);
571 }
572
573 fn paced(tracker: &mut VelocityTracker, speed: f32, step: f32, samples: usize) -> f32 {
576 let mut t = 0.0;
577 for i in 0..samples {
578 t = i as f32 * step;
579 tracker.add_position(t, Point::new(speed * t, 0.0));
580 }
581 t
582 }
583
584 #[test]
591 fn a_throw_delivered_a_frame_at_a_time_is_still_a_throw() {
592 for strategy in [
593 VelocityStrategy::Regression,
594 VelocityStrategy::RecentAverage(BOUNCING_FLING_WEIGHTS),
595 ] {
596 for step in [0.037, 0.045, 0.051] {
597 let mut tracker = VelocityTracker::new(strategy);
598 let last = paced(&mut tracker, 2000.0, step, 6);
599 let estimate = tracker.estimate(last + 0.041).expect("samples");
600 assert!(
601 (estimate.velocity.x - 2000.0).abs() < 40.0,
602 "{strategy:?} every {step} s: got {}",
603 estimate.velocity.x
604 );
605 }
606 }
607 }
608
609 #[test]
612 fn a_short_throw_at_slow_frames_is_still_a_throw() {
613 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
614 let last = paced(&mut tracker, 2000.0, 0.037, 3);
615 let estimate = tracker.estimate(last + 0.041).expect("samples");
616 assert!(
617 (estimate.velocity.x - 2000.0).abs() < 40.0,
618 "got {}",
619 estimate.velocity.x
620 );
621 }
622
623 #[test]
626 fn slow_frames_do_not_hide_a_finger_that_stopped() {
627 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
628 let last = paced(&mut tracker, 2000.0, 0.037, 6);
629 let estimate = tracker.estimate(last + 0.12).expect("samples");
630 assert_eq!(estimate, VelocityEstimate::STILL);
631 let mut crawling = VelocityTracker::new(VelocityStrategy::Regression);
633 let last = paced(&mut crawling, 2000.0, 0.2, 6);
634 assert_eq!(
635 crawling.estimate(last + 0.1).expect("samples"),
636 VelocityEstimate::STILL
637 );
638 }
639
640 #[test]
643 fn one_short_gap_does_not_change_the_pace() {
644 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
645 for (t, x) in [
646 (0.0, 0.0),
647 (0.037, 74.0),
648 (0.074, 148.0),
649 (0.111, 222.0),
650 (0.116, 232.0),
651 (0.153, 306.0),
652 ] {
653 tracker.add_position(t, Point::new(x, 0.0));
654 }
655 let estimate = tracker.estimate(0.153 + 0.05).expect("samples");
658 assert!(estimate.velocity.x > 1500.0, "got {}", estimate.velocity.x);
659 }
660
661 #[test]
665 fn a_pause_just_before_the_release_excuses_nothing() {
666 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
667 let mut t = 0.0;
668 for i in 0..6 {
669 t = i as f32 * 0.008;
670 tracker.add_position(t, Point::new(i as f32 * 16.0, 0.0));
671 }
672 let stopped = t + 0.06;
673 tracker.add_position(stopped, Point::new(80.0, 0.0));
674 let estimate = tracker.estimate(stopped + 0.005).expect("samples");
675 assert!(
676 estimate.velocity.x < 400.0,
677 "the throw before the pause leaked across it: {}",
678 estimate.velocity.x
679 );
680 assert!(
682 estimate.offset.0.abs() < 1.0 && estimate.duration < 0.01,
683 "the history ran back across the pause: {estimate:?}"
684 );
685 }
686
687 #[test]
688 fn samples_older_than_the_horizon_are_not_part_of_the_motion() {
689 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
690 for i in 0..6 {
693 tracker.add_position(i as f32 * 0.008, Point::new(i as f32 * 16.0, 0.0));
694 }
695 let base = 1.0;
696 for i in 0..6 {
697 tracker.add_position(
698 base + i as f32 * 0.008,
699 Point::new(1000.0 + i as f32 * 1.6, 0.0),
700 );
701 }
702 let now = base + 5.0 * 0.008;
703 let estimate = tracker.estimate(now).expect("samples");
704 assert!(
705 estimate.velocity.x < 400.0,
706 "the old fast gesture leaked in: {}",
707 estimate.velocity.x
708 );
709 assert!(estimate.duration < 0.1, "span = {}", estimate.duration);
710 }
711
712 #[test]
713 fn too_few_samples_give_travel_but_no_speed() {
714 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
715 tracker.add_position(0.0, Point::new(0.0, 0.0));
716 tracker.add_position(0.008, Point::new(10.0, 0.0));
717 let estimate = tracker.estimate(0.008).expect("samples");
718 assert_eq!(
719 estimate.velocity,
720 Velocity::ZERO,
721 "2 samples cannot regress"
722 );
723 assert_eq!(estimate.offset.0, 10.0, "but the travel is still known");
724 }
725
726 #[test]
727 fn an_empty_tracker_has_nothing_to_say() {
728 let tracker = VelocityTracker::new(VelocityStrategy::Regression);
729 assert!(tracker.estimate(0.0).is_none());
730 assert_eq!(tracker.velocity(0.0), Velocity::ZERO);
731 }
732
733 #[test]
734 fn clearing_starts_the_gesture_over() {
735 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
736 let now = steady(&mut tracker, 600.0, 10);
737 tracker.clear();
738 assert!(tracker.estimate(now).is_none());
739 }
740
741 #[test]
742 fn the_recent_average_leans_on_the_older_samples() {
743 let mut tracker =
746 VelocityTracker::new(VelocityStrategy::RecentAverage(BOUNCING_FLING_WEIGHTS));
747 let now = throw_then_lift(&mut tracker, 1);
748 let estimate = tracker.estimate(now).expect("samples");
749 assert!(
752 (estimate.velocity.x - 956.0).abs() < 20.0,
753 "got {}",
754 estimate.velocity.x
755 );
756 }
757
758 #[test]
759 fn the_recent_average_needs_no_history_to_answer() {
760 let mut tracker =
761 VelocityTracker::new(VelocityStrategy::RecentAverage(DESKTOP_FLING_WEIGHTS));
762 tracker.add_position(0.0, Point::new(0.0, 0.0));
763 tracker.add_position(0.008, Point::new(8.0, 0.0));
764 let estimate = tracker.estimate(0.008).expect("samples");
765 assert!(
767 (estimate.velocity.x - 200.0).abs() < 1.0,
768 "got {}",
769 estimate.velocity.x
770 );
771 }
772
773 #[test]
774 fn the_ring_buffer_wraps_without_losing_the_motion() {
775 let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
776 let now = steady(&mut tracker, 500.0, HISTORY * 3);
778 let estimate = tracker.estimate(now).expect("samples");
779 assert!(
780 (estimate.velocity.x - 500.0).abs() < 2.0,
781 "got {}",
782 estimate.velocity.x
783 );
784 }
785
786 #[test]
787 fn a_quadratic_is_recovered_exactly() {
788 let x: Vec<f64> = (0..8).map(|i| i as f64 * 0.5).collect();
790 let y: Vec<f64> = x.iter().map(|x| 3.0 + 2.0 * x + 4.0 * x * x).collect();
791 let fit = PolynomialFit::solve(&x, &y, 2).expect("a well-posed system");
792 assert!((fit.coefficients[0] - 3.0).abs() < 1e-6, "{:?}", fit);
793 assert!((fit.coefficients[1] - 2.0).abs() < 1e-6, "{:?}", fit);
794 assert!((fit.coefficients[2] - 4.0).abs() < 1e-6, "{:?}", fit);
795 assert!((fit.confidence - 1.0).abs() < 1e-9);
796 }
797
798 #[test]
799 fn a_degenerate_system_has_no_fit() {
800 let x = [1.0, 1.0, 1.0, 1.0];
802 let y = [0.0, 1.0, 2.0, 3.0];
803 assert!(PolynomialFit::solve(&x, &y, 2).is_none());
804 }
805
806 #[test]
807 fn confidence_falls_when_the_points_do_not_follow_the_curve() {
808 let x: Vec<f64> = (0..10).map(|i| i as f64).collect();
809 let clean: Vec<f64> = x.iter().map(|x| 2.0 * x).collect();
810 let noisy: Vec<f64> = x
811 .iter()
812 .enumerate()
813 .map(|(i, x)| 2.0 * x + if i % 2 == 0 { 40.0 } else { -40.0 })
814 .collect();
815 let a = PolynomialFit::solve(&x, &clean, 2).unwrap();
816 let b = PolynomialFit::solve(&x, &noisy, 2).unwrap();
817 assert!(
818 a.confidence > b.confidence,
819 "{} vs {}",
820 a.confidence,
821 b.confidence
822 );
823 assert!(b.confidence < 0.9);
824 }
825
826 #[test]
827 fn a_velocity_can_be_brought_into_range() {
828 let v = Velocity::new(3000.0, 4000.0); assert!((v.magnitude() - 5000.0).abs() < 1e-3);
830 let capped = v.clamp_magnitude(0.0, 1000.0);
831 assert!((capped.magnitude() - 1000.0).abs() < 1e-2);
832 assert!((capped.x / capped.y - v.x / v.y).abs() < 1e-4);
834 assert_eq!(v.clamp_magnitude(0.0, 8000.0), v);
836 assert_eq!(Velocity::ZERO.clamp_magnitude(50.0, 100.0), Velocity::ZERO);
838 }
839}