Files
makeCAD/crates/dynamics/src/integrate.rs
T
PaddleStroke 774c45a512 Dynamics: rigid-body solver core with joints, gravity and springs (#9719) (#9721)
* claim: #9719

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

* Dynamics: rigid-body solver core with joints, gravity and springs (#9719)

A new L1 crate, solidcraft-dynamics (geom only): rigid bodies in maximal coordinates, joint constraint equations with analytic Jacobians for rigid, revolute, slider, cylindrical, planar and ball joints, gravity, linear spring-dampers and constant loads; the index-1 equations of motion solved through a rank-revealing Schur complement (repeated joint equations are solved, not refused); RK4 and Dormand-Prince 5(4) with position and velocity projection; joint values, rates, reactions and energies at every output sample; caps on bodies, joints, rows, steps and samples; Result errors only.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

* Dynamics: take Vec3 from the record crate after the layer split, refresh the lock file

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

---------

Co-authored-by: Claude Opus 5.5 <noreply@anthropic.com>
Co-authored-by: bherbruck <brennen.herbruck@gmail.com>
2026-11-04 21:27:17 +00:00

204 lines
8.6 KiB
Rust
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
//! 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 (the system is autonomous, so the nodes are not needed): stage
/// weights, fifth-order weights and the difference between the fifth- and fourth-order weights
/// (the error estimate).
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, y: &[f64]) -> Result<Vec<f64>> {
self.sys.accelerations(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(y, self.settings.projection_tol).map_err(|e| at_time(e, t))?;
self.sys.project_velocities(y).map_err(|e| at_time(e, t))?;
}
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, y: &[f64], h: f64) -> Result<Vec<f64>> {
let k1 = self.deriv(y)?;
let k2 = self.deriv(&combine(y, std::slice::from_ref(&k1), &[0.5], h))?;
let k3 = self.deriv(&combine(y, std::slice::from_ref(&k2), &[0.5], h))?;
let k4 = self.deriv(&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, 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 in DP_A.iter().take(6) {
let stage = combine(y, &ks, a, h);
ks.push(self.deriv(&stage)?);
}
let y5 = combine(y, &ks, &DP_B, h);
ks.push(self.deriv(&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(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(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(y).map_err(|e| at_time(e, t))?;
let constraint_error = self.sys.constraint_error(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), 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 = System::initial_state(model)?;
// An inconsistent start (frames a little apart, velocities the joints don't allow) is
// brought onto the joints first, whether or not later steps project.
sys.project_positions(&mut y, settings.projection_tol).map_err(|e| at_time(e, 0.0))?;
sys.project_velocities(&mut y).map_err(|e| at_time(e, 0.0))?;
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 })
}