tawasuyu/brahman

1//! Resorte-amortiguador genérico de N dimensiones.
2//!
3//! Sirve para animaciones orgánicas: el "tilt" de la chacana hacia el mouse,
4//! el pulso del sol, las transiciones de hover. La integración es semi-implícita
5//! Euler — estable para `dt < 1/freq_hz`. Si `dt` real puede exceder ese límite,
6//! el caller subdivide.
7//!
8//! `damping_ratio`:
9//! - `1.0` crítico: settle en tiempo mínimo, sin overshoot.
10//! - `0.7` sub-crítico: overshoot suave (≈4.6 %), se siente vivo.
11//! - `< 0.5` muy oscilante (no recomendado fuera de FX).
12
13#![no_std]
14
15#[derive(Clone, Copy, Debug)]
16pub struct SpringDamper<const N: usize> {
17    pub position: [f32; N],
18    pub velocity: [f32; N],
19    pub target: [f32; N],
20    /// Frecuencia natural en Hz.
21    pub freq_hz: f32,
22    /// 1.0 = crítico, < 1.0 = oscila.
23    pub damping_ratio: f32,
24}
25
26impl<const N: usize> SpringDamper<N> {
27    pub const fn new(freq_hz: f32, damping_ratio: f32) -> Self {
28        Self {
29            position: [0.0; N],
30            velocity: [0.0; N],
31            target: [0.0; N],
32            freq_hz,
33            damping_ratio,
34        }
35    }
36
37    pub fn with_position(mut self, pos: [f32; N]) -> Self {
38        self.position = pos;
39        self.target = pos;
40        self
41    }
42
43    pub fn set_target(&mut self, t: [f32; N]) {
44        self.target = t;
45    }
46
47    /// Avanza la simulación. Caller suele pasarlo desde un `requestAnimationFrame`.
48    pub fn step(&mut self, dt: f32) {
49        // Tau = 2π. core::f32::consts::TAU está estable desde 1.47.
50        let omega = core::f32::consts::TAU * self.freq_hz;
51        let zeta = self.damping_ratio;
52        let k = omega * omega;
53        let c = 2.0 * zeta * omega;
54        let mut i = 0;
55        while i < N {
56            let dx = self.position[i] - self.target[i];
57            let a = -k * dx - c * self.velocity[i];
58            self.velocity[i] += a * dt;
59            self.position[i] += self.velocity[i] * dt;
60            i += 1;
61        }
62    }
63
64    /// `true` cuando el sistema está esencialmente parado en el target.
65    pub fn at_rest(&self, eps_pos: f32, eps_vel: f32) -> bool {
66        let mut i = 0;
67        while i < N {
68            if (self.position[i] - self.target[i]).abs() > eps_pos
69                || self.velocity[i].abs() > eps_vel
70            {
71                return false;
72            }
73            i += 1;
74        }
75        true
76    }
77}
78
79pub type SpringDamper1 = SpringDamper<1>;
80pub type SpringDamper2 = SpringDamper<2>;
81pub type SpringDamper3 = SpringDamper<3>;
82
83#[cfg(test)]
84mod tests {
85    use super::*;
86
87    #[test]
88    fn settles_at_target_when_critically_damped() {
89        let mut s = SpringDamper2::new(3.0, 1.0);
90        s.set_target([1.0, -0.5]);
91        for _ in 0..300 {
92            s.step(1.0 / 120.0);
93        }
94        assert!((s.position[0] - 1.0).abs() < 1e-3);
95        assert!((s.position[1] + 0.5).abs() < 1e-3);
96        assert!(s.at_rest(1e-3, 1e-3));
97    }
98
99    #[test]
100    fn underdamped_overshoots() {
101        let mut s = SpringDamper1::new(3.0, 0.3);
102        s.set_target([1.0]);
103        let mut peak = 0.0f32;
104        for _ in 0..240 {
105            s.step(1.0 / 240.0);
106            if s.position[0] > peak {
107                peak = s.position[0];
108            }
109        }
110        assert!(peak > 1.0, "underdamped should overshoot, peak={}", peak);
111    }
112
113    #[test]
114    fn at_rest_initially() {
115        let s: SpringDamper2 = SpringDamper::new(2.0, 1.0);
116        assert!(s.at_rest(1e-6, 1e-6));
117    }
118}