Skip to content

SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.

6.1 KB
Raw Blame
//! 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;