* 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:
@@ -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
@@ -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"
|
||||||
|
|||||||
@@ -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" }
|
||||||
|
|||||||
@@ -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 }
|
||||||
@@ -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 })
|
||||||
|
}
|
||||||
@@ -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;
|
||||||
@@ -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))
|
||||||
|
}
|
||||||
@@ -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)
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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(())
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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());
|
||||||
|
}
|
||||||
@@ -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) |
|
||||||
|
|||||||
@@ -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)),
|
||||||
|
|||||||
Reference in New Issue
Block a user