qualia_core_db/render/physics/response/
spring.rs1use crate::solvers::calculus::{ODEFunction, RungeKutta4Static};
2use crate::solvers::{SolverConfig, SolverResult, SolversError};
3
4#[derive(Clone, Copy, Debug, PartialEq)]
5pub struct SpringState {
6 pub position: f64,
7 pub velocity: f64,
8}
9#[derive(Clone, Copy, Debug)]
12pub struct DrivenSpring {
13 pub target: f64,
14 pub stiffness: f64,
15 pub damping: f64,
16 pub max_displacement: f64,
17 pub max_speed: f64,
18}
19impl ODEFunction for DrivenSpring {
20 fn derivatives(&self, _t: f64, y: &[f64; 4]) -> [f64; 4] {
21 [
22 y[1],
23 self.stiffness * (self.target - y[0]) - self.damping * y[1],
24 0.0,
25 0.0,
26 ]
27 }
28}
29pub fn step_spring(state: SpringState, model: DrivenSpring, dt: f64) -> SolverResult<SpringState> {
33 if [
34 state.position,
35 state.velocity,
36 model.target,
37 model.stiffness,
38 model.damping,
39 model.max_displacement,
40 model.max_speed,
41 dt,
42 ]
43 .iter()
44 .any(|n| !n.is_finite())
45 || !(0.0..=0.05).contains(&dt)
46 || !(0.0..=1000.0).contains(&model.stiffness)
47 || !(0.0..=100.0).contains(&model.damping)
48 || model.max_displacement <= 0.0
49 || model.max_speed <= 0.0
50 {
51 return Err(SolversError::InvalidParameters);
52 }
53 if dt == 0.0 {
54 return Ok(state);
55 }
56 let h = dt.min(1.0 / 240.0);
57 let mut solver = RungeKutta4Static::new(
58 h,
59 SolverConfig {
60 max_iterations: 16,
61 ..SolverConfig::default()
62 },
63 );
64 let integrated =
65 solver.integrate(&model, 0.0, [state.position, state.velocity, 0.0, 0.0], dt)?;
66 let lower = model.target - model.max_displacement;
67 let upper = model.target + model.max_displacement;
68 let position = integrated.y[0].clamp(lower, upper);
69 let mut velocity = integrated.y[1].clamp(-model.max_speed, model.max_speed);
70 if (position == lower && velocity < 0.0) || (position == upper && velocity > 0.0) {
71 velocity = 0.0;
72 }
73 Ok(SpringState { position, velocity })
74}