Skip to content

SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.

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

PaddleStroke774c45a51263
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index acb4e8b11c..f3cc9a22a9 100644
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -88,6 +88,8 @@ The [architecture page](docs/architecture.md) has the layer table. The short ver
- `crates/geom`: vectors, planes, meshes, measures.
- `crates/sketch`: our own constraint solver, plus profile finding.
+- `crates/dynamics`: multibody dynamics: rigid bodies, joint constraints, gravity and springs,
+ integrated in time (RFC #9719). It depends only on `geom`.
- `crates/kernel`: the B-rep kernel boundary. It wraps [truck](https://github.com/ricosjp/truck),
and it's the only crate allowed to touch truck. Fillets, chamfers, shell/offset, Delete Face,
Split Face, STEP/IGES read and write, and face provenance are our own code here. Patched
diff --git a/Cargo.lock b/Cargo.lock
index 321a8fbfc9..5a6aa4838b 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -4153,6 +4153,17 @@ dependencies = [
"web-time",
]
+[[package]]
+name = "solidcraft-dynamics"
+version = "0.40.0-rc.2"
+dependencies = [
+ "serde",
+ "serde_json",
+ "solidcore-record",
+ "solidcraft-geom",
+ "thiserror 2.0.21",
+]
+
[[package]]
name = "solidcraft-engine"
version = "0.40.0-rc.2"
diff --git a/Cargo.toml b/Cargo.toml
index 70db751cfd..203dc8c0e5 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -22,6 +22,7 @@ solidcraft-kernel = { path = "crates/kernel" }
solidcraft-smlib = { path = "crates/smlib" }
solidcraft-step = { path = "crates/step" }
solidcraft-render = { path = "crates/render" }
+solidcraft-dynamics = { path = "crates/dynamics" }
solidcraft-gpu = { path = "crates/gpu" }
solidcraft-doc = { path = "crates/doc" }
solidcraft-io = { path = "crates/io" }
diff --git a/crates/dynamics/Cargo.toml b/crates/dynamics/Cargo.toml
new file mode 100644
index 0000000000..bfb82432f1
--- /dev/null
+++ b/crates/dynamics/Cargo.toml
@@ -0,0 +1,18 @@
+[package]
+name = "solidcraft-dynamics"
+description = "SolidCraft multibody dynamics: rigid bodies, joint constraints, gravity and springs, integrated in time"
+version.workspace = true
+edition.workspace = true
+license.workspace = true
+
+[lints]
+workspace = true
+
+[dependencies]
+solidcore-record = { workspace = true }
+solidcraft-geom = { workspace = true }
+serde = { workspace = true }
+thiserror = { workspace = true }
+
+[dev-dependencies]
+serde_json = { workspace = true }
diff --git a/crates/dynamics/src/integrate.rs b/crates/dynamics/src/integrate.rs
new file mode 100644
index 0000000000..f32ed3ebfb
--- /dev/null
+++ b/crates/dynamics/src/integrate.rs
@@ -0,0 +1,203 @@
+//! 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 })
+}
diff --git a/crates/dynamics/src/lib.rs b/crates/dynamics/src/lib.rs
new file mode 100644
index 0000000000..24f21c4d65
--- /dev/null
+++ b/crates/dynamics/src/lib.rs
@@ -0,0 +1,136 @@
+//! Multibody dynamics for SolidCraft (RFC #9719): rigid bodies with masses and inertias, held
+//! together by joints, moved by gravity, springs, dampers and loads, integrated in time.
+//!
+//! The crate knows nothing of documents or occurrences: a [`Model`] lists bodies (mass,
+//! inertia about the centre of mass, starting pose and velocities), joints between frames on
+//! them ([`JointKind`]: rigid, revolute, slider, cylindrical, planar, ball) and force elements;
+//! [`simulate`] returns a [`Trajectory`] of poses, joint values and rates, joint reactions and
+//! energies at a fixed output interval. The assembly layer maps occurrences and joints to it.
+//!
+//! The formulation is the textbook one for constrained rigid bodies in maximal coordinates
+//! (Lagrange multipliers, index-1 form, see `system.rs`), stepped by Runge–Kutta 4 or
+//! Dormand–Prince 5(4) and projected back onto the joints after every step (`integrate.rs`).
+//!
+//! Units are the design's: mm, kg, s and radians, so forces are in mN (kg·mm/s²), torques in
+//! mN·mm, stiffness in N/m (kg/s²) and gravity in mm/s² ([`STANDARD_GRAVITY`]).
+//!
+//! Every input is checked ([`Model::validate`], [`Settings::validate`]) and every size and loop
+//! is capped ([`MAX_BODIES`], [`MAX_STEPS`], …): a bad model is an error, never a panic.
+#![deny(clippy::unwrap_used, clippy::expect_used, clippy::panic, clippy::unimplemented, clippy::todo, clippy::unreachable)]
+#![forbid(unsafe_code)]
+
+mod integrate;
+mod linalg;
+pub mod math;
+mod model;
+mod system;
+
+use serde::{Deserialize, Serialize};
+use solidcore_record::Vec3;
+
+pub use integrate::simulate;
+pub use math::{Mat3, Quat};
+pub use model::{
+ Anchor, Body, Force, Integrator, Joint, JointKind, MAX_BODIES, MAX_CONSTRAINT_ROWS, MAX_FORCES, MAX_JOINTS, MAX_MAGNITUDE,
+ MAX_PROJECTION_ITERATIONS, MAX_SAMPLES, MAX_STEPS, Marker, Model, STANDARD_GRAVITY, Settings,
+};
+
+/// Why a simulation could not run or stopped.
+#[derive(Clone, Debug, PartialEq, thiserror::Error)]
+pub enum DynamicsError {
+ #[error("invalid model: {0}")]
+ InvalidModel(String),
+ #[error("invalid settings: {0}")]
+ InvalidSettings(String),
+ #[error("too large: {0}")]
+ TooLarge(String),
+ #[error("the joint equations could not be solved at t = {time} s")]
+ Singular { time: f64 },
+ #[error("the motion diverged at t = {time} s")]
+ Diverged { time: f64 },
+ #[error("the step needed at t = {time} s is below the smallest allowed")]
+ StepTooSmall { time: f64 },
+ #[error("the run needs more than the step limit (stopped at t = {time} s)")]
+ StepLimit { time: f64 },
+}
+
+pub type Result<T> = std::result::Result<T, DynamicsError>;
+
+/// A body's state at one time (world).
+#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
+pub struct BodyState {
+ /// Centre of mass (mm).
+ pub position: Vec3,
+ /// The body's axes (body → world).
+ pub orientation: Quat,
+ /// Velocity of the centre of mass (mm/s).
+ pub velocity: Vec3,
+ /// Angular velocity (rad/s).
+ pub angular_velocity: Vec3,
+}
+
+/// A joint's state at one time.
+#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub struct JointResult {
+ /// The joint's values ([`JointKind::values`]; radians and mm). A ball joint gives B's turn
+ /// from A as a rotation vector in A's frame.
+ pub values: Vec<f64>,
+ /// Their rates (rad/s, mm/s); a ball joint gives the relative angular velocity in A's frame.
+ pub rates: Vec<f64>,
+ /// The force the joint puts on `b`'s side (world, mN). Joints whose equations repeat
+ /// others' (a planar linkage of revolutes) share their reactions in one of many valid ways.
+ pub force: Vec3,
+ /// The torque the joint puts on `b`'s side about its frame's origin (world, mN·mm).
+ pub torque: Vec3,
+}
+
+/// Energies at one time (kg·mm²/s² = µJ).
+#[derive(Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize)]
+pub struct Energy {
+ pub kinetic: f64,
+ /// Potential energy of gravity, zero at the world origin.
+ pub gravity: f64,
+ /// Energy stored in springs (preload included).
+ pub spring: f64,
+}
+
+impl Energy {
+ pub fn total(&self) -> f64 {
+ self.kinetic + self.gravity + self.spring
+ }
+}
+
+/// The system at one output time.
+#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Sample {
+ /// Time (s).
+ pub time: f64,
+ pub bodies: Vec<BodyState>,
+ pub joints: Vec<JointResult>,
+ pub energy: Energy,
+ /// The largest joint equation error (mm for points; directions are unitless).
+ pub constraint_error: f64,
+}
+
+/// What a run did.
+#[derive(Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize)]
+pub struct Stats {
+ /// Accepted steps.
+ pub steps: usize,
+ /// Rejected steps (adaptive integration).
+ pub rejected: usize,
+ /// The largest joint equation error at an output sample.
+ pub max_constraint_error: f64,
+ /// Joint equations that repeat others (the most at any sample).
+ pub redundant_equations: usize,
+}
+
+/// A run's result: the samples at t = 0, one output interval, two, … up to the duration.
+#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Trajectory {
+ pub samples: Vec<Sample>,
+ pub stats: Stats,
+}
+
+#[cfg(test)]
+mod tests;
diff --git a/crates/dynamics/src/linalg.rs b/crates/dynamics/src/linalg.rs
new file mode 100644
index 0000000000..c0ccc159e1
--- /dev/null
+++ b/crates/dynamics/src/linalg.rs
@@ -0,0 +1,107 @@
+//! Dense symmetric positive semi-definite solves for the constraint equations.
+//!
+//! The matrix `J M⁻¹ Jᵀ` is singular when joints repeat each other's constraints (every
+//! revolute of a planar linkage built in 3D also holds the linkage in its plane), but its
+//! right-hand side is then consistent. A Cholesky factorisation with diagonal pivoting stops at
+//! the numerical rank and sets the multipliers of the dependent rows to zero, which solves such
+//! a system exactly.
+
+/// A square row-major matrix.
+#[derive(Clone, Debug)]
+pub(crate) struct Sym {
+ n: usize,
+ data: Vec<f64>,
+}
+
+impl Sym {
+ pub(crate) fn zeros(n: usize) -> Sym {
+ Sym { n, data: vec![0.0; n.saturating_mul(n)] }
+ }
+ pub(crate) fn get(&self, r: usize, c: usize) -> f64 {
+ self.data.get(r * self.n + c).copied().unwrap_or(0.0)
+ }
+ pub(crate) fn set(&mut self, r: usize, c: usize, v: f64) {
+ if let Some(x) = self.data.get_mut(r * self.n + c) {
+ *x = v;
+ }
+ }
+ fn swap(&mut self, a: usize, b: usize) {
+ if a == b {
+ return;
+ }
+ let n = self.n;
+ for c in 0..n {
+ self.data.swap(a * n + c, b * n + c);
+ }
+ for r in 0..n {
+ self.data.swap(r * n + a, r * n + b);
+ }
+ }
+}
+
+/// Solve `A x = b` for a symmetric positive semi-definite `A`, dropping the directions whose
+/// pivot falls below `rel_tol` times the largest diagonal. Returns the solution and the rank;
+/// `None` when the input is not finite or the sizes disagree.
+pub(crate) fn solve_psd(mut a: Sym, b: &[f64], rel_tol: f64) -> Option<(Vec<f64>, usize)> {
+ let n = a.n;
+ if b.len() != n || a.data.len() != n.checked_mul(n)? || !b.iter().chain(&a.data).all(|x| x.is_finite()) {
+ return None;
+ }
+ let mut perm: Vec<usize> = (0..n).collect();
+ let mut rhs = b.to_vec();
+ let max_diag = (0..n).map(|i| a.get(i, i)).fold(0.0_f64, f64::max);
+ let tol = (max_diag * rel_tol).max(f64::MIN_POSITIVE);
+ let mut rank = 0;
+ for k in 0..n {
+ // The largest remaining diagonal becomes the pivot.
+ let (best, best_val) = (k..n).map(|i| (i, a.get(i, i))).fold((k, f64::NEG_INFINITY), |acc, x| if x.1 > acc.1 { x } else { acc });
+ if !(best_val > tol) {
+ break;
+ }
+ a.swap(k, best);
+ perm.swap(k, best);
+ rhs.swap(k, best);
+ let l = best_val.sqrt();
+ a.set(k, k, l);
+ // Both triangles are kept, so a later pivot swap moves whole rows and columns.
+ for i in k + 1..n {
+ let v = a.get(i, k) / l;
+ a.set(i, k, v);
+ a.set(k, i, v);
+ }
+ for j in k + 1..n {
+ let ljk = a.get(j, k);
+ if ljk == 0.0 {
+ continue;
+ }
+ for i in j..n {
+ let v = a.get(i, j) - a.get(i, k) * ljk;
+ a.set(i, j, v);
+ a.set(j, i, v);
+ }
+ }
+ rank = k + 1;
+ }
+ // L y = rhs (first `rank` rows), then Lᵀ x = y; the dependent unknowns stay 0.
+ let mut y = vec![0.0; n];
+ for i in 0..rank {
+ let s = (0..i).fold(rhs.get(i).copied().unwrap_or(0.0), |s, j| s - a.get(i, j) * y.get(j).copied().unwrap_or(0.0));
+ if let Some(v) = y.get_mut(i) {
+ *v = s / a.get(i, i);
+ }
+ }
+ let mut x = vec![0.0; n];
+ for i in (0..rank).rev() {
+ let s = (i + 1..rank).fold(y.get(i).copied().unwrap_or(0.0), |s, j| s - a.get(j, i) * x.get(j).copied().unwrap_or(0.0));
+ if let Some(v) = x.get_mut(i) {
+ *v = s / a.get(i, i);
+ }
+ }
+ let mut out = vec![0.0; n];
+ for (k, p) in perm.iter().enumerate() {
+ if let (Some(o), Some(v)) = (out.get_mut(*p), x.get(k)) {
+ *o = *v;
+ }
+ }
+ out.iter().all(|v| v.is_finite()).then_some((out, rank))
+}
diff --git a/crates/dynamics/src/math.rs b/crates/dynamics/src/math.rs
new file mode 100644
index 0000000000..8eeebc593e
--- /dev/null
+++ b/crates/dynamics/src/math.rs
@@ -0,0 +1,203 @@
+//! Rotations for the solver: a 3×3 matrix and a unit quaternion.
+
+use serde::{Deserialize, Serialize};
+use solidcore_record::Vec3;
+use solidcraft_geom::prelude::*;
+
+/// A 3×3 matrix, row-major (`m[row][col]`); acts on column vectors.
+#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Mat3(pub [[f64; 3]; 3]);
+
+impl Mat3 {
+ pub const IDENTITY: Mat3 = Mat3([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]);
+ pub const ZERO: Mat3 = Mat3([[0.0; 3]; 3]);
+
+ /// The matrix whose columns are `a`, `b` and `c`.
+ pub fn from_cols(a: Vec3, b: Vec3, c: Vec3) -> Mat3 {
+ Mat3([[a.x, b.x, c.x], [a.y, b.y, c.y], [a.z, b.z, c.z]])
+ }
+ pub fn diagonal(d: Vec3) -> Mat3 {
+ Mat3([[d.x, 0.0, 0.0], [0.0, d.y, 0.0], [0.0, 0.0, d.z]])
+ }
+ pub fn col(&self, i: usize) -> Vec3 {
+ let [r0, r1, r2] = self.0;
+ match i {
+ 0 => Vec3::new(r0[0], r1[0], r2[0]),
+ 1 => Vec3::new(r0[1], r1[1], r2[1]),
+ _ => Vec3::new(r0[2], r1[2], r2[2]),
+ }
+ }
+ pub fn row(&self, i: usize) -> Vec3 {
+ let [a, b, c] = match i {
+ 0 => self.0[0],
+ 1 => self.0[1],
+ _ => self.0[2],
+ };
+ Vec3::new(a, b, c)
+ }
+ pub fn mul_vec(&self, v: Vec3) -> Vec3 {
+ Vec3::new(self.row(0).dot(v), self.row(1).dot(v), self.row(2).dot(v))
+ }
+ pub fn mul(&self, o: &Mat3) -> Mat3 {
+ Mat3::from_cols(self.mul_vec(o.col(0)), self.mul_vec(o.col(1)), self.mul_vec(o.col(2)))
+ }
+ pub fn transpose(&self) -> Mat3 {
+ Mat3::from_cols(self.row(0), self.row(1), self.row(2))
+ }
+ pub fn is_finite(&self) -> bool {
+ self.0.iter().flatten().all(|x| x.is_finite())
+ }
+ /// The largest difference between `m[i][j]` and `m[j][i]`.
+ pub fn asymmetry(&self) -> f64 {
+ let m = &self.0;
+ (m[0][1] - m[1][0]).abs().max((m[0][2] - m[2][0]).abs()).max((m[1][2] - m[2][1]).abs())
+ }
+ pub fn determinant(&self) -> f64 {
+ self.col(0).dot(self.col(1).cross(self.col(2)))
+ }
+ /// The inverse of a symmetric positive definite matrix (an inertia tensor); `None` when it is
+ /// not one (by its leading principal minors, scaled to its size).
+ pub fn spd_inverse(&self) -> Option<Mat3> {
+ let m = &self.0;
+ let scale = m[0][0].abs().max(m[1][1].abs()).max(m[2][2].abs());
+ if !(scale > 0.0) || !self.is_finite() || self.asymmetry() > 1e-9 * scale {
+ return None;
+ }
+ let rel = 1e-12;
+ let m1 = m[0][0];
+ let m2 = m[0][0] * m[1][1] - m[0][1] * m[1][0];
+ let det = self.determinant();
+ if !(m1 > rel * scale) || !(m2 > rel * scale * scale) || !(det > rel * scale * scale * scale) {
+ return None;
+ }
+ // Adjugate over determinant: the cross products of the columns are the adjugate's rows.
+ let (a, b, c) = (self.col(0), self.col(1), self.col(2));
+ let inv = Mat3::from_cols(b.cross(c), c.cross(a), a.cross(b)).transpose();
+ let inv = Mat3(inv.0.map(|r| r.map(|x| x / det)));
+ inv.is_finite().then_some(inv)
+ }
+}
+
+/// `a × (b × c)`-style helper: the vector `a × (a × v)`.
+pub fn cross2(a: Vec3, v: Vec3) -> Vec3 {
+ a.cross(a.cross(v))
+}
+
+/// A rotation as a unit quaternion `w + xi + yj + zk`.
+#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Quat {
+ pub w: f64,
+ pub x: f64,
+ pub y: f64,
+ pub z: f64,
+}
+
+impl Default for Quat {
+ fn default() -> Self {
+ Quat::IDENTITY
+ }
+}
+
+impl Quat {
+ pub const IDENTITY: Quat = Quat { w: 1.0, x: 0.0, y: 0.0, z: 0.0 };
+
+ pub fn vec(self) -> Vec3 {
+ Vec3::new(self.x, self.y, self.z)
+ }
+ /// The turn by `angle` (radians) about `axis` (any length; the identity for a zero axis).
+ pub fn from_axis_angle(axis: Vec3, angle: f64) -> Quat {
+ match axis.normalized() {
+ Some(a) => {
+ let (s, c) = (angle * 0.5).sin_cos();
+ Quat { w: c, x: a.x * s, y: a.y * s, z: a.z * s }
+ }
+ None => Quat::IDENTITY,
+ }
+ }
+ /// The turn by the rotation vector `v` (axis times angle).
+ pub fn from_rotation_vector(v: Vec3) -> Quat {
+ // sin(θ/2)/θ by its series for small turns: a projection's corrections are far below
+ // the length a direction can be normalised from.
+ let angle = v.len();
+ let (s, c) = (angle * 0.5).sin_cos();
+ let k = if angle < 1e-4 { 0.5 - angle * angle / 48.0 } else { s / angle };
+ if !k.is_finite() || !v.is_finite() {
+ return Quat::IDENTITY;
+ }
+ Quat { w: c, x: v.x * k, y: v.y * k, z: v.z * k }
+ }
+ /// The rotation whose matrix has these orthonormal columns (a right-handed frame).
+ pub fn from_mat3(m: &Mat3) -> Option<Quat> {
+ let r = &m.0;
+ let tr = r[0][0] + r[1][1] + r[2][2];
+ let q = if tr > 0.0 {
+ let s = (tr + 1.0).sqrt() * 2.0;
+ Quat { w: 0.25 * s, x: (r[2][1] - r[1][2]) / s, y: (r[0][2] - r[2][0]) / s, z: (r[1][0] - r[0][1]) / s }
+ } else if r[0][0] > r[1][1] && r[0][0] > r[2][2] {
+ let s = (1.0 + r[0][0] - r[1][1] - r[2][2]).sqrt() * 2.0;
+ Quat { w: (r[2][1] - r[1][2]) / s, x: 0.25 * s, y: (r[0][1] + r[1][0]) / s, z: (r[0][2] + r[2][0]) / s }
+ } else if r[1][1] > r[2][2] {
+ let s = (1.0 + r[1][1] - r[0][0] - r[2][2]).sqrt() * 2.0;
+ Quat { w: (r[0][2] - r[2][0]) / s, x: (r[0][1] + r[1][0]) / s, y: 0.25 * s, z: (r[1][2] + r[2][1]) / s }
+ } else {
+ let s = (1.0 + r[2][2] - r[0][0] - r[1][1]).sqrt() * 2.0;
+ Quat { w: (r[1][0] - r[0][1]) / s, x: (r[0][2] + r[2][0]) / s, y: (r[1][2] + r[2][1]) / s, z: 0.25 * s }
+ };
+ q.normalized()
+ }
+ /// The Hamilton product `self ⊗ o` (turn by `o`, then by `self`).
+ pub fn compose(self, o: Quat) -> Quat {
+ Quat {
+ w: self.w * o.w - self.x * o.x - self.y * o.y - self.z * o.z,
+ x: self.w * o.x + self.x * o.w + self.y * o.z - self.z * o.y,
+ y: self.w * o.y - self.x * o.z + self.y * o.w + self.z * o.x,
+ z: self.w * o.z + self.x * o.y - self.y * o.x + self.z * o.w,
+ }
+ }
+ pub fn conjugate(self) -> Quat {
+ Quat { w: self.w, x: -self.x, y: -self.y, z: -self.z }
+ }
+ pub fn norm(self) -> f64 {
+ (self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
+ }
+ pub fn is_finite(self) -> bool {
+ self.w.is_finite() && self.x.is_finite() && self.y.is_finite() && self.z.is_finite()
+ }
+ /// Scaled to length 1; `None` for a zero or non-finite quaternion.
+ pub fn normalized(self) -> Option<Quat> {
+ let n = self.norm();
+ (n > 1e-300 && n.is_finite()).then(|| Quat { w: self.w / n, x: self.x / n, y: self.y / n, z: self.z / n })
+ }
+ pub fn to_mat3(self) -> Mat3 {
+ let Quat { w, x, y, z } = self;
+ Mat3([
+ [1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z), 2.0 * (x * z + w * y)],
+ [2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - w * x)],
+ [2.0 * (x * z - w * y), 2.0 * (y * z + w * x), 1.0 - 2.0 * (x * x + y * y)],
+ ])
+ }
+ pub fn rotate(self, v: Vec3) -> Vec3 {
+ // v + 2 q×(q×v + w v)
+ let q = self.vec();
+ v + q.cross(q.cross(v) + v * self.w) * 2.0
+ }
+ /// The rate of change of this orientation when the body turns at `omega` (world frame):
+ /// `q̇ = ½ (0, ω) ⊗ q`.
+ pub fn rate(self, omega: Vec3) -> Quat {
+ let o = Quat { w: 0.0, x: omega.x, y: omega.y, z: omega.z }.compose(self);
+ Quat { w: 0.5 * o.w, x: 0.5 * o.x, y: 0.5 * o.y, z: 0.5 * o.z }
+ }
+ /// `self + k·o`, component-wise (an integrator's stage; normalise afterwards).
+ pub fn add_scaled(self, o: Quat, k: f64) -> Quat {
+ Quat { w: self.w + k * o.w, x: self.x + k * o.x, y: self.y + k * o.y, z: self.z + k * o.z }
+ }
+ /// The rotation vector (axis times angle, angle in [0, π]).
+ pub fn rotation_vector(self) -> Vec3 {
+ let q = if self.w < 0.0 { Quat { w: -self.w, x: -self.x, y: -self.y, z: -self.z } } else { self };
+ let s = q.vec().len();
+ if s < 1e-12 {
+ return q.vec() * 2.0;
+ }
+ q.vec() * (2.0 * s.atan2(q.w) / s)
+ }
+}
diff --git a/crates/dynamics/src/model.rs b/crates/dynamics/src/model.rs
new file mode 100644
index 0000000000..e4af1e1b6e
--- /dev/null
+++ b/crates/dynamics/src/model.rs
@@ -0,0 +1,385 @@
+//! What a simulation is made of: rigid bodies, joints between frames on them, force elements
+//! and the solver's settings. Everything is checked by [`Model::validate`] before a run.
+
+use serde::{Deserialize, Serialize};
+use solidcore_record::Vec3;
+use solidcraft_geom::prelude::*;
+
+use crate::math::{Mat3, Quat};
+use crate::{DynamicsError, Result};
+
+/// Bodies one simulation takes at most.
+pub const MAX_BODIES: usize = 200;
+/// Joints one simulation takes at most.
+pub const MAX_JOINTS: usize = 400;
+/// Constraint equations one simulation takes at most (the dense solve grows with their cube).
+pub const MAX_CONSTRAINT_ROWS: usize = 1200;
+/// Force elements one simulation takes at most.
+pub const MAX_FORCES: usize = 2000;
+/// Integration steps one run takes at most (accepted and rejected together).
+pub const MAX_STEPS: usize = 2_000_000;
+/// Output samples one run keeps at most.
+pub const MAX_SAMPLES: usize = 100_000;
+/// Projection iterations per step at most.
+pub const MAX_PROJECTION_ITERATIONS: usize = 50;
+/// Coordinates are kept within this size (mm, mm/s, rad/s); beyond it the run stops with
+/// [`DynamicsError::Diverged`].
+pub const MAX_MAGNITUDE: f64 = 1e12;
+
+/// A rigid body. Lengths in mm, mass in kg, time in s, so forces are in kg·mm/s² (mN) and
+/// torques in mN·mm.
+#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Body {
+ /// Mass (kg), > 0.
+ pub mass: f64,
+ /// Inertia tensor about the centre of mass in the body's own axes (kg·mm²), symmetric
+ /// positive definite.
+ pub inertia: Mat3,
+ /// Centre of mass at the start (world, mm). The body's own frame has its origin here.
+ pub position: Vec3,
+ /// The body's axes at the start (body → world).
+ #[serde(default)]
+ pub orientation: Quat,
+ /// Velocity of the centre of mass at the start (world, mm/s).
+ #[serde(default)]
+ pub velocity: Vec3,
+ /// Angular velocity at the start (world, rad/s).
+ #[serde(default)]
+ pub angular_velocity: Vec3,
+}
+
+impl Body {
+ /// A body at rest at `position`, axes along the world's, with a diagonal inertia.
+ pub fn at_rest(mass: f64, principal_inertia: Vec3, position: Vec3) -> Body {
+ Body {
+ mass,
+ inertia: Mat3::diagonal(principal_inertia),
+ position,
+ orientation: Quat::IDENTITY,
+ velocity: Vec3::ZERO,
+ angular_velocity: Vec3::ZERO,
+ }
+ }
+}
+
+/// A frame on a body (in the body's own coordinates: origin at its centre of mass) or, with no
+/// body, fixed in the world. `z` and `x` need not be unit or square: `x` is laid square to `z`.
+#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Marker {
+ #[serde(default, skip_serializing_if = "Option::is_none")]
+ pub body: Option<usize>,
+ pub origin: Vec3,
+ pub z: Vec3,
+ pub x: Vec3,
+}
+
+impl Marker {
+ pub fn new(body: Option<usize>, origin: Vec3, z: Vec3, x: Vec3) -> Marker {
+ Marker { body, origin, z, x }
+ }
+ /// A frame fixed in the world.
+ pub fn ground(origin: Vec3, z: Vec3, x: Vec3) -> Marker {
+ Marker { body: None, origin, z, x }
+ }
+}
+
+/// A point on a body (body coordinates) or, with no body, in the world.
+#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Anchor {
+ #[serde(default, skip_serializing_if = "Option::is_none")]
+ pub body: Option<usize>,
+ pub point: Vec3,
+}
+
+/// The joints the solver knows; each holds `b`'s frame to `a`'s as the matching assembly joint
+/// does (z is the joint's axis), with the frames coinciding at zero joint values.
+#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
+#[serde(rename_all = "snake_case")]
+pub enum JointKind {
+ /// No relative motion (6 equations).
+ Rigid,
+ /// Turns about z (5 equations).
+ Revolute,
+ /// Slides along z (5 equations).
+ Slider,
+ /// Turns about and slides along z (4 equations).
+ Cylindrical,
+ /// Slides in the xy plane and turns about z (3 equations).
+ Planar,
+ /// Turns any way about the common origin (3 equations).
+ Ball,
+}
+
+impl JointKind {
+ /// How many constraint equations the joint adds.
+ pub fn equations(self) -> usize {
+ match self {
+ JointKind::Rigid => 6,
+ JointKind::Revolute | JointKind::Slider => 5,
+ JointKind::Cylindrical => 4,
+ JointKind::Planar | JointKind::Ball => 3,
+ }
+ }
+ /// The joint's values, in the order [`crate::JointResult::values`] gives them.
+ pub fn values(self) -> &'static [&'static str] {
+ match self {
+ JointKind::Rigid => &[],
+ JointKind::Revolute => &["angle"],
+ JointKind::Slider => &["distance"],
+ JointKind::Cylindrical => &["angle", "distance"],
+ JointKind::Planar => &["x", "y", "angle"],
+ JointKind::Ball => &["rx", "ry", "rz"],
+ }
+ }
+}
+
+/// A joint: `b`'s frame held to `a`'s.
+#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Joint {
+ pub kind: JointKind,
+ pub a: Marker,
+ pub b: Marker,
+}
+
+/// A force element.
+#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
+#[serde(tag = "type", rename_all = "snake_case")]
+pub enum Force {
+ /// A linear spring and damper between two points: the tension is
+ /// `stiffness·(length − free_length) + damping·(rate of length) + preload`.
+ Spring {
+ a: Anchor,
+ b: Anchor,
+ /// kg/s² (= N/m = mN/mm), ≥ 0.
+ stiffness: f64,
+ /// kg/s (= N·s/m), ≥ 0.
+ damping: f64,
+ /// mm, ≥ 0.
+ free_length: f64,
+ /// mN, tension at free length.
+ #[serde(default)]
+ preload: f64,
+ },
+ /// A constant force (world, mN) at a point of a body, and a constant torque (world, mN·mm).
+ Load {
+ body: usize,
+ #[serde(default)]
+ point: Vec3,
+ #[serde(default)]
+ force: Vec3,
+ #[serde(default)]
+ torque: Vec3,
+ },
+}
+
+/// How the equations are stepped in time.
+#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
+#[serde(tag = "type", rename_all = "snake_case")]
+pub enum Integrator {
+ /// Classical fourth-order Runge–Kutta with a fixed step (s).
+ Rk4 { step: f64 },
+ /// Dormand–Prince 5(4) with error control: steps between `min_step` and `max_step` (s)
+ /// keep the local error under `abs_tol + rel_tol·|y|`.
+ Adaptive { rel_tol: f64, abs_tol: f64, min_step: f64, max_step: f64 },
+}
+
+/// A run's settings.
+#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
+pub struct Settings {
+ /// Simulated time (s), > 0.
+ pub duration: f64,
+ /// Time between output samples (s), > 0; the integrator lands on every sample.
+ pub output_interval: f64,
+ pub integrator: Integrator,
+ /// Project positions and velocities back onto the joints after every step (on by default).
+ #[serde(default = "yes")]
+ pub projection: bool,
+ /// The constraint error (mm, or radians times mm for directions) projection reaches.
+ #[serde(default = "default_projection_tol")]
+ pub projection_tol: f64,
+ /// Baumgarte stabilisation `(α, β)` (1/s): the constraint accelerations become
+ /// `Φ̈ + 2αΦ̇ + β²Φ = 0`. Off by default (projection keeps the joints).
+ #[serde(default, skip_serializing_if = "Option::is_none")]
+ pub baumgarte: Option<(f64, f64)>,
+}
+
+fn yes() -> bool {
+ true
+}
+fn default_projection_tol() -> f64 {
+ 1e-10
+}
+
+impl Settings {
+ /// Fixed-step RK4 settings.
+ pub fn rk4(duration: f64, step: f64, output_interval: f64) -> Settings {
+ Settings {
+ duration,
+ output_interval,
+ integrator: Integrator::Rk4 { step },
+ projection: true,
+ projection_tol: default_projection_tol(),
+ baumgarte: None,
+ }
+ }
+ /// Adaptive settings with the given relative and absolute tolerances.
+ pub fn adaptive(duration: f64, output_interval: f64, rel_tol: f64, abs_tol: f64) -> Settings {
+ Settings {
+ duration,
+ output_interval,
+ integrator: Integrator::Adaptive { rel_tol, abs_tol, min_step: 1e-9, max_step: output_interval },
+ projection: true,
+ projection_tol: default_projection_tol(),
+ baumgarte: None,
+ }
+ }
+}
+
+/// A multibody system.
+#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)]
+pub struct Model {
+ pub bodies: Vec<Body>,
+ #[serde(default)]
+ pub joints: Vec<Joint>,
+ #[serde(default)]
+ pub forces: Vec<Force>,
+ /// Gravity (mm/s²), e.g. `(0, 0, −9806.65)`.
+ #[serde(default)]
+ pub gravity: Vec3,
+}
+
+/// Standard gravity (mm/s²).
+pub const STANDARD_GRAVITY: f64 = 9806.65;
+
+fn finite_vec(v: Vec3) -> bool {
+ v.is_finite() && v.x.abs() < MAX_MAGNITUDE && v.y.abs() < MAX_MAGNITUDE && v.z.abs() < MAX_MAGNITUDE
+}
+
+fn bad(what: String) -> DynamicsError {
+ DynamicsError::InvalidModel(what)
+}
+
+/// A marker's axes in its body's coordinates (columns x, y, z), from its `z` and `x`.
+pub(crate) fn marker_axes(m: &Marker) -> Option<Mat3> {
+ let z = m.z.normalized()?;
+ let x = (m.x - z * m.x.dot(z)).normalized()?;
+ let y = z.cross(x).normalized()?;
+ Some(Mat3::from_cols(x, y, z))
+}
+
+impl Model {
+ /// Checks every number and reference, and the caps. Called by [`crate::simulate`].
+ pub fn validate(&self) -> Result<()> {
+ if self.bodies.is_empty() {
+ return Err(bad("no bodies".into()));
+ }
+ if self.bodies.len() > MAX_BODIES {
+ return Err(DynamicsError::TooLarge(format!("{} bodies (at most {MAX_BODIES})", self.bodies.len())));
+ }
+ if self.joints.len() > MAX_JOINTS {
+ return Err(DynamicsError::TooLarge(format!("{} joints (at most {MAX_JOINTS})", self.joints.len())));
+ }
+ if self.forces.len() > MAX_FORCES {
+ return Err(DynamicsError::TooLarge(format!("{} force elements (at most {MAX_FORCES})", self.forces.len())));
+ }
+ let rows: usize = self.joints.iter().map(|j| j.kind.equations()).sum();
+ if rows > MAX_CONSTRAINT_ROWS {
+ return Err(DynamicsError::TooLarge(format!("{rows} constraint equations (at most {MAX_CONSTRAINT_ROWS})")));
+ }
+ if !finite_vec(self.gravity) {
+ return Err(bad("gravity is not a finite vector".into()));
+ }
+ for (i, b) in self.bodies.iter().enumerate() {
+ if !(b.mass > 0.0) || !(b.mass < MAX_MAGNITUDE) {
+ return Err(bad(format!("body {i}: mass must be positive and finite")));
+ }
+ if b.inertia.spd_inverse().is_none() {
+ return Err(bad(format!("body {i}: inertia must be symmetric positive definite")));
+ }
+ if !finite_vec(b.position) || !finite_vec(b.velocity) || !finite_vec(b.angular_velocity) {
+ return Err(bad(format!("body {i}: position and velocities must be finite")));
+ }
+ if b.orientation.normalized().is_none() {
+ return Err(bad(format!("body {i}: orientation is not a rotation")));
+ }
+ }
+ let body_ok = |b: Option<usize>| b.is_none_or(|b| b < self.bodies.len());
+ for (i, j) in self.joints.iter().enumerate() {
+ for m in [&j.a, &j.b] {
+ if !body_ok(m.body) {
+ return Err(bad(format!("joint {i}: no body {}", m.body.unwrap_or(0))));
+ }
+ if !finite_vec(m.origin) || marker_axes(m).is_none() {
+ return Err(bad(format!("joint {i}: a frame's origin or axes are not usable")));
+ }
+ }
+ if j.a.body == j.b.body {
+ return Err(bad(format!("joint {i}: both frames are on the same body")));
+ }
+ }
+ for (i, f) in self.forces.iter().enumerate() {
+ match f {
+ Force::Spring { a, b, stiffness, damping, free_length, preload } => {
+ if !body_ok(a.body) || !body_ok(b.body) {
+ return Err(bad(format!("force {i}: no such body")));
+ }
+ if !finite_vec(a.point) || !finite_vec(b.point) {
+ return Err(bad(format!("force {i}: anchor points must be finite")));
+ }
+ let ok = |x: f64| (0.0..MAX_MAGNITUDE).contains(&x);
+ if !ok(*stiffness) || !ok(*damping) || !ok(*free_length) || !preload.is_finite() || preload.abs() >= MAX_MAGNITUDE {
+ return Err(bad(format!("force {i}: stiffness, damping and free length must be ≥ 0 and finite")));
+ }
+ }
+ Force::Load { body, point, force, torque } => {
+ if *body >= self.bodies.len() {
+ return Err(bad(format!("force {i}: no body {body}")));
+ }
+ if !finite_vec(*point) || !finite_vec(*force) || !finite_vec(*torque) {
+ return Err(bad(format!("force {i}: point, force and torque must be finite")));
+ }
+ }
+ }
+ }
+ Ok(())
+ }
+}
+
+impl Settings {
+ /// Checks the times, tolerances and that the run fits the step and sample caps.
+ pub fn validate(&self) -> Result<()> {
+ let pos = |x: f64| x > 0.0 && x < MAX_MAGNITUDE;
+ if !pos(self.duration) || !pos(self.output_interval) {
+ return Err(DynamicsError::InvalidSettings("duration and output interval must be positive and finite".into()));
+ }
+ let samples = (self.duration / self.output_interval).ceil();
+ if !(samples < MAX_SAMPLES as f64) {
+ return Err(DynamicsError::TooLarge(format!("{samples} output samples (at most {MAX_SAMPLES})")));
+ }
+ if !pos(self.projection_tol) {
+ return Err(DynamicsError::InvalidSettings("projection tolerance must be positive".into()));
+ }
+ if let Some((a, b)) = self.baumgarte
+ && (!(0.0..MAX_MAGNITUDE).contains(&a) || !(0.0..MAX_MAGNITUDE).contains(&b))
+ {
+ return Err(DynamicsError::InvalidSettings("Baumgarte terms must be ≥ 0 and finite".into()));
+ }
+ match self.integrator {
+ Integrator::Rk4 { step } => {
+ if !pos(step) {
+ return Err(DynamicsError::InvalidSettings("step must be positive".into()));
+ }
+ let steps = (self.duration / step.min(self.output_interval)).ceil();
+ if !(steps < MAX_STEPS as f64) {
+ return Err(DynamicsError::TooLarge(format!("{steps} steps (at most {MAX_STEPS})")));
+ }
+ }
+ Integrator::Adaptive { rel_tol, abs_tol, min_step, max_step } => {
+ if !pos(rel_tol) || !pos(abs_tol) || !pos(min_step) || !pos(max_step) || min_step > max_step {
+ return Err(DynamicsError::InvalidSettings("tolerances and step bounds must be positive, min ≤ max".into()));
+ }
+ }
+ }
+ Ok(())
+ }
+}
diff --git a/crates/dynamics/src/system.rs b/crates/dynamics/src/system.rs
new file mode 100644
index 0000000000..b118242659
--- /dev/null
+++ b/crates/dynamics/src/system.rs
@@ -0,0 +1,594 @@
+//! The equations of motion: body states, joint constraint equations with their Jacobians,
+//! applied forces, the constrained accelerations and the projections back onto the joints.
+//!
+//! Bodies use maximal coordinates: centre-of-mass position `x`, unit quaternion `q`, velocity
+//! `v` and angular velocity `ω` (both world). With `u = (v, ω)` per body, mass matrix
+//! `M = diag(m·I₃, I_w)` (`I_w = R I_body Rᵀ`), joint equations `Φ(x, q) = 0`, their Jacobian
+//! `J` (`Φ̇ = J u`) and `γ = −J̇ u`, the accelerations solve
+//!
+//! ```text
+//! M u̇ + Jᵀ λ = F − ω × I_w ω, J u̇ = γ
+//! ```
+//!
+//! through the Schur complement `(J M⁻¹ Jᵀ) λ = J M⁻¹ F − γ` (`linalg::solve_psd`, which takes
+//! repeated joint equations), and `λ` gives the joints' reactions.
+
+use solidcore_record::Vec3;
+use solidcraft_geom::prelude::*;
+
+use crate::linalg::{Sym, solve_psd};
+use crate::math::{Mat3, Quat, cross2};
+use crate::model::{Force, JointKind, MAX_MAGNITUDE, MAX_PROJECTION_ITERATIONS, Model, marker_axes};
+use crate::{BodyState, DynamicsError, Energy, JointResult, Result};
+
+/// Numbers per body in the flat state: x (3), q (4), v (3), ω (3).
+pub(crate) const STRIDE: usize = 13;
+
+/// Pivots under this fraction of the largest are taken as repeated equations.
+const RANK_TOL: f64 = 1e-10;
+
+fn v3(y: &[f64], at: usize) -> Vec3 {
+ let g = |i: usize| y.get(at + i).copied().unwrap_or(0.0);
+ Vec3::new(g(0), g(1), g(2))
+}
+fn put3(y: &mut [f64], at: usize, v: Vec3) {
+ for (i, c) in [v.x, v.y, v.z].into_iter().enumerate() {
+ if let Some(s) = y.get_mut(at + i) {
+ *s = c;
+ }
+ }
+}
+fn quat(y: &[f64], at: usize) -> Quat {
+ let g = |i: usize| y.get(at + i).copied().unwrap_or(0.0);
+ Quat { w: g(0), x: g(1), y: g(2), z: g(3) }
+}
+fn put_quat(y: &mut [f64], at: usize, q: Quat) {
+ for (i, c) in [q.w, q.x, q.y, q.z].into_iter().enumerate() {
+ if let Some(s) = y.get_mut(at + i) {
+ *s = c;
+ }
+ }
+}
+
+/// One body's precomputed constants.
+struct BodyConst {
+ mass: f64,
+ inertia: Mat3,
+ inertia_inv: Mat3,
+}
+
+/// A joint frame: its body, origin and axes in the body's coordinates.
+struct MarkerConst {
+ body: Option<usize>,
+ origin: Vec3,
+ axes: Mat3,
+}
+
+struct JointConst {
+ kind: JointKind,
+ a: MarkerConst,
+ b: MarkerConst,
+}
+
+/// A body's kinematics at one state.
+#[derive(Clone, Copy)]
+struct Kin {
+ x: Vec3,
+ rot: Mat3,
+ v: Vec3,
+ w: Vec3,
+}
+
+const GROUND: Kin = Kin { x: Vec3::ZERO, rot: Mat3::IDENTITY, v: Vec3::ZERO, w: Vec3::ZERO };
+
+/// A joint frame at one state (world).
+struct MarkerWorld {
+ body: Option<usize>,
+ /// Origin minus the body's centre of mass.
+ r: Vec3,
+ /// Origin.
+ p: Vec3,
+ /// Velocity of the origin.
+ pdot: Vec3,
+ axes: [Vec3; 3],
+ w: Vec3,
+}
+
+/// A body's part of one Jacobian row: (linear, angular).
+type Block = (usize, Vec3, Vec3);
+
+/// One constraint equation at a state.
+pub(crate) struct Row {
+ a: Option<Block>,
+ b: Option<Block>,
+ pub(crate) phi: f64,
+ /// `J̇ u`: the equation's second derivative at zero accelerations.
+ jdot_u: f64,
+}
+
+impl Row {
+ fn blocks(&self) -> impl Iterator<Item = &Block> {
+ self.a.iter().chain(self.b.iter())
+ }
+ /// `J u`.
+ fn rate(&self, kin: &[Kin]) -> f64 {
+ self.blocks().map(|(i, l, a)| kin.get(*i).map_or(0.0, |k| l.dot(k.v) + a.dot(k.w))).sum()
+ }
+}
+
+/// A model ready to integrate.
+pub(crate) struct System {
+ bodies: Vec<BodyConst>,
+ joints: Vec<JointConst>,
+ forces: Vec<Force>,
+ gravity: Vec3,
+ baumgarte: Option<(f64, f64)>,
+}
+
+/// Accelerations and multipliers at one state.
+pub(crate) struct Accel {
+ /// The flat state's derivative.
+ pub(crate) ydot: Vec<f64>,
+ /// One multiplier per constraint row, in joint order.
+ lambda: Vec<f64>,
+ rows: Vec<Row>,
+ /// The rank of `J M⁻¹ Jᵀ`.
+ pub(crate) rank: usize,
+}
+
+impl Accel {
+ /// The number of joint equations.
+ pub(crate) fn row_count(&self) -> usize {
+ self.rows.len()
+ }
+}
+
+fn marker_const(m: &crate::Marker) -> Result<MarkerConst> {
+ let axes = marker_axes(m).ok_or_else(|| DynamicsError::InvalidModel("a joint frame's axes are not usable".into()))?;
+ Ok(MarkerConst { body: m.body, origin: m.origin, axes })
+}
+
+impl System {
+ pub(crate) fn new(model: &Model, baumgarte: Option<(f64, f64)>) -> Result<System> {
+ model.validate()?;
+ let bodies = model
+ .bodies
+ .iter()
+ .map(|b| {
+ let inertia_inv = b.inertia.spd_inverse().ok_or_else(|| DynamicsError::InvalidModel("inertia is not positive definite".into()))?;
+ Ok(BodyConst { mass: b.mass, inertia: b.inertia, inertia_inv })
+ })
+ .collect::<Result<Vec<_>>>()?;
+ let joints = model
+ .joints
+ .iter()
+ .map(|j| Ok(JointConst { kind: j.kind, a: marker_const(&j.a)?, b: marker_const(&j.b)? }))
+ .collect::<Result<Vec<_>>>()?;
+ Ok(System { bodies, joints, forces: model.forces.clone(), gravity: model.gravity, baumgarte })
+ }
+
+ /// The flat state of the model's starting conditions.
+ pub(crate) fn initial_state(model: &Model) -> Result<Vec<f64>> {
+ let mut y = vec![0.0; model.bodies.len() * STRIDE];
+ for (i, b) in model.bodies.iter().enumerate() {
+ let at = i * STRIDE;
+ let q = b.orientation.normalized().ok_or_else(|| DynamicsError::InvalidModel(format!("body {i}: orientation is not a rotation")))?;
+ put3(&mut y, at, b.position);
+ put_quat(&mut y, at + 3, q);
+ put3(&mut y, at + 7, b.velocity);
+ put3(&mut y, at + 10, b.angular_velocity);
+ }
+ Ok(y)
+ }
+
+ /// Unit quaternions again after a step.
+ pub(crate) fn normalize(&self, y: &mut [f64]) -> Result<()> {
+ for i in 0..self.bodies.len() {
+ let q = quat(y, i * STRIDE + 3).normalized().ok_or(DynamicsError::Diverged { time: f64::NAN })?;
+ put_quat(y, i * STRIDE + 3, q);
+ }
+ Ok(())
+ }
+
+ fn kinematics(&self, y: &[f64]) -> Vec<Kin> {
+ (0..self.bodies.len())
+ .map(|i| {
+ let at = i * STRIDE;
+ Kin { x: v3(y, at), rot: quat(y, at + 3).to_mat3(), v: v3(y, at + 7), w: v3(y, at + 10) }
+ })
+ .collect()
+ }
+
+ fn kin_of(kin: &[Kin], body: Option<usize>) -> Kin {
+ body.and_then(|b| kin.get(b).copied()).unwrap_or(GROUND)
+ }
+
+ fn marker_world(kin: &[Kin], m: &MarkerConst) -> MarkerWorld {
+ let k = Self::kin_of(kin, m.body);
+ let r = k.rot.mul_vec(m.origin);
+ let axes = [k.rot.mul_vec(m.axes.col(0)), k.rot.mul_vec(m.axes.col(1)), k.rot.mul_vec(m.axes.col(2))];
+ MarkerWorld { body: m.body, r, p: k.x + r, pdot: k.v + k.w.cross(r), axes, w: k.w }
+ }
+
+ /// The constraint equations of every joint at `kin`, in joint order.
+ fn rows(&self, kin: &[Kin]) -> Vec<Row> {
+ let mut rows = Vec::new();
+ for j in &self.joints {
+ let a = Self::marker_world(kin, &j.a);
+ let b = Self::marker_world(kin, &j.b);
+ joint_rows(j.kind, &a, &b, &mut rows);
+ }
+ rows
+ }
+
+ /// `M⁻¹` applied to one body's (force, torque).
+ fn minv(&self, kin: &[Kin], body: usize, f: Vec3, t: Vec3) -> (Vec3, Vec3) {
+ match (self.bodies.get(body), kin.get(body)) {
+ (Some(b), Some(k)) => {
+ let iw_inv = k.rot.mul(&b.inertia_inv).mul(&k.rot.transpose());
+ (f * (1.0 / b.mass), iw_inv.mul_vec(t))
+ }
+ _ => (Vec3::ZERO, Vec3::ZERO),
+ }
+ }
+
+ /// `J M⁻¹ Jᵀ`, and each row's `M⁻¹ Jᵀ` blocks.
+ fn schur(&self, kin: &[Kin], rows: &[Row]) -> (Sym, Vec<Vec<Block>>) {
+ let w: Vec<Vec<Block>> = rows
+ .iter()
+ .map(|r| {
+ r.blocks()
+ .map(|(i, l, a)| {
+ let (ml, ma) = self.minv(kin, *i, *l, *a);
+ (*i, ml, ma)
+ })
+ .collect()
+ })
+ .collect();
+ let n = rows.len();
+ let mut s = Sym::zeros(n);
+ for (r, row) in rows.iter().enumerate() {
+ for (c, wc) in w.iter().enumerate().skip(r) {
+ let mut sum = 0.0;
+ for (i, l, a) in row.blocks() {
+ for (k, ml, ma) in wc {
+ if i == k {
+ sum += l.dot(*ml) + a.dot(*ma);
+ }
+ }
+ }
+ s.set(r, c, sum);
+ s.set(c, r, sum);
+ }
+ }
+ (s, w)
+ }
+
+ /// Applied forces and torques (about each centre of mass, world), gyroscopic terms included.
+ fn applied(&self, kin: &[Kin]) -> Vec<(Vec3, Vec3)> {
+ let mut out: Vec<(Vec3, Vec3)> = self
+ .bodies
+ .iter()
+ .zip(kin)
+ .map(|(b, k)| {
+ let iw = k.rot.mul(&b.inertia).mul(&k.rot.transpose());
+ (self.gravity * b.mass, -k.w.cross(iw.mul_vec(k.w)))
+ })
+ .collect();
+ let mut add = |body: Option<usize>, r: Vec3, f: Vec3, t: Vec3| {
+ if let Some(slot) = body.and_then(|b| out.get_mut(b)) {
+ slot.0 += f;
+ slot.1 += r.cross(f) + t;
+ }
+ };
+ for f in &self.forces {
+ match *f {
+ Force::Spring { a, b, stiffness, damping, free_length, preload } => {
+ let (ka, kb) = (Self::kin_of(kin, a.body), Self::kin_of(kin, b.body));
+ let (ra, rb) = (ka.rot.mul_vec(a.point), kb.rot.mul_vec(b.point));
+ let d = (kb.x + rb) - (ka.x + ra);
+ let len = d.len();
+ let Some(n) = d.normalized().filter(|_| len > 1e-9) else { continue };
+ let rate = n.dot((kb.v + kb.w.cross(rb)) - (ka.v + ka.w.cross(ra)));
+ let tension = stiffness * (len - free_length) + damping * rate + preload;
+ add(a.body, ra, n * tension, Vec3::ZERO);
+ add(b.body, rb, n * -tension, Vec3::ZERO);
+ }
+ Force::Load { body, point, force, torque } => {
+ let r = kin.get(body).map_or(Vec3::ZERO, |k| k.rot.mul_vec(point));
+ add(Some(body), r, force, torque);
+ }
+ }
+ }
+ out
+ }
+
+ /// The state's derivative and the joints' multipliers.
+ pub(crate) fn accelerations(&self, y: &[f64]) -> Result<Accel> {
+ let kin = self.kinematics(y);
+ let applied = self.applied(&kin);
+ let rows = self.rows(&kin);
+ let (s, w) = self.schur(&kin, &rows);
+ // Unconstrained accelerations M⁻¹F.
+ let free: Vec<(Vec3, Vec3)> = applied.iter().enumerate().map(|(i, (f, t))| self.minv(&kin, i, *f, *t)).collect();
+ let rhs: Vec<f64> = rows
+ .iter()
+ .map(|r| {
+ let jmf: f64 = r.blocks().map(|(i, l, a)| free.get(*i).map_or(0.0, |(fl, fa)| l.dot(*fl) + a.dot(*fa))).sum();
+ let stab = self.baumgarte.map_or(0.0, |(alpha, beta)| 2.0 * alpha * r.rate(&kin) + beta * beta * r.phi);
+ jmf + r.jdot_u + stab
+ })
+ .collect();
+ let (lambda, rank) =
+ if rows.is_empty() { (Vec::new(), 0) } else { solve_psd(s, &rhs, RANK_TOL).ok_or(DynamicsError::Singular { time: f64::NAN })? };
+ let mut acc = free;
+ for (wr, l) in w.iter().zip(&lambda) {
+ for (i, ml, ma) in wr {
+ if let Some(a) = acc.get_mut(*i) {
+ a.0 -= *ml * *l;
+ a.1 -= *ma * *l;
+ }
+ }
+ }
+ let mut ydot = vec![0.0; y.len()];
+ for (i, (k, a)) in kin.iter().zip(&acc).enumerate() {
+ let at = i * STRIDE;
+ put3(&mut ydot, at, k.v);
+ put_quat(&mut ydot, at + 3, quat(y, at + 3).rate(k.w));
+ put3(&mut ydot, at + 7, a.0);
+ put3(&mut ydot, at + 10, a.1);
+ }
+ if !ydot.iter().all(|x| x.is_finite()) {
+ return Err(DynamicsError::Diverged { time: f64::NAN });
+ }
+ Ok(Accel { ydot, lambda, rows, rank })
+ }
+
+ /// The largest joint equation error at `y`.
+ pub(crate) fn constraint_error(&self, y: &[f64]) -> f64 {
+ self.rows(&self.kinematics(y)).iter().map(|r| r.phi.abs()).fold(0.0, f64::max)
+ }
+
+ /// Moves the bodies the least (mass-weighted) onto the joints: Gauss–Newton on `Φ = 0`.
+ /// Returns the error left.
+ pub(crate) fn project_positions(&self, y: &mut [f64], tol: f64) -> Result<f64> {
+ let mut err;
+ let mut iteration = 0;
+ loop {
+ let kin = self.kinematics(y);
+ let rows = self.rows(&kin);
+ err = rows.iter().map(|r| r.phi.abs()).fold(0.0, f64::max);
+ if !(err > tol) || iteration >= MAX_PROJECTION_ITERATIONS {
+ break;
+ }
+ iteration += 1;
+ let (s, w) = self.schur(&kin, &rows);
+ let phi: Vec<f64> = rows.iter().map(|r| r.phi).collect();
+ let (mu, _) = solve_psd(s, &phi, RANK_TOL).ok_or(DynamicsError::Singular { time: f64::NAN })?;
+ let mut delta = vec![(Vec3::ZERO, Vec3::ZERO); self.bodies.len()];
+ for (wr, m) in w.iter().zip(&mu) {
+ for (i, ml, ma) in wr {
+ if let Some(d) = delta.get_mut(*i) {
+ d.0 -= *ml * *m;
+ d.1 -= *ma * *m;
+ }
+ }
+ }
+ for (i, (dx, dth)) in delta.into_iter().enumerate() {
+ let at = i * STRIDE;
+ put3(y, at, v3(y, at) + dx);
+ let q = Quat::from_rotation_vector(dth).compose(quat(y, at + 3)).normalized().ok_or(DynamicsError::Diverged { time: f64::NAN })?;
+ put_quat(y, at + 3, q);
+ }
+ }
+ if !err.is_finite() || !y.iter().all(|x| x.is_finite() && x.abs() < MAX_MAGNITUDE) {
+ return Err(DynamicsError::Diverged { time: f64::NAN });
+ }
+ Ok(err)
+ }
+
+ /// Removes the velocities the joints don't allow (mass-weighted): `J u = 0`.
+ pub(crate) fn project_velocities(&self, y: &mut [f64]) -> Result<()> {
+ let kin = self.kinematics(y);
+ let rows = self.rows(&kin);
+ if rows.is_empty() {
+ return Ok(());
+ }
+ let (s, w) = self.schur(&kin, &rows);
+ let ju: Vec<f64> = rows.iter().map(|r| r.rate(&kin)).collect();
+ let (mu, _) = solve_psd(s, &ju, RANK_TOL).ok_or(DynamicsError::Singular { time: f64::NAN })?;
+ for (wr, m) in w.iter().zip(&mu) {
+ for (i, ml, ma) in wr {
+ let at = i * STRIDE;
+ put3(y, at + 7, v3(y, at + 7) - *ml * *m);
+ put3(y, at + 10, v3(y, at + 10) - *ma * *m);
+ }
+ }
+ Ok(())
+ }
+
+ /// The bodies' states.
+ pub(crate) fn body_states(&self, y: &[f64]) -> Vec<BodyState> {
+ (0..self.bodies.len())
+ .map(|i| {
+ let at = i * STRIDE;
+ BodyState { position: v3(y, at), orientation: quat(y, at + 3), velocity: v3(y, at + 7), angular_velocity: v3(y, at + 10) }
+ })
+ .collect()
+ }
+
+ /// Kinetic, gravity and spring energy.
+ pub(crate) fn energy(&self, y: &[f64]) -> Energy {
+ let kin = self.kinematics(y);
+ let mut e = Energy::default();
+ for (b, k) in self.bodies.iter().zip(&kin) {
+ let iw = k.rot.mul(&b.inertia).mul(&k.rot.transpose());
+ e.kinetic += 0.5 * b.mass * k.v.len2() + 0.5 * k.w.dot(iw.mul_vec(k.w));
+ e.gravity -= b.mass * self.gravity.dot(k.x);
+ }
+ for f in &self.forces {
+ if let Force::Spring { a, b, stiffness, free_length, preload, .. } = *f {
+ let (ka, kb) = (Self::kin_of(&kin, a.body), Self::kin_of(&kin, b.body));
+ let stretch = ((kb.x + kb.rot.mul_vec(b.point)) - (ka.x + ka.rot.mul_vec(a.point))).len() - free_length;
+ e.spring += 0.5 * stiffness * stretch * stretch + preload * stretch;
+ }
+ }
+ e
+ }
+
+ /// Each joint's values, rates and reaction (on `b`'s side, at its frame's origin, world),
+ /// from the multipliers `acc` found at `y`.
+ pub(crate) fn joint_results(&self, y: &[f64], acc: &Accel) -> Vec<JointResult> {
+ let kin = self.kinematics(y);
+ let mut at = 0;
+ let mut out = Vec::with_capacity(self.joints.len());
+ for j in &self.joints {
+ let a = Self::marker_world(&kin, &j.a);
+ let b = Self::marker_world(&kin, &j.b);
+ let (values, rates) = joint_values(j.kind, &a, &b);
+ let n = j.kind.equations();
+ // The wrench the joint's equations put on each body: −Jᵀλ.
+ let (mut fa, mut ta, mut fb, mut tb) = (Vec3::ZERO, Vec3::ZERO, Vec3::ZERO, Vec3::ZERO);
+ for k in at..at + n {
+ let (Some(row), Some(l)) = (acc.rows.get(k), acc.lambda.get(k)) else { continue };
+ if let Some((_, lin, ang)) = row.a {
+ fa -= lin * *l;
+ ta -= ang * *l;
+ }
+ if let Some((_, lin, ang)) = row.b {
+ fb -= lin * *l;
+ tb -= ang * *l;
+ }
+ }
+ at += n;
+ let (force, torque) = if b.body.is_some() {
+ (fb, tb - b.r.cross(fb))
+ } else {
+ // B is the ground: it takes what A takes, turned round, about B's origin.
+ let xa = Self::kin_of(&kin, a.body).x;
+ (-fa, -(ta + (xa - b.p).cross(fa)))
+ };
+ out.push(JointResult { values, rates, force, torque });
+ }
+ out
+ }
+}
+
+/// `(Φ, J, J̇u)` for one body point's world component along `e`: `e·(p_B − p_A)`.
+fn point_row(a: &MarkerWorld, b: &MarkerWorld, e: Vec3) -> Row {
+ let acc = |m: &MarkerWorld| cross2(m.w, m.r);
+ Row { a: a.body.map(|i| (i, -e, -a.r.cross(e))), b: b.body.map(|i| (i, e, b.r.cross(e))), phi: e.dot(b.p - a.p), jdot_u: e.dot(acc(b) - acc(a)) }
+}
+
+/// `u·v = 0` for a direction `u` of A and `v` of B.
+fn dot1_row(a: &MarkerWorld, u: Vec3, b: &MarkerWorld, v: Vec3) -> Row {
+ let (ud, vd) = (a.w.cross(u), b.w.cross(v));
+ let (udd, vdd) = (cross2(a.w, u), cross2(b.w, v));
+ Row {
+ a: a.body.map(|i| (i, Vec3::ZERO, u.cross(v))),
+ b: b.body.map(|i| (i, Vec3::ZERO, v.cross(u))),
+ phi: u.dot(v),
+ jdot_u: udd.dot(v) + 2.0 * ud.dot(vd) + u.dot(vdd),
+ }
+}
+
+/// `d·u = 0` for `d = p_B − p_A` and a direction `u` of A.
+fn dot2_row(a: &MarkerWorld, u: Vec3, b: &MarkerWorld) -> Row {
+ let d = b.p - a.p;
+ let dd = b.pdot - a.pdot;
+ let ddd = cross2(b.w, b.r) - cross2(a.w, a.r);
+ let (ud, udd) = (a.w.cross(u), cross2(a.w, u));
+ Row {
+ a: a.body.map(|i| (i, -u, u.cross(d) - a.r.cross(u))),
+ b: b.body.map(|i| (i, u, b.r.cross(u))),
+ phi: d.dot(u),
+ jdot_u: ddd.dot(u) + 2.0 * dd.dot(ud) + d.dot(udd),
+ }
+}
+
+fn joint_rows(kind: JointKind, a: &MarkerWorld, b: &MarkerWorld, rows: &mut Vec<Row>) {
+ let [ax, ay, az] = a.axes;
+ let [bx, by, bz] = b.axes;
+ let point = |rows: &mut Vec<Row>| rows.extend([Vec3::X, Vec3::Y, Vec3::Z].map(|e| point_row(a, b, e)));
+ match kind {
+ JointKind::Rigid => {
+ point(rows);
+ rows.extend([dot1_row(a, ay, b, bz), dot1_row(a, az, b, bx), dot1_row(a, ax, b, by)]);
+ }
+ JointKind::Revolute => {
+ point(rows);
+ rows.extend([dot1_row(a, ax, b, bz), dot1_row(a, ay, b, bz)]);
+ }
+ JointKind::Ball => point(rows),
+ JointKind::Cylindrical => {
+ rows.extend([dot2_row(a, ax, b), dot2_row(a, ay, b), dot1_row(a, ax, b, bz), dot1_row(a, ay, b, bz)]);
+ }
+ JointKind::Slider => {
+ rows.extend([dot2_row(a, ax, b), dot2_row(a, ay, b), dot1_row(a, ax, b, bz), dot1_row(a, ay, b, bz), dot1_row(a, ax, b, by)]);
+ }
+ JointKind::Planar => {
+ rows.extend([dot2_row(a, az, b), dot1_row(a, az, b, bx), dot1_row(a, az, b, by)]);
+ }
+ }
+}
+
+/// A joint's values and their rates (see [`JointKind::values`]).
+fn joint_values(kind: JointKind, a: &MarkerWorld, b: &MarkerWorld) -> (Vec<f64>, Vec<f64>) {
+ let [ax, ay, az] = a.axes;
+ let bx = b.axes[0];
+ let d = b.p - a.p;
+ let dd = b.pdot - a.pdot;
+ let angle = || bx.dot(ay).atan2(bx.dot(ax));
+ let spin = || (b.w - a.w).dot(az);
+ let along = |u: Vec3| (d.dot(u), dd.dot(u) + d.dot(a.w.cross(u)));
+ match kind {
+ JointKind::Rigid => (vec![], vec![]),
+ JointKind::Revolute => (vec![angle()], vec![spin()]),
+ JointKind::Slider => {
+ let (s, r) = along(az);
+ (vec![s], vec![r])
+ }
+ JointKind::Cylindrical => {
+ let (s, r) = along(az);
+ (vec![angle(), s], vec![spin(), r])
+ }
+ JointKind::Planar => {
+ let ((x, xr), (y, yr)) = (along(ax), along(ay));
+ (vec![x, y, angle()], vec![xr, yr, spin()])
+ }
+ JointKind::Ball => {
+ // B's turn from A as a rotation vector in A's frame, and the relative angular
+ // velocity in A's frame.
+ let am = Mat3::from_cols(ax, ay, az);
+ let rel = am.transpose().mul(&Mat3::from_cols(b.axes[0], b.axes[1], b.axes[2]));
+ let rv = Quat::from_mat3(&rel).map_or(Vec3::ZERO, Quat::rotation_vector);
+ let w = am.transpose().mul_vec(b.w - a.w);
+ (vec![rv.x, rv.y, rv.z], vec![w.x, w.y, w.z])
+ }
+ }
+}
+
+#[cfg(test)]
+pub(crate) mod testing {
+ //! Finite-difference checks of the Jacobians and `J̇u` (tests only).
+ use super::*;
+
+ /// `(Φ, J u, J̇ u)` of every row at `y`.
+ pub(crate) fn rows_at(sys: &System, y: &[f64]) -> Vec<(f64, f64, f64)> {
+ let kin = sys.kinematics(y);
+ sys.rows(&kin).iter().map(|r| (r.phi, r.rate(&kin), r.jdot_u)).collect()
+ }
+
+ /// The state moved along its velocities for `h` (positions and orientations only).
+ pub(crate) fn drift(sys: &System, y: &[f64], h: f64) -> Vec<f64> {
+ let mut out = y.to_vec();
+ let kin = sys.kinematics(y);
+ for (i, k) in kin.iter().enumerate() {
+ let at = i * STRIDE;
+ put3(&mut out, at, k.x + k.v * h);
+ let q = Quat::from_rotation_vector(k.w * h).compose(quat(y, at + 3));
+ put_quat(&mut out, at + 3, q);
+ }
+ out
+ }
+}
diff --git a/crates/dynamics/src/tests.rs b/crates/dynamics/src/tests.rs
new file mode 100644
index 0000000000..877568fd8e
--- /dev/null
+++ b/crates/dynamics/src/tests.rs
@@ -0,0 +1,540 @@
+//! Analytic checks: free fall, pendulum periods, spring-mass oscillation, energy and momentum
+//! conservation, slider-crank kinematics, joint reactions, Jacobians against finite
+//! differences, and hostile input.
+
+use std::f64::consts::{PI, TAU};
+
+use solidcore_record::Vec3;
+use solidcraft_geom::prelude::*;
+
+use crate::system::{System, testing};
+use crate::*;
+
+const G: f64 = STANDARD_GRAVITY;
+
+fn down() -> Vec3 {
+ Vec3::new(0.0, 0.0, -G)
+}
+
+fn ok<T>(r: Result<T>) -> T {
+ match r {
+ Ok(v) => v,
+ Err(e) => panic!("simulation failed: {e}"),
+ }
+}
+
+/// A body hanging `len` mm under a hinge at the origin turning about Y, at angle `theta` from
+/// straight down (towards +X) and turning at `omega`.
+fn pendulum(len: f64, theta: f64, omega: f64, inertia: f64) -> Model {
+ let dir = Vec3::new(theta.sin(), 0.0, -theta.cos());
+ // Turning about +Y takes −Z towards −X; the hinge's own Y axis is the world's.
+ let rot = Quat::from_axis_angle(Vec3::Y, -theta);
+ let mut body = Body::at_rest(1.0, Vec3::new(inertia, inertia, inertia), dir * len);
+ body.orientation = rot;
+ body.angular_velocity = Vec3::Y * -omega;
+ body.velocity = (Vec3::Y * -omega).cross(dir * len);
+ let joint = Joint {
+ kind: JointKind::Revolute,
+ a: Marker::ground(Vec3::ZERO, Vec3::Y, Vec3::X),
+ b: Marker::new(Some(0), Vec3::new(0.0, 0.0, len), Vec3::Y, Vec3::X),
+ };
+ Model { bodies: vec![body], joints: vec![joint], forces: vec![], gravity: down() }
+}
+
+/// Times where `f` crosses zero upwards, linearly interpolated between samples.
+fn upward_crossings(traj: &Trajectory, f: impl Fn(&Sample) -> f64) -> Vec<f64> {
+ traj.samples
+ .windows(2)
+ .filter_map(|w| {
+ let (a, b) = (w.first()?, w.get(1)?);
+ let (fa, fb) = (f(a), f(b));
+ (fa < 0.0 && fb >= 0.0).then(|| a.time + (b.time - a.time) * fa / (fa - fb))
+ })
+ .collect()
+}
+
+fn mean_period(crossings: &[f64]) -> f64 {
+ assert!(crossings.len() >= 3, "too few crossings: {crossings:?}");
+ let (first, last) = (crossings.first().copied().unwrap_or(0.0), crossings.last().copied().unwrap_or(0.0));
+ (last - first) / (crossings.len() - 1) as f64
+}
+
+fn angle_of(s: &Sample) -> f64 {
+ s.joints.first().and_then(|j| j.values.first()).copied().unwrap_or(f64::NAN)
+}
+
+#[test]
+fn free_fall_follows_the_parabola() {
+ let mut body = Body::at_rest(2.5, Vec3::new(10.0, 20.0, 30.0), Vec3::new(1.0, 2.0, 3.0));
+ body.velocity = Vec3::new(10.0, -5.0, 50.0);
+ body.angular_velocity = Vec3::new(0.0, 0.0, 1.0);
+ let model = Model { bodies: vec![body.clone()], joints: vec![], forces: vec![], gravity: down() };
+ let traj = ok(simulate(&model, &Settings::rk4(1.0, 0.01, 0.1)));
+ assert_eq!(traj.samples.len(), 11);
+ for s in &traj.samples {
+ let t = s.time;
+ let expect = body.position + body.velocity * t + down() * (0.5 * t * t);
+ let got = s.bodies.first().map(|b| b.position).unwrap_or(Vec3::ZERO);
+ assert!(got.dist(expect) < 1e-7, "t={t}: {got:?} vs {expect:?}");
+ // A symmetric body spinning about z keeps spinning.
+ let w = s.bodies.first().map(|b| b.angular_velocity).unwrap_or(Vec3::ZERO);
+ assert!(w.dist(Vec3::Z) < 1e-12);
+ // Energy is conserved exactly enough.
+ let e0 = traj.samples.first().map(|s| s.energy.total()).unwrap_or(0.0);
+ assert!((s.energy.total() - e0).abs() < 1e-6 * e0.abs().max(1.0));
+ }
+}
+
+#[test]
+fn pendulum_small_angle_period() {
+ let (len, inertia) = (100.0, 40.0);
+ let i_pivot = inertia + len * len;
+ let period = TAU * (i_pivot / (G * len)).sqrt();
+ let amplitude = 0.01;
+ let model = pendulum(len, 0.0, amplitude * TAU / period, inertia);
+ let traj = ok(simulate(&model, &Settings::rk4(3.0, 1e-3, 1e-3)));
+ let measured = mean_period(&upward_crossings(&traj, angle_of));
+ // The finite amplitude lengthens the period by about θ²/16 (6e-6 here).
+ assert!((measured - period).abs() < 1e-4 * period, "period {measured} vs {period}");
+ assert!(traj.stats.max_constraint_error < 1e-9, "{:?}", traj.stats);
+}
+
+#[test]
+fn adaptive_integration_matches_the_pendulum_period() {
+ let (len, inertia) = (100.0, 40.0);
+ let period = TAU * ((inertia + len * len) / (G * len)).sqrt();
+ let model = pendulum(len, 0.0, 0.01 * TAU / period, inertia);
+ let traj = ok(simulate(&model, &Settings::adaptive(3.0, 1e-3, 1e-9, 1e-9)));
+ let measured = mean_period(&upward_crossings(&traj, angle_of));
+ assert!((measured - period).abs() < 1e-4 * period, "period {measured} vs {period}");
+ // Larger tolerances take fewer steps than samples (the output still lands on each).
+ let coarse = Settings {
+ integrator: Integrator::Adaptive { rel_tol: 1e-6, abs_tol: 1e-6, min_step: 1e-9, max_step: 0.05 },
+ ..Settings::rk4(3.0, 1.0, 0.05)
+ };
+ let traj = ok(simulate(&model, &coarse));
+ assert_eq!(traj.samples.len(), 61);
+ let measured = mean_period(&upward_crossings(&traj, angle_of));
+ assert!((measured - period).abs() < 2e-3 * period, "coarse period {measured} vs {period}");
+}
+
+#[test]
+fn hanging_body_pin_carries_its_weight() {
+ let model = pendulum(100.0, 0.0, 0.0, 40.0);
+ let traj = ok(simulate(&model, &Settings::rk4(0.1, 1e-3, 0.05)));
+ for s in &traj.samples {
+ let j = s.joints.first().map(|j| (j.force, j.torque)).unwrap_or_default();
+ assert!(j.0.dist(Vec3::new(0.0, 0.0, G)) < 1e-6 * G, "force {:?}", j.0);
+ assert!(j.1.len() < 1e-6 * G, "torque {:?}", j.1);
+ assert!(angle_of(s).abs() < 1e-12);
+ }
+}
+
+#[test]
+fn large_swing_conserves_energy() {
+ // Released from horizontal: a compound pendulum swinging through ±90°.
+ let model = pendulum(100.0, PI / 2.0, 0.0, 400.0);
+ let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-3, 1e-2)));
+ let e0 = traj.samples.first().map(|s| s.energy.total()).unwrap_or(0.0);
+ let scale = G * 100.0;
+ for s in &traj.samples {
+ assert!((s.energy.total() - e0).abs() < 1e-6 * scale, "t={} drift {}", s.time, s.energy.total() - e0);
+ }
+}
+
+/// Two links hinged about Y, released from horizontal.
+fn double_pendulum() -> Model {
+ let link = |x: f64| Body::at_rest(1.0, Vec3::new(100.0, 900.0, 900.0), Vec3::new(x, 0.0, 0.0));
+ let hinge = |a: Option<usize>, pa: Vec3, b: usize, pb: Vec3| Joint {
+ kind: JointKind::Revolute,
+ a: Marker::new(a, pa, Vec3::Y, Vec3::X),
+ b: Marker::new(Some(b), pb, Vec3::Y, Vec3::X),
+ };
+ Model {
+ bodies: vec![link(50.0), link(150.0)],
+ joints: vec![
+ hinge(None, Vec3::ZERO, 0, Vec3::new(-50.0, 0.0, 0.0)),
+ hinge(Some(0), Vec3::new(50.0, 0.0, 0.0), 1, Vec3::new(-50.0, 0.0, 0.0)),
+ ],
+ forces: vec![],
+ gravity: down(),
+ }
+}
+
+#[test]
+fn double_pendulum_conserves_energy() {
+ let traj = ok(simulate(&double_pendulum(), &Settings::rk4(5.0, 5e-4, 1e-2)));
+ let e0 = traj.samples.first().map(|s| s.energy.total()).unwrap_or(0.0);
+ let scale = 2.0 * G * 150.0;
+ let worst = traj.samples.iter().map(|s| (s.energy.total() - e0).abs()).fold(0.0, f64::max);
+ assert!(worst < 1e-5 * scale, "energy drift {worst} of {scale}");
+ // It did swing hard: the lower link went well past vertical.
+ let lowest = traj.samples.iter().filter_map(|s| s.bodies.get(1)).map(|b| b.position.z).fold(0.0, f64::min);
+ assert!(lowest < -100.0, "{lowest}");
+ assert_eq!(traj.stats.redundant_equations, 0);
+}
+
+#[test]
+fn constraint_drift_stays_bounded_over_long_runs() {
+ // Projection holds the joints to its tolerance (1e-10) however long the run.
+ let traj = ok(simulate(&double_pendulum(), &Settings::rk4(20.0, 1e-3, 0.1)));
+ assert!(traj.stats.max_constraint_error <= 1e-10, "{:?}", traj.stats);
+ // Without projection, Baumgarte terms keep the drift bounded too, if less tightly.
+ let settings = Settings { projection: false, baumgarte: Some((20.0, 20.0)), ..Settings::rk4(20.0, 1e-3, 0.1) };
+ let traj = ok(simulate(&double_pendulum(), &settings));
+ assert!(traj.stats.max_constraint_error < 1e-3, "{:?}", traj.stats);
+}
+
+#[test]
+fn slider_crank_follows_its_closed_form() {
+ let (r, l) = (50.0, 150.0);
+ let theta0 = PI / 3.0;
+ let pin = Vec3::new(r * theta0.cos(), 0.0, -r * theta0.sin());
+ let s0 = r * theta0.cos() + (l * l - (r * theta0.sin()).powi(2)).sqrt();
+ let slide = Vec3::new(s0, 0.0, 0.0);
+ let u = (slide - pin) * (1.0 / l);
+ let mut crank = Body::at_rest(1.0, Vec3::new(100.0, 2000.0, 2000.0), pin * 0.5);
+ crank.orientation = Quat::from_axis_angle(Vec3::Y, theta0);
+ crank.angular_velocity = Vec3::Y * TAU;
+ crank.velocity = (Vec3::Y * TAU).cross(pin * 0.5);
+ let mut rod = Body::at_rest(0.5, Vec3::new(50.0, 1000.0, 1000.0), (pin + slide) * 0.5);
+ rod.orientation = Quat::from_axis_angle(Vec3::Y, (-u.z).atan2(u.x));
+ let slider = Body::at_rest(0.3, Vec3::new(50.0, 50.0, 50.0), slide);
+ let hinge = |a: Option<usize>, pa: Vec3, b: usize, pb: Vec3| Joint {
+ kind: JointKind::Revolute,
+ a: Marker::new(a, pa, Vec3::Y, Vec3::X),
+ b: Marker::new(Some(b), pb, Vec3::Y, Vec3::X),
+ };
+ let model = Model {
+ bodies: vec![crank, rod, slider],
+ joints: vec![
+ hinge(None, Vec3::ZERO, 0, Vec3::new(-r / 2.0, 0.0, 0.0)),
+ hinge(Some(0), Vec3::new(r / 2.0, 0.0, 0.0), 1, Vec3::new(-l / 2.0, 0.0, 0.0)),
+ hinge(Some(1), Vec3::new(l / 2.0, 0.0, 0.0), 2, Vec3::ZERO),
+ Joint { kind: JointKind::Slider, a: Marker::ground(Vec3::ZERO, Vec3::X, Vec3::Y), b: Marker::new(Some(2), Vec3::ZERO, Vec3::X, Vec3::Y) },
+ ],
+ forces: vec![],
+ gravity: Vec3::ZERO,
+ };
+ let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-3, 1e-2)));
+ // A planar linkage built from 3D joints repeats three equations; they are solved, not refused.
+ assert_eq!(traj.stats.redundant_equations, 3);
+ let mut turned = 0.0_f64;
+ let mut last = theta0;
+ for s in &traj.samples {
+ let theta = angle_of(s);
+ let x = s.joints.get(3).and_then(|j| j.values.first()).copied().unwrap_or(f64::NAN);
+ let expect = r * theta.cos() + (l * l - (r * theta.sin()).powi(2)).sqrt();
+ assert!((x - expect).abs() < 1e-6, "t={}: slider {x} vs {expect}", s.time);
+ let step = (theta - last + PI).rem_euclid(TAU) - PI;
+ turned += step;
+ last = theta;
+ }
+ assert!(turned.abs() > TAU, "the crank turned {turned} rad");
+ assert!(traj.stats.max_constraint_error < 1e-9, "{:?}", traj.stats);
+ // No forces: kinetic energy is conserved.
+ let e0 = traj.samples.first().map(|s| s.energy.kinetic).unwrap_or(0.0);
+ assert!(traj.samples.iter().all(|s| (s.energy.kinetic - e0).abs() < 1e-6 * e0), "kinetic energy drifted");
+}
+
+/// A body on a slider along X, pulled toward the origin by a spring of free length 40 mm,
+/// starting 10 mm stretched, with gravity across the slide.
+fn spring_mass(damping: f64) -> Model {
+ Model {
+ bodies: vec![Body::at_rest(2.0, Vec3::new(500.0, 500.0, 500.0), Vec3::new(50.0, 0.0, 0.0))],
+ joints: vec![Joint {
+ kind: JointKind::Slider,
+ a: Marker::ground(Vec3::ZERO, Vec3::X, Vec3::Y),
+ b: Marker::new(Some(0), Vec3::ZERO, Vec3::X, Vec3::Y),
+ }],
+ forces: vec![Force::Spring {
+ a: Anchor { body: None, point: Vec3::ZERO },
+ b: Anchor { body: Some(0), point: Vec3::ZERO },
+ stiffness: 800.0,
+ damping,
+ free_length: 40.0,
+ preload: 0.0,
+ }],
+ gravity: down(),
+ }
+}
+
+#[test]
+fn spring_mass_oscillates_at_its_natural_frequency() {
+ // ω = √(k/m) = 20 rad/s.
+ let traj = ok(simulate(&spring_mass(0.0), &Settings::rk4(1.0, 1e-4, 1e-3)));
+ for s in &traj.samples {
+ let x = s.bodies.first().map(|b| b.position.x).unwrap_or(f64::NAN);
+ let expect = 40.0 + 10.0 * (20.0 * s.time).cos();
+ assert!((x - expect).abs() < 1e-6, "t={}: {x} vs {expect}", s.time);
+ // The slide holds the body up against gravity.
+ let f = s.joints.first().map(|j| j.force).unwrap_or_default();
+ assert!((f.z - 2.0 * G).abs() < 1e-6 * G && f.x.abs() < 1e-6 * G, "reaction {f:?}");
+ }
+ let e0 = traj.samples.first().map(|s| s.energy.total()).unwrap_or(0.0);
+ assert!(traj.samples.iter().all(|s| (s.energy.total() - e0).abs() < 1e-8 * 800.0 * 100.0));
+}
+
+#[test]
+fn spring_damper_decays_as_the_closed_form() {
+ // c = 8 kg/s: ζ = c / (2√(km)) = 0.1.
+ let (wn, zeta) = (20.0_f64, 0.1_f64);
+ let wd = wn * (1.0 - zeta * zeta).sqrt();
+ let traj = ok(simulate(&spring_mass(8.0), &Settings::rk4(1.0, 1e-4, 1e-3)));
+ for s in &traj.samples {
+ let t = s.time;
+ let x = s.bodies.first().map(|b| b.position.x).unwrap_or(f64::NAN);
+ let expect = 40.0 + (-zeta * wn * t).exp() * 10.0 * ((wd * t).cos() + zeta * wn / wd * (wd * t).sin());
+ assert!((x - expect).abs() < 1e-6, "t={t}: {x} vs {expect}");
+ }
+}
+
+#[test]
+fn torque_free_body_keeps_energy_and_momentum() {
+ let mut body = Body::at_rest(1.0, Vec3::new(1000.0, 2000.0, 3000.0), Vec3::ZERO);
+ body.orientation = Quat::from_axis_angle(Vec3::new(1.0, 2.0, 3.0), 0.7);
+ body.angular_velocity = Vec3::new(1.0, 0.3, 5.0);
+ let model = Model { bodies: vec![body], joints: vec![], forces: vec![], gravity: Vec3::ZERO };
+ let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-4, 1e-2)));
+ let momentum = |s: &Sample| {
+ let b = s.bodies.first().copied();
+ b.map_or(Vec3::ZERO, |b| {
+ let r = b.orientation.to_mat3();
+ r.mul(&Mat3::diagonal(Vec3::new(1000.0, 2000.0, 3000.0))).mul(&r.transpose()).mul_vec(b.angular_velocity)
+ })
+ };
+ let (l0, e0) = traj.samples.first().map(|s| (momentum(s), s.energy.kinetic)).unwrap_or_default();
+ let mut tumbled = false;
+ for s in &traj.samples {
+ assert!(momentum(s).dist(l0) < 1e-7 * l0.len(), "t={} momentum {:?} vs {l0:?}", s.time, momentum(s));
+ assert!((s.energy.kinetic - e0).abs() < 1e-7 * e0);
+ let w = s.bodies.first().map(|b| b.angular_velocity).unwrap_or_default();
+ tumbled |= w.dist(Vec3::new(1.0, 0.3, 5.0)) > 0.1;
+ }
+ assert!(tumbled, "the angular velocity should precess");
+}
+
+#[test]
+fn spherical_pendulum_keeps_energy_and_vertical_momentum() {
+ // A body on a ball joint, tilted and pushed sideways: it swings round the vertical.
+ let com = Vec3::new(60.0, 0.0, -80.0);
+ let mut body = Body::at_rest(1.5, Vec3::new(300.0, 500.0, 700.0), com);
+ body.velocity = Vec3::new(0.0, 400.0, 0.0);
+ let model = Model {
+ bodies: vec![body],
+ joints: vec![Joint {
+ kind: JointKind::Ball,
+ a: Marker::ground(Vec3::ZERO, Vec3::Z, Vec3::X),
+ b: Marker::new(Some(0), -com, Vec3::Z, Vec3::X),
+ }],
+ forces: vec![],
+ gravity: down(),
+ };
+ let traj = ok(simulate(&model, &Settings::rk4(3.0, 5e-4, 1e-2)));
+ let lz = |s: &Sample| {
+ s.bodies.first().map_or(0.0, |b| {
+ let r = b.orientation.to_mat3();
+ let iw = r.mul(&Mat3::diagonal(Vec3::new(300.0, 500.0, 700.0))).mul(&r.transpose());
+ (b.position.cross(b.velocity * 1.5) + iw.mul_vec(b.angular_velocity)).z
+ })
+ };
+ let (l0, e0) = traj.samples.first().map(|s| (lz(s), s.energy.total())).unwrap_or_default();
+ assert!(l0.abs() > 1e3);
+ for s in &traj.samples {
+ assert!((lz(s) - l0).abs() < 1e-6 * l0.abs(), "t={}: Lz {} vs {l0}", s.time, lz(s));
+ assert!((s.energy.total() - e0).abs() < 1e-6 * G * 100.0, "t={}: energy drift {}", s.time, s.energy.total() - e0);
+ // The joint holds the body's pivot at the origin.
+ let pivot = s.bodies.first().map(|b| b.position + b.orientation.rotate(-com)).unwrap_or_default();
+ assert!(pivot.len() < 1e-9, "{pivot:?}");
+ }
+}
+
+/// Every joint kind between two moving bodies and between the ground and a body: the
+/// Jacobian and `J̇u` agree with finite differences of `Φ` along the motion.
+#[test]
+fn jacobians_match_finite_differences() {
+ let kinds = [JointKind::Rigid, JointKind::Revolute, JointKind::Slider, JointKind::Cylindrical, JointKind::Planar, JointKind::Ball];
+ let mut a = Body::at_rest(1.0, Vec3::new(1.0, 2.0, 3.0), Vec3::new(10.0, -20.0, 5.0));
+ a.orientation = Quat::from_axis_angle(Vec3::new(0.3, -1.0, 0.4), 0.9);
+ a.velocity = Vec3::new(3.0, -1.0, 2.0);
+ a.angular_velocity = Vec3::new(0.7, -0.4, 1.1);
+ let mut b = Body::at_rest(2.0, Vec3::new(3.0, 2.0, 1.0), Vec3::new(-5.0, 15.0, 30.0));
+ b.orientation = Quat::from_axis_angle(Vec3::new(1.0, 0.2, -0.5), -1.3);
+ b.velocity = Vec3::new(-2.0, 4.0, 1.0);
+ b.angular_velocity = Vec3::new(-0.5, 1.3, 0.2);
+ for kind in kinds {
+ for a_body in [Some(0), None] {
+ let joint = Joint {
+ kind,
+ a: Marker::new(a_body, Vec3::new(4.0, -3.0, 7.0), Vec3::new(0.2, 0.5, 1.0), Vec3::new(1.0, 0.0, 0.3)),
+ b: Marker::new(Some(1), Vec3::new(-6.0, 2.0, 1.0), Vec3::new(-0.4, 1.0, 0.1), Vec3::new(0.0, 0.3, 1.0)),
+ };
+ let model = Model { bodies: vec![a.clone(), b.clone()], joints: vec![joint], forces: vec![], gravity: Vec3::ZERO };
+ let sys = ok(System::new(&model, None));
+ let y = ok(System::initial_state(&model));
+ let h = 1e-4;
+ let (now, plus, minus) = (
+ testing::rows_at(&sys, &y),
+ testing::rows_at(&sys, &testing::drift(&sys, &y, h)),
+ testing::rows_at(&sys, &testing::drift(&sys, &y, -h)),
+ );
+ assert_eq!(now.len(), kind.equations());
+ for ((phi, ju, jdu), ((p, _, _), (m, _, _))) in now.iter().zip(plus.iter().zip(&minus)) {
+ let rate = (p - m) / (2.0 * h);
+ let accel = (p - 2.0 * phi + m) / (h * h);
+ assert!((rate - ju).abs() < 1e-5 * (1.0 + ju.abs()), "{kind:?} {a_body:?}: J u {ju} vs {rate}");
+ assert!((accel - jdu).abs() < 1e-3 * (1.0 + jdu.abs()), "{kind:?} {a_body:?}: J̇ u {jdu} vs {accel}");
+ }
+ }
+ }
+}
+
+#[test]
+fn joint_values_read_the_relative_motion() {
+ // A body on each joint kind, displaced along its free directions, reads them back.
+ let place = |kind: JointKind, offset: Vec3, turn: f64| {
+ let mut body = Body::at_rest(1.0, Vec3::new(1.0, 1.0, 1.0), offset);
+ body.orientation = Quat::from_axis_angle(Vec3::Z, turn);
+ let model = Model {
+ bodies: vec![body],
+ joints: vec![Joint { kind, a: Marker::ground(Vec3::ZERO, Vec3::Z, Vec3::X), b: Marker::new(Some(0), Vec3::ZERO, Vec3::Z, Vec3::X) }],
+ forces: vec![],
+ gravity: Vec3::ZERO,
+ };
+ let traj = ok(simulate(&model, &Settings::rk4(0.01, 0.01, 0.01)));
+ traj.samples.first().and_then(|s| s.joints.first()).map(|j| j.values.clone()).unwrap_or_default()
+ };
+ let close = |a: &[f64], b: &[f64]| a.len() == b.len() && a.iter().zip(b).all(|(x, y)| (x - y).abs() < 1e-9);
+ assert!(close(&place(JointKind::Revolute, Vec3::ZERO, 0.4), &[0.4]));
+ assert!(close(&place(JointKind::Slider, Vec3::new(0.0, 0.0, 12.0), 0.0), &[12.0]));
+ assert!(close(&place(JointKind::Cylindrical, Vec3::new(0.0, 0.0, -3.0), -1.0), &[-1.0, -3.0]));
+ assert!(close(&place(JointKind::Planar, Vec3::new(5.0, -7.0, 0.0), 2.0), &[5.0, -7.0, 2.0]));
+ assert!(close(&place(JointKind::Ball, Vec3::ZERO, 0.5), &[0.0, 0.0, 0.5]));
+ assert!(close(&place(JointKind::Rigid, Vec3::ZERO, 0.0), &[]));
+}
+
+#[test]
+fn inconsistent_start_is_projected_onto_the_joints() {
+ // The pendulum's body starts 2 mm off its hinge and moving across it.
+ let mut model = pendulum(100.0, 0.3, 0.0, 40.0);
+ if let Some(b) = model.bodies.first_mut() {
+ b.position += Vec3::new(1.0, 2.0, -1.0);
+ b.velocity = Vec3::new(0.0, 50.0, 0.0);
+ }
+ let traj = ok(simulate(&model, &Settings::rk4(0.5, 1e-3, 0.05)));
+ assert!(traj.stats.max_constraint_error < 1e-9, "{:?}", traj.stats);
+ let y = traj.samples.first().and_then(|s| s.bodies.first()).map(|b| b.velocity.y).unwrap_or(f64::NAN);
+ assert!(y.abs() < 1e-9, "velocity along the hinge is removed: {y}");
+}
+
+#[test]
+fn hostile_input_is_an_error_not_a_panic() {
+ let good = pendulum(100.0, 0.2, 0.0, 40.0);
+ let settings = Settings::rk4(0.1, 1e-3, 1e-2);
+ let mut cases: Vec<(Model, Settings)> = Vec::new();
+ let mut with = |f: &dyn Fn(&mut Model)| {
+ let mut m = good.clone();
+ f(&mut m);
+ cases.push((m, settings));
+ };
+ with(&|m| m.bodies.clear());
+ with(&|m| m.bodies.iter_mut().for_each(|b| b.mass = f64::NAN));
+ with(&|m| m.bodies.iter_mut().for_each(|b| b.mass = 0.0));
+ with(&|m| m.bodies.iter_mut().for_each(|b| b.mass = -1.0));
+ with(&|m| m.bodies.iter_mut().for_each(|b| b.inertia = Mat3::diagonal(Vec3::new(1.0, -1.0, 1.0))));
+ with(&|m| m.bodies.iter_mut().for_each(|b| b.inertia = Mat3([[1.0, 5.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])));
+ with(&|m| m.bodies.iter_mut().for_each(|b| b.inertia = Mat3::ZERO));
+ with(&|m| m.bodies.iter_mut().for_each(|b| b.position.x = f64::INFINITY));
+ with(&|m| m.bodies.iter_mut().for_each(|b| b.orientation = Quat { w: 0.0, x: 0.0, y: 0.0, z: 0.0 }));
+ with(&|m| m.bodies.iter_mut().for_each(|b| b.velocity.z = 1e300));
+ with(&|m| m.gravity = Vec3::new(f64::NAN, 0.0, 0.0));
+ with(&|m| m.joints.iter_mut().for_each(|j| j.b.body = Some(7)));
+ with(&|m| m.joints.iter_mut().for_each(|j| j.a.body = j.b.body));
+ with(&|m| m.joints.iter_mut().for_each(|j| j.a.z = Vec3::ZERO));
+ with(&|m| m.joints.iter_mut().for_each(|j| j.b.x = j.b.z));
+ with(&|m| m.forces.push(Force::Load { body: 3, point: Vec3::ZERO, force: Vec3::X, torque: Vec3::ZERO }));
+ with(&|m| {
+ m.forces.push(Force::Spring {
+ a: Anchor { body: None, point: Vec3::ZERO },
+ b: Anchor { body: Some(0), point: Vec3::ZERO },
+ stiffness: -1.0,
+ damping: 0.0,
+ free_length: 0.0,
+ preload: 0.0,
+ })
+ });
+ with(&|m| {
+ m.forces.push(Force::Spring {
+ a: Anchor { body: Some(9), point: Vec3::ZERO },
+ b: Anchor { body: Some(0), point: Vec3::ZERO },
+ stiffness: 1.0,
+ damping: 0.0,
+ free_length: 0.0,
+ preload: 0.0,
+ })
+ });
+ with(&|m| m.bodies = vec![Body::at_rest(1.0, Vec3::new(1.0, 1.0, 1.0), Vec3::ZERO); MAX_BODIES + 1]);
+ with(&|m| {
+ m.joints = vec![
+ m.joints.first().copied().unwrap_or(Joint {
+ kind: JointKind::Ball,
+ a: Marker::ground(Vec3::ZERO, Vec3::Z, Vec3::X),
+ b: Marker::new(Some(0), Vec3::ZERO, Vec3::Z, Vec3::X)
+ });
+ MAX_JOINTS + 1
+ ]
+ });
+ let bad_settings = [
+ Settings::rk4(0.0, 1e-3, 1e-2),
+ Settings::rk4(-1.0, 1e-3, 1e-2),
+ Settings::rk4(f64::NAN, 1e-3, 1e-2),
+ Settings::rk4(1.0, 0.0, 1e-2),
+ Settings::rk4(1.0, f64::INFINITY, 1e-2),
+ Settings::rk4(1.0, 1e-3, 0.0),
+ Settings::rk4(1e6, 1e-6, 1e-2),
+ Settings::rk4(1e3, 1e-3, 1e-6),
+ Settings { projection_tol: -1.0, ..settings },
+ Settings { baumgarte: Some((f64::NAN, 1.0)), ..settings },
+ Settings { integrator: Integrator::Adaptive { rel_tol: 1e-6, abs_tol: 1e-6, min_step: 1.0, max_step: 0.1 }, ..settings },
+ Settings { integrator: Integrator::Adaptive { rel_tol: 0.0, abs_tol: 1e-6, min_step: 1e-9, max_step: 0.1 }, ..settings },
+ ];
+ cases.extend(bad_settings.into_iter().map(|s| (good.clone(), s)));
+ for (i, (m, s)) in cases.iter().enumerate() {
+ assert!(simulate(m, s).is_err(), "case {i} should be refused");
+ }
+ // Inputs that are valid but extreme stop with an error or finish; they never panic.
+ let stiff = Model {
+ forces: vec![Force::Spring {
+ a: Anchor { body: None, point: Vec3::ZERO },
+ b: Anchor { body: Some(0), point: Vec3::ZERO },
+ stiffness: 1e11,
+ damping: 0.0,
+ free_length: 0.0,
+ preload: 0.0,
+ }],
+ ..good.clone()
+ };
+ let _ = simulate(&stiff, &Settings::rk4(0.1, 1e-2, 1e-2));
+ let tight = Settings { integrator: Integrator::Adaptive { rel_tol: 1e-14, abs_tol: 1e-14, min_step: 1e-3, max_step: 1e-3 }, ..settings };
+ let _ = simulate(&stiff, &tight);
+}
+
+#[test]
+fn models_round_trip_through_json() {
+ let mut model = spring_mass(1.5);
+ model.forces.push(Force::Load { body: 0, point: Vec3::new(1.0, 0.0, 0.0), force: Vec3::new(0.0, 5.0, 0.0), torque: Vec3::ZERO });
+ let text = match serde_json::to_string(&model) {
+ Ok(t) => t,
+ Err(e) => panic!("{e}"),
+ };
+ let back: Model = match serde_json::from_str(&text) {
+ Ok(m) => m,
+ Err(e) => panic!("{e}"),
+ };
+ assert_eq!(back, model);
+ let settings: Settings = match serde_json::from_str(r#"{"duration":1.0,"output_interval":0.1,"integrator":{"type":"rk4","step":0.001}}"#) {
+ Ok(s) => s,
+ Err(e) => panic!("{e}"),
+ };
+ assert!(settings.projection && settings.validate().is_ok());
+}
diff --git a/docs/architecture.md b/docs/architecture.md
index d0e93cc623..b83822d916 100644
--- a/docs/architecture.md
+++ b/docs/architecture.md
@@ -4,7 +4,7 @@
| Layer | Crates | Role |
|---|---|---|
| L0 | `geom` | vectors, planes, profiles, meshes and their measures |
-| L1 | `sketch`, `kernel`, `render` | constraint solver and profiles; the B-rep kernel boundary (truck); view math and the headless scene description |
+| L1 | `sketch`, `kernel`, `render`, `dynamics` | constraint solver and profiles; the B-rep kernel boundary (truck); view math and the headless scene description; multibody dynamics (rigid bodies, joints, forces, time integration) |
| L2 | `doc` | parameters, expressions, the feature timeline and its incremental evaluation |
| L3 | `io` | design files, STL/OBJ/STEP/3MF export, STEP and 3MF/STL/OBJ/glTF import features |
| L4 | `engine` | the session and the command registry (everything is a command) |
diff --git a/xtask/src/layers.rs b/xtask/src/layers.rs
index 94866b91ac..48fc4b2a31 100644
--- a/xtask/src/layers.rs
+++ b/xtask/src/layers.rs
@@ -51,6 +51,8 @@ pub const TABLE: &[(&str, Class)] = &[
("step", Class::Layer(1)),
("kernel", Class::Layer(1)),
("render", Class::Layer(1)),
+ // multibody dynamics (rigid bodies, joints, forces, time integration; geom only)
+ ("dynamics", Class::Layer(1)),
("doc", Class::Layer(2)),
("io", Class::Layer(3)),
("engine", Class::Layer(4)),