use lgwks_deps::libm;
use super::super::error::{MotionError, MotionErrorCode};
#[derive(Clone, Copy, Debug)]
pub struct CriticalSpring {
position: f64,
velocity: f64,
target: f64,
omega: f64,
}
impl CriticalSpring {
pub fn new(position: f64, response_seconds: f64) -> Result<Self, MotionError> {
let domain_ok = position.is_finite()
&& position.abs() <= 1.0e9
&& response_seconds.is_finite()
&& (0.016..=10.0).contains(&response_seconds);
MotionError::require(
domain_ok,
MotionErrorCode::SpringUnstable,
"finite position and response in [0.016, 10] seconds required",
)?;
Ok(Self {
position,
velocity: 0.0,
target: position,
omega: 6.0 / response_seconds,
})
}
#[must_use]
pub const fn position(&self) -> f64 {
self.position
}
#[must_use]
pub const fn velocity(&self) -> f64 {
self.velocity
}
#[must_use]
pub const fn target(&self) -> f64 {
self.target
}
pub fn retarget(&mut self, target: f64) -> Result<(), MotionError> {
MotionError::require(
target.is_finite() && target.abs() <= 1.0e9,
MotionErrorCode::SpringUnstable,
"target must be finite and within the presentation domain",
)?;
self.target = target;
Ok(())
}
pub fn advance(&mut self, seconds: f64) -> Result<(), MotionError> {
MotionError::require(
seconds.is_finite() && (0.0..=60.0).contains(&seconds),
MotionErrorCode::SpringUnstable,
"elapsed time must be finite and in [0, 60] seconds",
)?;
if seconds <= 0.0 {
return Ok(());
}
let displacement = self.position - self.target;
let coefficient = self.velocity + self.omega * displacement;
let decay = libm::exp(-self.omega * seconds);
let position = self.target + (displacement + coefficient * seconds) * decay;
let velocity = (self.velocity - self.omega * coefficient * seconds) * decay;
MotionError::require(
position.is_finite() && velocity.is_finite(),
MotionErrorCode::SpringUnstable,
"motion evaluation overflowed",
)?;
self.position = position;
self.velocity = velocity;
Ok(())
}
#[must_use]
pub fn is_settled(&self) -> bool {
(self.position - self.target).abs() < 1.0e-5 && self.velocity.abs() < 1.0e-4
}
pub fn snap(&mut self) {
self.position = self.target;
self.velocity = 0.0;
}
}