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

* claim: #9719

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

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

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

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

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

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

---------

Co-authored-by: Claude Opus 5.5 <noreply@anthropic.com>
Co-authored-by: bherbruck <brennen.herbruck@gmail.com>
This commit is contained in:
PaddleStroke
2026-11-04 21:27:17 +00:00
committed by GitHub
parent 315d8cfeb7
commit 774c45a512
13 changed files with 2203 additions and 1 deletions
+2
View File
@@ -88,6 +88,8 @@ The [architecture page](docs/architecture.md) has the layer table. The short ver
- `crates/geom`: vectors, planes, meshes, measures. - `crates/geom`: vectors, planes, meshes, measures.
- `crates/sketch`: our own constraint solver, plus profile finding. - `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), - `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, 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 Split Face, STEP/IGES read and write, and face provenance are our own code here. Patched
Generated
+11
View File
@@ -4153,6 +4153,17 @@ dependencies = [
"web-time", "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]] [[package]]
name = "solidcraft-engine" name = "solidcraft-engine"
version = "0.40.0-rc.2" version = "0.40.0-rc.2"
+1
View File
@@ -22,6 +22,7 @@ solidcraft-kernel = { path = "crates/kernel" }
solidcraft-smlib = { path = "crates/smlib" } solidcraft-smlib = { path = "crates/smlib" }
solidcraft-step = { path = "crates/step" } solidcraft-step = { path = "crates/step" }
solidcraft-render = { path = "crates/render" } solidcraft-render = { path = "crates/render" }
solidcraft-dynamics = { path = "crates/dynamics" }
solidcraft-gpu = { path = "crates/gpu" } solidcraft-gpu = { path = "crates/gpu" }
solidcraft-doc = { path = "crates/doc" } solidcraft-doc = { path = "crates/doc" }
solidcraft-io = { path = "crates/io" } solidcraft-io = { path = "crates/io" }
+18
View File
@@ -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 }
+203
View File
@@ -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 })
}
+136
View File
@@ -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;
+107
View File
@@ -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))
}
+203
View File
@@ -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)
}
}
+385
View File
@@ -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(())
}
}
+594
View File
@@ -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
}
}
+540
View File
@@ -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());
}
+1 -1
View File
@@ -4,7 +4,7 @@
| Layer | Crates | Role | | Layer | Crates | Role |
|---|---|---| |---|---|---|
| L0 | `geom` | vectors, planes, profiles, meshes and their measures | | 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 | | 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 | | 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) | | L4 | `engine` | the session and the command registry (everything is a command) |
+2
View File
@@ -51,6 +51,8 @@ pub const TABLE: &[(&str, Class)] = &[
("step", Class::Layer(1)), ("step", Class::Layer(1)),
("kernel", Class::Layer(1)), ("kernel", Class::Layer(1)),
("render", 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)), ("doc", Class::Layer(2)),
("io", Class::Layer(3)), ("io", Class::Layer(3)),
("engine", Class::Layer(4)), ("engine", Class::Layer(4)),