SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.
//! Time stepping: classical Runge–Kutta 4 with a fixed step, or Dormand–Prince 5(4) with an
//! embedded error estimate, each followed by projection onto the joints (positions, then
//! velocities), landing on every output sample.
use crate::model::{Integrator, MAX_MAGNITUDE, MAX_STEPS, Model, Settings};
use crate::system::System;
use crate::{DynamicsError, Result, Sample, Stats, Trajectory};
/// Dormand–Prince 5(4) tableau: nodes, stage weights, fifth-order weights and the difference
/// between the fifth- and fourth-order weights (the error estimate).
const DP_C: [f64; 7] = [0.0, 1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0];
const DP_A: [[f64; 6]; 7] = [
[0.0; 6],
[1.0 / 5.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[3.0 / 40.0, 9.0 / 40.0, 0.0, 0.0, 0.0, 0.0],
[44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0, 0.0, 0.0, 0.0],
[19372.0 / 6561.0, -25360.0 / 2187.0, 64448.0 / 6561.0, -212.0 / 729.0, 0.0, 0.0],
[9017.0 / 3168.0, -355.0 / 33.0, 46732.0 / 5247.0, 49.0 / 176.0, -5103.0 / 18656.0, 0.0],
[35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0],
];
const DP_B: [f64; 7] = [35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0, 0.0];
const DP_E: [f64; 7] = [
35.0 / 384.0 - 5179.0 / 57600.0,
0.0,
500.0 / 1113.0 - 7571.0 / 16695.0,
125.0 / 192.0 - 393.0 / 640.0,
-2187.0 / 6784.0 + 92097.0 / 339200.0,
11.0 / 84.0 - 187.0 / 2100.0,
-1.0 / 40.0,
];
/// `y + Σ k_i · w_i · h`.
fn combine(y: &[f64], ks: &[Vec<f64>], weights: &[f64], h: f64) -> Vec<f64> {
let mut out = y.to_vec();
for (k, w) in ks.iter().zip(weights) {
if *w == 0.0 {
continue;
}
for (o, d) in out.iter_mut().zip(k) {
*o += h * w * d;
}
}
out
}
fn at_time(e: DynamicsError, t: f64) -> DynamicsError {
match e {
DynamicsError::Singular { .. } => DynamicsError::Singular { time: t },
DynamicsError::Diverged { .. } => DynamicsError::Diverged { time: t },
other => other,
}
}
struct Run<'a> {
sys: &'a System,
settings: &'a Settings,
stats: Stats,
}
impl Run<'_> {
fn deriv(&self, t: f64, y: &[f64]) -> Result<Vec<f64>> {
self.sys.accelerations(t, y).map(|a| a.ydot)
}
/// Normalise, project and check a new state.
fn finish_step(&mut self, y: &mut [f64], t: f64) -> Result<()> {
self.sys.normalize(y).map_err(|e| at_time(e, t))?;
if self.settings.projection {
self.sys.project_positions(t, y, self.settings.projection_tol).map_err(|e| at_time(e, t))?;
self.sys.project_velocities(t, y).map_err(|e| at_time(e, t))?;
}
self.sys.resync(y);
if !y.iter().all(|x| x.is_finite() && x.abs() < MAX_MAGNITUDE) {
return Err(DynamicsError::Diverged { time: t });
}
Ok(())
}
fn count_step(&mut self, t: f64) -> Result<()> {
if self.stats.steps + self.stats.rejected >= MAX_STEPS {
return Err(DynamicsError::StepLimit { time: t });
}
Ok(())
}
fn rk4(&mut self, t: f64, y: &[f64], h: f64) -> Result<Vec<f64>> {
let k1 = self.deriv(t, y)?;
let k2 = self.deriv(t + 0.5 * h, &combine(y, std::slice::from_ref(&k1), &[0.5], h))?;
let k3 = self.deriv(t + 0.5 * h, &combine(y, std::slice::from_ref(&k2), &[0.5], h))?;
let k4 = self.deriv(t + h, &combine(y, std::slice::from_ref(&k3), &[1.0], h))?;
Ok(combine(y, &[k1, k2, k3, k4], &[1.0 / 6.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 6.0], h))
}
/// One Dormand–Prince step: the fifth-order state and the scaled error norm.
fn dopri(&mut self, t: f64, y: &[f64], h: f64, rel_tol: f64, abs_tol: f64) -> Result<(Vec<f64>, f64)> {
let mut ks: Vec<Vec<f64>> = Vec::with_capacity(7);
for (a, c) in DP_A.iter().zip(DP_C).take(6) {
let stage = combine(y, &ks, a, h);
ks.push(self.deriv(t + c * h, &stage)?);
}
let y5 = combine(y, &ks, &DP_B, h);
ks.push(self.deriv(t + h, &y5)?);
let err = combine(&vec![0.0; y.len()], &ks, &DP_E, h);
let n = y.len().max(1) as f64;
let sum: f64 = err
.iter()
.zip(y.iter().zip(&y5))
.map(|(e, (a, b))| {
let scale = abs_tol + rel_tol * a.abs().max(b.abs());
(e / scale).powi(2)
})
.sum();
Ok((y5, (sum / n).sqrt()))
}
/// Advance `y` from `t` to `t_end` exactly.
fn advance(&mut self, y: &mut Vec<f64>, t: &mut f64, t_end: f64, h_next: &mut f64) -> Result<()> {
let span = t_end - *t;
let eps = 1e-12 * t_end.abs().max(1.0);
match self.settings.integrator {
Integrator::Rk4 { step } => {
// Equal steps no longer than `step` that land on t_end.
let n = (span / step).ceil().max(1.0);
let h = span / n;
for _ in 0..n as usize {
self.count_step(*t)?;
let mut next = self.rk4(*t, y, h).map_err(|e| at_time(e, *t))?;
*t += h;
self.finish_step(&mut next, *t)?;
*y = next;
self.stats.steps += 1;
}
*t = t_end;
}
Integrator::Adaptive { rel_tol, abs_tol, min_step, max_step } => {
while t_end - *t > eps {
self.count_step(*t)?;
let remaining = t_end - *t;
let h = h_next.clamp(min_step, max_step).min(remaining);
let (mut next, err) = self.dopri(*t, y, h, rel_tol, abs_tol).map_err(|e| at_time(e, *t))?;
// The usual controller: 0.9·err^(−1/5), within [0.2, 5].
let factor = if err > 0.0 { (0.9 * err.powf(-0.2)).clamp(0.2, 5.0) } else { 5.0 };
if err <= 1.0 {
let landed = h >= remaining;
*t = if landed { t_end } else { *t + h };
self.finish_step(&mut next, *t)?;
*y = next;
self.stats.steps += 1;
// Keep the step chosen before a shortened landing step.
if !landed || factor < 1.0 {
*h_next = h * factor;
}
} else {
self.stats.rejected += 1;
if h <= min_step * (1.0 + 1e-9) {
return Err(DynamicsError::StepTooSmall { time: *t });
}
*h_next = (h * factor).max(min_step);
}
}
*t = t_end;
}
}
Ok(())
}
fn sample(&mut self, y: &[f64], t: f64) -> Result<Sample> {
let acc = self.sys.accelerations(t, y).map_err(|e| at_time(e, t))?;
let constraint_error = self.sys.constraint_error(t, y);
self.stats.max_constraint_error = self.stats.max_constraint_error.max(constraint_error);
self.stats.redundant_equations = self.stats.redundant_equations.max(acc.row_count().saturating_sub(acc.rank));
Ok(Sample {
time: t,
bodies: self.sys.body_states(y),
joints: self.sys.joint_results(y, &acc),
drives: self.sys.drive_efforts(&acc),
energy: self.sys.energy(y),
constraint_error,
})
}
}
/// Simulates `model` for `settings.duration`, returning the states at every output interval
/// (the first at t = 0, after the start has been projected onto the joints).
pub fn simulate(model: &Model, settings: &Settings) -> Result<Trajectory> {
settings.validate()?;
let sys = System::new(model, settings.baumgarte)?;
let mut y = sys.initial_state(model)?;
// An inconsistent start (frames a little apart, velocities the joints don't allow, drives
// starting elsewhere) is brought onto the joints first, whether or not later steps project.
sys.project_positions(0.0, &mut y, settings.projection_tol).map_err(|e| at_time(e, 0.0))?;
sys.project_velocities(0.0, &mut y).map_err(|e| at_time(e, 0.0))?;
sys.resync(&mut y);
let mut run = Run { sys: &sys, settings, stats: Stats::default() };
let count = (settings.duration / settings.output_interval).ceil() as usize;
let mut samples = Vec::with_capacity(count + 1);
samples.push(run.sample(&y, 0.0)?);
let mut t = 0.0;
let mut h_next = match settings.integrator {
Integrator::Rk4 { step } => step,
Integrator::Adaptive { min_step, max_step, .. } => (settings.output_interval * 0.1).clamp(min_step, max_step),
};
for k in 1..=count {
let t_end = (k as f64 * settings.output_interval).min(settings.duration);
if t_end <= t {
continue;
}
run.advance(&mut y, &mut t, t_end, &mut h_next)?;
samples.push(run.sample(&y, t)?);
}
Ok(Trajectory { samples, stats: run.stats })
}