From 774c45a512638136dea23513cb4a0bcf6bf8abaf Mon Sep 17 00:00:00 2001 From: PaddleStroke Date: Wed, 4 Nov 2026 21:27:17 +0000 Subject: [PATCH] Dynamics: rigid-body solver core with joints, gravity and springs (#9719) (#9721) * claim: #9719 Co-Authored-By: Claude Opus 5.5 * 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 * Dynamics: take Vec3 from the record crate after the layer split, refresh the lock file Co-Authored-By: Claude Opus 5.5 --------- Co-authored-by: Claude Opus 5.5 Co-authored-by: bherbruck --- CONTRIBUTING.md | 2 + Cargo.lock | 11 + Cargo.toml | 1 + crates/dynamics/Cargo.toml | 18 + crates/dynamics/src/integrate.rs | 203 +++++++++++ crates/dynamics/src/lib.rs | 136 +++++++ crates/dynamics/src/linalg.rs | 107 ++++++ crates/dynamics/src/math.rs | 203 +++++++++++ crates/dynamics/src/model.rs | 385 ++++++++++++++++++++ crates/dynamics/src/system.rs | 594 +++++++++++++++++++++++++++++++ crates/dynamics/src/tests.rs | 540 ++++++++++++++++++++++++++++ docs/architecture.md | 2 +- xtask/src/layers.rs | 2 + 13 files changed, 2203 insertions(+), 1 deletion(-) create mode 100644 crates/dynamics/Cargo.toml create mode 100644 crates/dynamics/src/integrate.rs create mode 100644 crates/dynamics/src/lib.rs create mode 100644 crates/dynamics/src/linalg.rs create mode 100644 crates/dynamics/src/math.rs create mode 100644 crates/dynamics/src/model.rs create mode 100644 crates/dynamics/src/system.rs create mode 100644 crates/dynamics/src/tests.rs diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index acb4e8b11..f3cc9a22a 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 321a8fbfc..5a6aa4838 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 70db751cf..203dc8c0e 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 000000000..bfb82432f --- /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 000000000..f32ed3ebf --- /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], weights: &[f64], h: f64) -> Vec { + 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> { + 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> { + 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)> { + let mut ks: Vec> = 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, 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 { + 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 { + 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 000000000..24f21c4d6 --- /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 = std::result::Result; + +/// 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, + /// Their rates (rad/s, mm/s); a ball joint gives the relative angular velocity in A's frame. + pub rates: Vec, + /// 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, + pub joints: Vec, + 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, + 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 000000000..c0ccc159e --- /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, +} + +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, 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 = (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 000000000..8eeebc593 --- /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 { + 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 { + 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 { + 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 000000000..e4af1e1b6 --- /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, + pub origin: Vec3, + pub z: Vec3, + pub x: Vec3, +} + +impl Marker { + pub fn new(body: Option, 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, + 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, + #[serde(default)] + pub joints: Vec, + #[serde(default)] + pub forces: Vec, + /// 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 { + 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| 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 000000000..b11824265 --- /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, + 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, + /// 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, + b: Option, + pub(crate) phi: f64, + /// `J̇ u`: the equation's second derivative at zero accelerations. + jdot_u: f64, +} + +impl Row { + fn blocks(&self) -> impl Iterator { + 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, + joints: Vec, + forces: Vec, + 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, + /// One multiplier per constraint row, in joint order. + lambda: Vec, + rows: Vec, + /// 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 { + 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 { + 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::>>()?; + let joints = model + .joints + .iter() + .map(|j| Ok(JointConst { kind: j.kind, a: marker_const(&j.a)?, b: marker_const(&j.b)? })) + .collect::>>()?; + 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> { + 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 { + (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) -> 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 { + 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>) { + let w: Vec> = 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, 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 { + 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 = 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 { + 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 = 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 = 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 { + (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 { + 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) { + let [ax, ay, az] = a.axes; + let [bx, by, bz] = b.axes; + let point = |rows: &mut Vec| 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, Vec) { + 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 { + 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 000000000..877568fd8 --- /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(r: Result) -> 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 { + 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, 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, 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 d0e93cc62..b83822d91 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 94866b91a..48fc4b2a3 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)),