1const EPSILON: f64 = 1e-6;
46
47#[inline]
63pub fn fps(n: u32) -> f64 {
64 if n == 0 {
65 return 0.0;
66 }
67 1.0 / f64::from(n)
68}
69
70#[derive(Debug, Clone, Copy, Default, PartialEq)]
114pub struct Spring {
115 pos_pos_coef: f64,
116 pos_vel_coef: f64,
117 vel_pos_coef: f64,
118 vel_vel_coef: f64,
119}
120
121impl Spring {
122 pub fn new(delta_time: f64, angular_frequency: f64, damping_ratio: f64) -> Self {
151 let angular_frequency = angular_frequency.max(0.0);
153 let damping_ratio = damping_ratio.max(0.0);
154
155 if angular_frequency < EPSILON {
158 return Self {
159 pos_pos_coef: 1.0,
160 pos_vel_coef: 0.0,
161 vel_pos_coef: 0.0,
162 vel_vel_coef: 1.0,
163 };
164 }
165
166 if damping_ratio > 1.0 + EPSILON {
167 Self::over_damped(delta_time, angular_frequency, damping_ratio)
169 } else if damping_ratio < 1.0 - EPSILON {
170 Self::under_damped(delta_time, angular_frequency, damping_ratio)
172 } else {
173 Self::critically_damped(delta_time, angular_frequency)
175 }
176 }
177
178 fn over_damped(delta_time: f64, angular_frequency: f64, damping_ratio: f64) -> Self {
180 let za = -angular_frequency * damping_ratio;
181 let zb = angular_frequency * (damping_ratio * damping_ratio - 1.0).sqrt();
182 let z1 = za - zb;
183 let z2 = za + zb;
184
185 let e1 = exp(z1 * delta_time);
186 let e2 = exp(z2 * delta_time);
187
188 let inv_two_zb = 1.0 / (2.0 * zb); let e1_over_two_zb = e1 * inv_two_zb;
191 let e2_over_two_zb = e2 * inv_two_zb;
192
193 let z1e1_over_two_zb = z1 * e1_over_two_zb;
194 let z2e2_over_two_zb = z2 * e2_over_two_zb;
195
196 Self {
197 pos_pos_coef: e1_over_two_zb * z2 - z2e2_over_two_zb + e2,
198 pos_vel_coef: -e1_over_two_zb + e2_over_two_zb,
199 vel_pos_coef: (z1e1_over_two_zb - z2e2_over_two_zb + e2) * z2,
200 vel_vel_coef: -z1e1_over_two_zb + z2e2_over_two_zb,
201 }
202 }
203
204 fn under_damped(delta_time: f64, angular_frequency: f64, damping_ratio: f64) -> Self {
206 let omega_zeta = angular_frequency * damping_ratio;
207 let alpha = angular_frequency * (1.0 - damping_ratio * damping_ratio).sqrt();
208
209 let exp_term = exp(-omega_zeta * delta_time);
210 let cos_term = cos(alpha * delta_time);
211 let sin_term = sin(alpha * delta_time);
212
213 let inv_alpha = 1.0 / alpha;
214
215 let exp_sin = exp_term * sin_term;
216 let exp_cos = exp_term * cos_term;
217 let exp_omega_zeta_sin_over_alpha = exp_term * omega_zeta * sin_term * inv_alpha;
218
219 Self {
220 pos_pos_coef: exp_cos + exp_omega_zeta_sin_over_alpha,
221 pos_vel_coef: exp_sin * inv_alpha,
222 vel_pos_coef: -exp_sin * alpha - omega_zeta * exp_omega_zeta_sin_over_alpha,
223 vel_vel_coef: exp_cos - exp_omega_zeta_sin_over_alpha,
224 }
225 }
226
227 fn critically_damped(delta_time: f64, angular_frequency: f64) -> Self {
229 let exp_term = exp(-angular_frequency * delta_time);
230 let time_exp = delta_time * exp_term;
231 let time_exp_freq = time_exp * angular_frequency;
232
233 Self {
234 pos_pos_coef: time_exp_freq + exp_term,
235 pos_vel_coef: time_exp,
236 vel_pos_coef: -angular_frequency * time_exp_freq,
237 vel_vel_coef: -time_exp_freq + exp_term,
238 }
239 }
240
241 #[inline]
273 pub fn update(&self, pos: f64, vel: f64, equilibrium_pos: f64) -> (f64, f64) {
274 let old_pos = pos - equilibrium_pos;
276 let old_vel = vel;
277
278 let new_pos = old_pos * self.pos_pos_coef + old_vel * self.pos_vel_coef + equilibrium_pos;
279 let new_vel = old_pos * self.vel_pos_coef + old_vel * self.vel_vel_coef;
280
281 (new_pos, new_vel)
282 }
283}
284
285#[cfg(feature = "std")]
288#[inline]
289fn exp(x: f64) -> f64 {
290 x.exp()
291}
292
293#[cfg(not(feature = "std"))]
294#[inline]
295fn exp(x: f64) -> f64 {
296 libm::exp(x)
298}
299
300#[cfg(feature = "std")]
301#[inline]
302fn sin(x: f64) -> f64 {
303 x.sin()
304}
305
306#[cfg(not(feature = "std"))]
307#[inline]
308fn sin(x: f64) -> f64 {
309 libm::sin(x)
310}
311
312#[cfg(feature = "std")]
313#[inline]
314fn cos(x: f64) -> f64 {
315 x.cos()
316}
317
318#[cfg(not(feature = "std"))]
319#[inline]
320fn cos(x: f64) -> f64 {
321 libm::cos(x)
322}
323
324#[cfg(test)]
325mod tests {
326 use super::*;
327
328 const TOLERANCE: f64 = 1e-10;
329
330 fn approx_eq(a: f64, b: f64) -> bool {
331 (a - b).abs() < TOLERANCE
332 }
333
334 #[test]
335 fn test_fps() {
336 assert!(approx_eq(fps(60), 1.0 / 60.0));
337 assert!(approx_eq(fps(30), 1.0 / 30.0));
338 assert!(approx_eq(fps(120), 1.0 / 120.0));
339 assert!(approx_eq(fps(0), 0.0));
340 }
341
342 #[test]
343 fn test_identity_spring() {
344 let spring = Spring::new(fps(60), 0.0, 0.5);
346
347 let (new_pos, new_vel) = spring.update(10.0, 5.0, 100.0);
348
349 assert!(approx_eq(new_pos, 10.0));
350 assert!(approx_eq(new_vel, 5.0));
351 }
352
353 #[test]
354 fn test_critically_damped_approaches_target() {
355 let spring = Spring::new(fps(60), 5.0, 1.0);
356 let mut pos = 0.0;
357 let mut vel = 0.0;
358 let target = 100.0;
359
360 for _ in 0..300 {
362 (pos, vel) = spring.update(pos, vel, target);
363 }
364
365 assert!(
367 (pos - target).abs() < 0.01,
368 "Expected pos ≈ {target}, got {pos}"
369 );
370 assert!(vel.abs() < 0.01, "Expected vel ≈ 0, got {vel}");
371 }
372
373 #[test]
374 fn test_under_damped_oscillates() {
375 let spring = Spring::new(fps(60), 10.0, 0.1);
376 let mut pos = 0.0;
377 let mut vel = 0.0;
378 let target = 100.0;
379
380 let mut crossed_target = false;
381 let mut overshot = false;
382
383 for _ in 0..120 {
385 let old_pos = pos;
386 (pos, vel) = spring.update(pos, vel, target);
387
388 if old_pos < target && pos >= target {
390 crossed_target = true;
391 }
392
393 if pos > target {
395 overshot = true;
396 }
397 }
398
399 assert!(crossed_target, "Under-damped spring should cross target");
400 assert!(overshot, "Under-damped spring should overshoot target");
401 }
402
403 #[test]
404 fn test_over_damped_no_oscillation() {
405 let spring = Spring::new(fps(60), 5.0, 2.0);
406 let mut pos = 0.0;
407 let mut vel = 0.0;
408 let target = 100.0;
409
410 let mut max_pos: f64 = 0.0;
411
412 for _ in 0..600 {
414 (pos, vel) = spring.update(pos, vel, target);
415 max_pos = max_pos.max(pos);
416 }
417
418 assert!(
420 max_pos <= target + TOLERANCE,
421 "Over-damped spring should not overshoot: max_pos={max_pos}, target={target}"
422 );
423
424 assert!(
426 (pos - target).abs() < 1.0,
427 "Over-damped spring should approach target"
428 );
429 }
430
431 #[test]
432 fn test_spring_is_copy() {
433 let spring = Spring::new(fps(60), 5.0, 0.5);
434 let spring2 = spring; let _ = spring.update(0.0, 0.0, 100.0);
436 let _ = spring2.update(0.0, 0.0, 100.0);
437 }
438
439 #[test]
440 fn test_negative_values_clamped() {
441 let spring = Spring::new(fps(60), -5.0, 0.5);
443 let (new_pos, new_vel) = spring.update(10.0, 5.0, 100.0);
444
445 assert!(approx_eq(new_pos, 10.0));
447 assert!(approx_eq(new_vel, 5.0));
448 }
449
450 #[test]
455 fn test_zero_damping_oscillates_indefinitely() {
456 let spring = Spring::new(fps(60), 5.0, 0.0);
458 let mut pos = 0.0;
459 let mut vel = 0.0;
460 let target = 100.0;
461
462 let mut oscillations = 0;
464 let mut last_sign = f64::signum(pos - target);
465
466 for _ in 0..600 {
467 (pos, vel) = spring.update(pos, vel, target);
468 let current_sign = f64::signum(pos - target);
469 #[allow(clippy::float_cmp)] if current_sign != last_sign && current_sign != 0.0 {
471 oscillations += 1;
472 last_sign = current_sign;
473 }
474 }
475
476 assert!(
478 oscillations >= 5,
479 "Zero damping should oscillate indefinitely, got {oscillations} oscillations"
480 );
481 }
482
483 #[test]
484 fn test_very_high_stiffness_snaps() {
485 let spring = Spring::new(fps(60), 100.0, 1.0);
487 let mut pos = 0.0;
488 let mut vel = 0.0;
489 let target = 100.0;
490
491 for _ in 0..30 {
493 (pos, vel) = spring.update(pos, vel, target);
494 }
495
496 assert!(
498 (pos - target).abs() < 1.0,
499 "High stiffness should snap quickly, got pos={pos}"
500 );
501 }
502
503 #[test]
504 fn test_negative_target() {
505 let spring = Spring::new(fps(60), 5.0, 1.0);
506 let mut pos = 100.0;
507 let mut vel = 0.0;
508 let target = -50.0;
509
510 for _ in 0..300 {
512 (pos, vel) = spring.update(pos, vel, target);
513 }
514
515 assert!(
517 (pos - target).abs() < 0.1,
518 "Should approach negative target, got pos={pos}"
519 );
520 }
521
522 #[test]
523 fn test_very_small_movements() {
524 let spring = Spring::new(fps(60), 5.0, 1.0);
525 let mut pos = 0.0;
526 let mut vel = 0.0;
527 let target = 0.001; for _ in 0..300 {
530 (pos, vel) = spring.update(pos, vel, target);
531 }
532
533 assert!(
535 (pos - target).abs() < 0.0001,
536 "Should handle small movements, got pos={pos}, target={target}"
537 );
538 }
539
540 #[test]
541 fn test_large_time_delta() {
542 let spring = Spring::new(1.0, 5.0, 1.0); let mut pos = 0.0;
545 let mut vel = 0.0;
546 let target = 100.0;
547
548 for _ in 0..10 {
550 (pos, vel) = spring.update(pos, vel, target);
551 }
552
553 assert!(
555 (pos - target).abs() < 5.0,
556 "Large delta should still converge, got pos={pos}"
557 );
558 }
559
560 #[test]
561 fn test_accumulated_error_bounded() {
562 let spring = Spring::new(fps(60), 5.0, 0.5);
564 let mut pos = 0.0;
565 let mut vel = 0.0;
566 let target = 100.0;
567
568 for _ in 0..3600 {
570 (pos, vel) = spring.update(pos, vel, target);
571 }
572
573 assert!(
575 (pos - target).abs() < 0.001,
576 "Accumulated error should be bounded, got pos={pos}"
577 );
578 assert!(
579 vel.abs() < 0.001,
580 "Velocity should decay completely, got vel={vel}"
581 );
582 }
583
584 #[test]
585 fn test_spring_default() {
586 let spring = Spring::default();
587 let (new_pos, new_vel) = spring.update(10.0, 5.0, 100.0);
591 assert!(approx_eq(new_pos, 100.0));
593 assert!(approx_eq(new_vel, 0.0));
594 }
595
596 #[test]
597 fn test_spring_clone() {
598 let spring1 = Spring::new(fps(60), 5.0, 0.5);
599 let spring2 = spring1;
600
601 let result1 = spring1.update(0.0, 0.0, 100.0);
603 let result2 = spring2.update(0.0, 0.0, 100.0);
604
605 assert!(approx_eq(result1.0, result2.0));
606 assert!(approx_eq(result1.1, result2.1));
607 }
608
609 #[test]
610 fn test_spring_equilibrium_at_target() {
611 let spring = Spring::new(fps(60), 5.0, 0.5);
613 let target = 50.0;
614 let (new_pos, new_vel) = spring.update(target, 0.0, target);
615
616 assert!(approx_eq(new_pos, target));
617 assert!(approx_eq(new_vel, 0.0));
618 }
619
620 #[test]
621 fn test_fps_various_rates() {
622 assert!(approx_eq(fps(30), 1.0 / 30.0));
624 assert!(approx_eq(fps(60), 1.0 / 60.0));
625 assert!(approx_eq(fps(120), 1.0 / 120.0));
626 assert!(approx_eq(fps(144), 1.0 / 144.0));
627 assert!(approx_eq(fps(240), 1.0 / 240.0));
628 assert!(approx_eq(fps(1), 1.0));
629 }
630
631 #[test]
632 fn test_damping_ratio_boundary() {
633 let under = Spring::new(fps(60), 5.0, 0.999);
635 let critical = Spring::new(fps(60), 5.0, 1.0);
636 let over = Spring::new(fps(60), 5.0, 1.001);
637
638 let _ = under.update(0.0, 0.0, 100.0);
640 let _ = critical.update(0.0, 0.0, 100.0);
641 let _ = over.update(0.0, 0.0, 100.0);
642 }
643
644 #[test]
645 fn test_initial_velocity() {
646 let spring = Spring::new(fps(60), 5.0, 1.0);
648 let mut pos = 0.0;
649 let mut vel = 1000.0; let target = 50.0;
651
652 for _ in 0..600 {
653 (pos, vel) = spring.update(pos, vel, target);
654 }
655
656 assert!(
657 (pos - target).abs() < 0.1,
658 "Should converge despite initial velocity"
659 );
660 }
661}
662
663#[cfg(test)]
668mod property_tests {
669 use super::*;
670 use proptest::prelude::*;
671
672 fn convergence_frames(angular_freq: f64, damping_ratio: f64) -> usize {
678 let (tau, multiplier) = if damping_ratio > 1.0 {
679 let discriminant = (damping_ratio * damping_ratio - 1.0).sqrt();
681 (1.0 / (angular_freq * (damping_ratio - discriminant)), 8.0)
682 } else {
683 (1.0 / (angular_freq * damping_ratio.max(0.01)), 10.0)
686 };
687 #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
688 { (tau * multiplier * 60.0) as usize }.clamp(600, 18000)
689 }
690
691 proptest! {
696 #[test]
697 fn spring_update_never_produces_nan_or_inf(
698 delta_time in 0.0001f64..1.0,
699 angular_freq in 0.0f64..500.0,
700 damping_ratio in 0.0f64..50.0,
701 pos in -1e6f64..1e6,
702 vel in -1e6f64..1e6,
703 target in -1e6f64..1e6,
704 ) {
705 let spring = Spring::new(delta_time, angular_freq, damping_ratio);
706 let (new_pos, new_vel) = spring.update(pos, vel, target);
707
708 prop_assert!(!new_pos.is_nan(), "position was NaN for dt={delta_time}, af={angular_freq}, dr={damping_ratio}, pos={pos}, vel={vel}, target={target}");
709 prop_assert!(!new_pos.is_infinite(), "position was Inf for dt={delta_time}, af={angular_freq}, dr={damping_ratio}, pos={pos}, vel={vel}, target={target}");
710 prop_assert!(!new_vel.is_nan(), "velocity was NaN for dt={delta_time}, af={angular_freq}, dr={damping_ratio}, pos={pos}, vel={vel}, target={target}");
711 prop_assert!(!new_vel.is_infinite(), "velocity was Inf for dt={delta_time}, af={angular_freq}, dr={damping_ratio}, pos={pos}, vel={vel}, target={target}");
712 }
713
714 #[test]
715 fn spring_multi_frame_never_produces_nan_or_inf(
716 angular_freq in 0.1f64..100.0,
717 damping_ratio in 0.01f64..10.0,
718 target in -1000.0f64..1000.0,
719 ) {
720 let spring = Spring::new(fps(60), angular_freq, damping_ratio);
721 let mut pos = 0.0;
722 let mut vel = 0.0;
723
724 for _ in 0..600 {
725 (pos, vel) = spring.update(pos, vel, target);
726 prop_assert!(!pos.is_nan(), "position became NaN during simulation");
727 prop_assert!(!pos.is_infinite(), "position became Inf during simulation");
728 prop_assert!(!vel.is_nan(), "velocity became NaN during simulation");
729 prop_assert!(!vel.is_infinite(), "velocity became Inf during simulation");
730 }
731 }
732 }
733
734 proptest! {
739 #[test]
740 fn damped_spring_converges_to_target(
741 angular_freq in 1.0f64..50.0,
742 damping_ratio in 0.2f64..10.0,
743 target in -500.0f64..500.0,
744 ) {
745 let spring = Spring::new(fps(60), angular_freq, damping_ratio);
746 let mut pos = 0.0;
747 let mut vel = 0.0;
748
749 let frames = convergence_frames(angular_freq, damping_ratio);
750
751 for _ in 0..frames {
752 (pos, vel) = spring.update(pos, vel, target);
753 }
754
755 let error = (pos - target).abs();
756 let tolerance = 1.0f64.max(target.abs() * 0.005);
758 prop_assert!(
759 error < tolerance,
760 "Spring did not converge: pos={pos}, target={target}, error={error}, tol={tolerance}, af={angular_freq}, dr={damping_ratio}, frames={frames}"
761 );
762 }
763
764 #[test]
765 fn spring_final_velocity_near_zero(
766 angular_freq in 1.0f64..50.0,
767 damping_ratio in 0.2f64..10.0,
768 target in -500.0f64..500.0,
769 ) {
770 let spring = Spring::new(fps(60), angular_freq, damping_ratio);
771 let mut pos = 0.0;
772 let mut vel = 0.0;
773
774 let frames = convergence_frames(angular_freq, damping_ratio);
775
776 for _ in 0..frames {
777 (pos, vel) = spring.update(pos, vel, target);
778 }
779
780 let tolerance = 1.0f64.max(target.abs() * 0.005);
782 prop_assert!(
783 vel.abs() < tolerance,
784 "Velocity did not decay: vel={vel}, tol={tolerance}, af={angular_freq}, dr={damping_ratio}, frames={frames}"
785 );
786 }
787 }
788
789 proptest! {
794 #[test]
795 fn higher_stiffness_means_faster_initial_response(
796 low_freq in 1.0f64..10.0,
797 high_freq_add in 10.0f64..90.0,
798 ) {
799 let high_freq = low_freq + high_freq_add;
800 let damping = 1.0; let target = 100.0;
802
803 let spring_low = Spring::new(fps(60), low_freq, damping);
804 let spring_high = Spring::new(fps(60), high_freq, damping);
805
806 let (pos_low, _) = spring_low.update(0.0, 0.0, target);
807 let (pos_high, _) = spring_high.update(0.0, 0.0, target);
808
809 prop_assert!(
811 pos_high >= pos_low,
812 "Higher stiffness should respond faster: pos_high={pos_high}, pos_low={pos_low}"
813 );
814 }
815
816 #[test]
817 fn over_damped_does_not_overshoot(
818 angular_freq in 1.0f64..50.0,
819 damping_excess in 0.5f64..10.0,
820 target in 1.0f64..1000.0,
821 ) {
822 let damping_ratio = 1.0 + damping_excess; let spring = Spring::new(fps(60), angular_freq, damping_ratio);
824 let mut pos = 0.0;
825 let mut vel = 0.0;
826
827 for _ in 0..600 {
828 (pos, vel) = spring.update(pos, vel, target);
829 prop_assert!(
831 pos <= target + 0.01,
832 "Over-damped spring overshot: pos={pos}, target={target}, af={angular_freq}, dr={damping_ratio}"
833 );
834 }
835 }
836
837 #[test]
838 fn under_damped_oscillates(
839 angular_freq in 5.0f64..50.0,
840 damping_ratio in 0.01f64..0.3,
841 ) {
842 let spring = Spring::new(fps(60), angular_freq, damping_ratio);
843 let target = 100.0;
844 let mut pos = 0.0;
845 let mut vel = 0.0;
846 let mut overshot = false;
847
848 for _ in 0..300 {
849 (pos, vel) = spring.update(pos, vel, target);
850 if pos > target {
851 overshot = true;
852 break;
853 }
854 }
855
856 prop_assert!(
857 overshot,
858 "Under-damped spring should overshoot: af={angular_freq}, dr={damping_ratio}"
859 );
860 }
861 }
862
863 proptest! {
868 #[test]
869 fn at_equilibrium_stays_at_equilibrium(
870 angular_freq in 0.1f64..100.0,
871 damping_ratio in 0.0f64..10.0,
872 target in -1000.0f64..1000.0,
873 ) {
874 let spring = Spring::new(fps(60), angular_freq, damping_ratio);
875 let (new_pos, new_vel) = spring.update(target, 0.0, target);
876
877 let pos_error = (new_pos - target).abs();
878 prop_assert!(
879 pos_error < 1e-10,
880 "Position drifted from equilibrium: error={pos_error}"
881 );
882 prop_assert!(
883 new_vel.abs() < 1e-10,
884 "Velocity non-zero at equilibrium: vel={new_vel}"
885 );
886 }
887 }
888
889 proptest! {
894 #[test]
895 fn frame_independence_approximate(
896 angular_freq in 1.0f64..20.0,
897 damping_ratio in 0.1f64..5.0,
898 target in 10.0f64..500.0,
899 ) {
900 let spring_60 = Spring::new(fps(60), angular_freq, damping_ratio);
903 let spring_120 = Spring::new(fps(120), angular_freq, damping_ratio);
904
905 let mut pos_60 = 0.0;
906 let mut vel_60 = 0.0;
907 for _ in 0..60 {
908 (pos_60, vel_60) = spring_60.update(pos_60, vel_60, target);
909 }
910
911 let mut pos_120 = 0.0;
912 let mut vel_120 = 0.0;
913 for _ in 0..120 {
914 (pos_120, vel_120) = spring_120.update(pos_120, vel_120, target);
915 }
916
917 let pos_diff = (pos_60 - pos_120).abs();
919 let tolerance = target.abs() * 0.05; prop_assert!(
921 pos_diff < tolerance,
922 "Frame rate independence violated: pos@60fps={pos_60}, pos@120fps={pos_120}, diff={pos_diff}, tol={tolerance}"
923 );
924 }
925 }
926
927 proptest! {
932 #[test]
933 fn negative_angular_freq_acts_as_identity(
934 neg_freq in -100.0f64..0.0,
935 damping in 0.0f64..10.0,
936 pos in -1000.0f64..1000.0,
937 vel in -1000.0f64..1000.0,
938 target in -1000.0f64..1000.0,
939 ) {
940 let spring = Spring::new(fps(60), neg_freq, damping);
941 let (new_pos, new_vel) = spring.update(pos, vel, target);
942
943 let pos_error = (new_pos - pos).abs();
945 let vel_error = (new_vel - vel).abs();
946 prop_assert!(pos_error < 1e-10, "Identity spring changed position: {new_pos} != {pos}");
947 prop_assert!(vel_error < 1e-10, "Identity spring changed velocity: {new_vel} != {vel}");
948 }
949
950 #[test]
951 fn zero_delta_time_identity(
952 angular_freq in 0.1f64..100.0,
953 damping_ratio in 0.0f64..10.0,
954 pos in -1000.0f64..1000.0,
955 vel in -1000.0f64..1000.0,
956 target in -1000.0f64..1000.0,
957 ) {
958 let spring = Spring::new(0.0, angular_freq, damping_ratio);
959 let (new_pos, new_vel) = spring.update(pos, vel, target);
960
961 prop_assert!(!new_pos.is_nan(), "NaN with zero delta time");
964 prop_assert!(!new_vel.is_nan(), "NaN velocity with zero delta time");
965 }
966 }
967}