SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.
//! 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), force elements
//! (point and joint spring-dampers, motors along joint values, loads), drives that make a joint
//! value follow a [`Profile`] in time, and motion links (gears, racks, screws) between values;
//! [`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, Coord, Drive, Force, Integrator, Joint, JointKind, Link, MAX_BODIES, MAX_CONSTRAINT_ROWS, MAX_FORCES, MAX_JOINTS, MAX_MAGNITUDE,
MAX_PROFILE_POINTS, MAX_PROJECTION_ITERATIONS, MAX_SAMPLES, MAX_STEPS, Marker, Model, Profile, 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, point and joint ones (preload included).
pub spring: f64,
/// Work put in since the start by motors, drives and loads.
#[serde(default)]
pub input: f64,
/// Energy taken out since the start by dampers.
#[serde(default)]
pub dissipated: f64,
}
impl Energy {
/// Kinetic plus potential energy.
pub fn total(&self) -> f64 {
self.kinetic + self.gravity + self.spring
}
/// `total + dissipated − input`: constant along a run up to the integration error.
pub fn balance(&self) -> f64 {
self.total() + self.dissipated - self.input
}
}
/// 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>,
/// What each drive applies along its joint value to make it follow (mN·mm for an angle, mN
/// for a distance), in [`Model::drives`] order.
#[serde(default, skip_serializing_if = "Vec::is_empty")]
pub drives: Vec<f64>,
pub energy: Energy,
/// The largest constraint 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;