Skip to main content

qualia_core_db/render/physics/response/
spring.rs

1use 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/// Stiffness and damping are per unit mass: acceleration is
10/// `stiffness * (target - position) - damping * velocity`.
11#[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}
29/// One admitted response interval, internally substepped by the shared solver.
30/// Hard travel/speed constraints are projected after integration. Contact
31/// initialization/teleport policy belongs to the caller and can bypass this.
32pub 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}