Files
solidcraft/crates/dynamics/src/lib.rs
T
774c45a512 Dynamics: rigid-body solver core with joints, gravity and springs (#9719) (#9721)
* claim: #9719

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

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

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

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

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

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

---------

Co-authored-by: Claude Opus 5.5 <noreply@anthropic.com>
Co-authored-by: bherbruck <brennen.herbruck@gmail.com>
2026-11-04 21:27:17 +00:00

137 lines
5.2 KiB
Rust
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
//! 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;