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.mdindex 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. Patcheddiff --git a/Cargo.lock b/Cargo.lockindex 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.tomlindex 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.tomlnew file mode 100644index 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.rsnew file mode 100644index 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.rsnew file mode 100644index 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.rsnew file mode 100644index 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.rsnew file mode 100644index 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.rsnew file mode 100644index 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.rsnew file mode 100644index 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.rsnew file mode 100644index 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.mdindex 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.rsindex 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)),