use super::super::error::{MotionError, MotionErrorCode};
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct SpringConfig {
tension: f32,
friction: f32,
mass: f32,
}
impl SpringConfig {
pub fn new(tension: f32, friction: f32, mass: f32) -> Result<Self, MotionError> {
MotionError::require(
tension.is_finite() && tension >= 0.0,
MotionErrorCode::SpringUnstable,
"tension must be finite and >= 0",
)?;
MotionError::require(
friction.is_finite() && friction >= 0.0,
MotionErrorCode::SpringUnstable,
"friction must be finite and >= 0",
)?;
MotionError::require(
mass.is_finite() && mass > 0.0,
MotionErrorCode::SpringUnstable,
"mass must be finite and > 0",
)?;
Ok(Self {
tension,
friction,
mass,
})
}
#[must_use]
pub const fn tension(&self) -> f32 {
self.tension
}
#[must_use]
pub const fn friction(&self) -> f32 {
self.friction
}
#[must_use]
pub const fn mass(&self) -> f32 {
self.mass
}
}
#[must_use]
pub fn integrate_spring(
position: f32,
velocity: f32,
target: f32,
config: &SpringConfig,
dt_seconds: f32,
) -> (f32, f32) {
let acceleration = |pos: f32, vel: f32| -> f32 {
(-config.tension * (pos - target) - config.friction * vel) / config.mass
};
let vel_k1 = acceleration(position, velocity);
let pos_k1 = velocity;
let vel_k2 = acceleration(
position + 0.5 * dt_seconds * pos_k1,
velocity + 0.5 * dt_seconds * vel_k1,
);
let pos_k2 = velocity + 0.5 * dt_seconds * vel_k1;
let vel_k3 = acceleration(
position + 0.5 * dt_seconds * pos_k2,
velocity + 0.5 * dt_seconds * vel_k2,
);
let pos_k3 = velocity + 0.5 * dt_seconds * vel_k2;
let vel_k4 = acceleration(
position + dt_seconds * pos_k3,
velocity + dt_seconds * vel_k3,
);
let pos_k4 = velocity + dt_seconds * vel_k3;
let new_position =
position + (dt_seconds / 6.0) * (pos_k1 + 2.0 * pos_k2 + 2.0 * pos_k3 + pos_k4);
let new_velocity =
velocity + (dt_seconds / 6.0) * (vel_k1 + 2.0 * vel_k2 + 2.0 * vel_k3 + vel_k4);
(new_position, new_velocity)
}