1#[derive(Debug, Clone, Copy, PartialEq)]
10pub struct SpringConfig {
11 pub mass: f64,
13 pub stiffness: f64,
15 pub damping: f64,
17 pub rest_velocity_threshold: f64,
19 pub rest_displacement_threshold: f64,
21}
22
23impl SpringConfig {
24 pub const fn new(stiffness: f64, damping: f64) -> Self {
26 Self {
27 mass: 1.0,
28 stiffness,
29 damping,
30 rest_velocity_threshold: 1e-4,
31 rest_displacement_threshold: 1e-4,
32 }
33 }
34
35 pub const fn snappy() -> Self {
37 Self::new(280.0, 30.0)
38 }
39
40 pub const fn gentle() -> Self {
42 Self::new(120.0, 14.0)
43 }
44
45 pub const fn bouncy() -> Self {
47 Self::new(180.0, 12.0)
48 }
49
50 pub const fn wobbly() -> Self {
52 Self::new(180.0, 8.0)
53 }
54
55 pub const fn stiff() -> Self {
57 Self::new(400.0, 40.0)
58 }
59
60 #[inline]
62 pub fn natural_frequency(&self) -> f64 {
63 (self.stiffness / self.mass.max(1e-6)).sqrt()
64 }
65
66 #[inline]
68 pub fn damping_ratio(&self) -> f64 {
69 self.damping / (2.0 * (self.mass.max(1e-6) * self.stiffness.max(1e-6)).sqrt())
70 }
71}
72
73impl Default for SpringConfig {
74 fn default() -> Self {
75 Self::snappy()
76 }
77}
78
79#[derive(Debug, Clone, Copy, PartialEq)]
81pub struct SpringState1D {
82 pub position: f64,
83 pub velocity: f64,
84 pub target: f64,
85}
86
87impl SpringState1D {
88 pub const fn new(position: f64, velocity: f64, target: f64) -> Self {
90 Self {
91 position,
92 velocity,
93 target,
94 }
95 }
96
97 pub fn evaluate_at(&self, config: &SpringConfig, t: f64) -> (f64, f64, bool) {
99 if t <= 0.0 {
100 let settled = (self.position - self.target).abs() <= config.rest_displacement_threshold
101 && self.velocity.abs() <= config.rest_velocity_threshold;
102 return (self.position, self.velocity, settled);
103 }
104
105 let x0 = self.position - self.target;
106 let v0 = self.velocity;
107 let omega_n = config.natural_frequency();
108 let zeta = config.damping_ratio();
109
110 let (pos_offset, vel) = if (zeta - 1.0).abs() < 1e-5 {
111 let decay = (-omega_n * t).exp();
113 let c1 = x0;
114 let c2 = v0 + omega_n * x0;
115 let x = (c1 + c2 * t) * decay;
116 let v = (c2 - omega_n * (c1 + c2 * t)) * decay;
117 (x, v)
118 } else if zeta < 1.0 {
119 let omega_d = omega_n * (1.0 - zeta * zeta).sqrt();
121 let decay = (-zeta * omega_n * t).exp();
122 let cos_term = (omega_d * t).cos();
123 let sin_term = (omega_d * t).sin();
124 let c1 = x0;
125 let c2 = (v0 + zeta * omega_n * x0) / omega_d;
126 let x = decay * (c1 * cos_term + c2 * sin_term);
127 let v = decay
128 * ((-zeta * omega_n * c1 + omega_d * c2) * cos_term
129 - (omega_d * c1 + zeta * omega_n * c2) * sin_term);
130 (x, v)
131 } else {
132 let gamma = omega_n * (zeta * zeta - 1.0).sqrt();
134 let r1 = -zeta * omega_n + gamma;
135 let r2 = -zeta * omega_n - gamma;
136 let c2 = (v0 - r1 * x0) / (r2 - r1);
137 let c1 = x0 - c2;
138 let exp1 = (r1 * t).exp();
139 let exp2 = (r2 * t).exp();
140 let x = c1 * exp1 + c2 * exp2;
141 let v = c1 * r1 * exp1 + c2 * r2 * exp2;
142 (x, v)
143 };
144
145 let final_pos = self.target + pos_offset;
146 let settled = pos_offset.abs() <= config.rest_displacement_threshold
147 && vel.abs() <= config.rest_velocity_threshold;
148
149 (final_pos, vel, settled)
150 }
151
152 pub fn step(&mut self, config: &SpringConfig, dt: f64) -> bool {
154 let (p, v, settled) = self.evaluate_at(config, dt);
155 if settled {
156 self.position = self.target;
157 self.velocity = 0.0;
158 true
159 } else {
160 self.position = p;
161 self.velocity = v;
162 false
163 }
164 }
165}
166
167#[derive(Debug, Clone, Copy, PartialEq)]
169pub struct SpringState3D {
170 pub position: [f64; 3],
171 pub velocity: [f64; 3],
172 pub target: [f64; 3],
173}
174
175impl SpringState3D {
176 pub const fn new(position: [f64; 3], velocity: [f64; 3], target: [f64; 3]) -> Self {
178 Self {
179 position,
180 velocity,
181 target,
182 }
183 }
184
185 pub fn evaluate_at(&self, config: &SpringConfig, t: f64) -> ([f64; 3], [f64; 3], bool) {
187 let mut out_pos = [0.0; 3];
188 let mut out_vel = [0.0; 3];
189 let mut settled = true;
190
191 for i in 0..3 {
192 let s1 = SpringState1D::new(self.position[i], self.velocity[i], self.target[i]);
193 let (p, v, s) = s1.evaluate_at(config, t);
194 out_pos[i] = p;
195 out_vel[i] = v;
196 if !s {
197 settled = false;
198 }
199 }
200
201 (out_pos, out_vel, settled)
202 }
203
204 pub fn step(&mut self, config: &SpringConfig, dt: f64) -> bool {
206 let (p, v, settled) = self.evaluate_at(config, dt);
207 if settled {
208 self.position = self.target;
209 self.velocity = [0.0, 0.0, 0.0];
210 true
211 } else {
212 self.position = p;
213 self.velocity = v;
214 false
215 }
216 }
217}
218
219#[cfg(test)]
220mod tests {
221 use super::*;
222
223 #[test]
224 fn critically_damped_spring_converges_to_target() {
225 let config = SpringConfig::snappy();
226 let mut state = SpringState1D::new(0.0, 0.0, 100.0);
227
228 let mut steps = 0;
230 while !state.step(&config, 1.0 / 60.0) && steps < 300 {
231 steps += 1;
232 }
233
234 assert_eq!(state.position, 100.0);
235 assert_eq!(state.velocity, 0.0);
236 assert!(steps < 200, "spring should settle in under 3.3s");
237 }
238
239 #[test]
240 fn under_damped_spring_bounces() {
241 let config = SpringConfig::bouncy();
242 let state = SpringState1D::new(0.0, 0.0, 100.0);
243
244 let mut max_pos = 0.0;
246 for i in 1..100 {
247 let t = i as f64 * 0.02;
248 let (p, _, _) = state.evaluate_at(&config, t);
249 if p > max_pos {
250 max_pos = p;
251 }
252 }
253
254 assert!(
255 max_pos > 100.0,
256 "bouncy spring must overshoot target: max={max_pos}"
257 );
258 }
259
260 #[test]
261 fn spring_3d_settles_all_axes() {
262 let config = SpringConfig::stiff();
263 let mut state = SpringState3D::new([0.0, 50.0, -100.0], [0.0; 3], [10.0, 0.0, 0.0]);
264
265 for _ in 0..150 {
266 state.step(&config, 1.0 / 60.0);
267 }
268
269 assert_eq!(state.position, [10.0, 0.0, 0.0]);
270 assert_eq!(state.velocity, [0.0, 0.0, 0.0]);
271 }
272}