Crate spatial_math

Expand description

§Spatial Math Library

A Rust implementation of spatial vector mathematics based on Roy Featherstone’s rigid body dynamics algorithms. This library provides efficient, type-safe operations for articulated body simulation and robotics applications.

§Overview

Spatial mathematics treats linear and angular quantities as unified 6-dimensional vectors, providing elegant and computationally efficient formulations for rigid body dynamics. The approach eliminates many of the complexities associated with traditional 3D vector calculations while maintaining physical correctness.

§Key Concepts

Spatial Vectors: 6D vectors combining linear and angular components
Motion Vectors: Spatial velocity (ω, v) - angular velocity ω and linear velocity v
Force Vectors: Spatial force (n, f) - moment/torque n and force f
Spatial Transforms: 6×6 matrices for coordinate frame transformations
Spatial Inertia: 6×6 inertia matrices for rigid and articulated bodies

§Mathematical Foundation

Based on Featherstone’s spatial vector algebra from “Rigid Body Dynamics Algorithms” (2008), this library implements:

Spatial vector arithmetic operations
Plücker coordinate transforms
Articulated body inertia algorithms
Efficient 6×6 matrix operations with symmetric storage

§Features

Zero-cost abstractions: All operations compile to efficient native code
Type safety: Distinct types for motion vs force vectors prevent misuse
Memory efficient: Symmetric matrices stored in compact 6-element arrays
Comprehensive: Complete spatial vector algebra implementation
Configurable precision: Support for both f32 and f64 floating point
Serde support: Optional serialization/deserialization

§Quick Start

use spatial_math::*;

// Create spatial vectors
let velocity = SpatialMotionVector::from_array([1.0, 0.0, 0.0,  // angular velocity ω
                                             0.0, 1.0, 0.0]); // linear velocity v
let force = SpatialForceVector::from_array([0.0, 0.0, 1.0,   // moment n
                                         1.0, 0.0, 0.0]);   // force f

// Create a transform
let transform = PluckerTransform::identity();

// Spatial vector operations
let power = velocity.dot(&force);  // Power = v ⋅ f
let transformed_velocity = transform * velocity;

// Create rigid body inertia
let inertia = RigidBodyInertia::new(
    2.0,                           // mass
    vec3(0.0, 0.0, 1.0),          // center of mass
    SymmetricMat3::from_array([1.0, 0.0, 0.0, 1.0, 0.0, 1.0]) // inertia at CoM
);

// Calculate spatial force from motion
let spatial_force = inertia * velocity;

§Library Structure

§Core Types

SpatialMotionVector: Spatial velocity (6×1 vector [ω, v])
SpatialForceVector: Spatial force (6×1 vector [n, f])
RigidBodyInertia: Rigid body spatial inertia (6×6 matrix)
ArticulatedBodyInertia: Articulated body spatial inertia (6×6 matrix)
PluckerTransform: Coordinate frame transformation (6×6 matrix)
Pose: Position and orientation in 3D space

§Utility Types

SymmetricMat3: 3×3 symmetric matrix with compact storage
Vec3, Mat3: 3D vector and matrix types
UnitQuat: Unit quaternion for rotations

§Extensions

Vec3Ext: Cross-product matrix conversion
MatrixExt: NaN detection and matrix utilities
ViewMatrix: Memory-safe matrix views

§Mathematical Conventions

§Spatial Vector Layout

SpatialMotionVector = [ωx, ωy, ωz, vx, vy, vz]ᵀ
SpatialForceVector  = [nx, ny, nz, fx, fy, fz]ᵀ

§Transform Matrix

X = | E   0 |
    | -E×r E |

where E is the 3×3 rotation matrix and r is the translation vector.

§Inertia Matrix (Rigid Body)

I = | I_3×3   m×r |
    | r×m     m   |

where I_3×3 is the rotational inertia, m is mass, and r is center of mass position.

§Performance Characteristics

Memory layout: Optimized for cache-friendly access patterns
Symmetric storage: 3×3 symmetric matrices use 6 elements instead of 9
Zero allocations: All operations are stack-allocated
SIMD optimization: Uses nalgebra’s vectorized operations when available

§Use Cases

§Robotics

Forward/inverse dynamics calculations
Robot arm kinematics and dynamics
Multi-body system simulation
Control algorithm implementation

§Physics Simulation

Articulated body dynamics
Constraint force calculation
Collision response with rotational effects
Multi-body integration schemes

§Computer Graphics

Rigid body animation
Physics-based character control
Articulated figure dynamics

§References

Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer.
Featherstone, R. (2014). Spatial Vector Algebra and its Application in Robot Dynamics. Springer.

§Features

f64: Enable double precision floating point (default: f32)
serde: Enable serialization/deserialization support
approx: Enable approximate equality for testing

§License

This project is licensed under the terms specified in the LICENSE file.

§Contributing

Contributions are welcome! Please ensure all code follows the existing style conventions and includes appropriate documentation for mathematical operations.

Re-exports§

pub extern crate nalgebra as na;

Modules§

exts: Extension traits for spatial algebra operations.

Structs§

ArticulatedBodyInertia: Articulated body spatial inertia matrix for complex multi-body systems.
PluckerTransform: A spatial transform using Plücker coordinates.
PlukerRotation: A Plücker rotation representing a 3D rotation from reference to local frame.
Pose: A 6DOF pose represented by rotation and translation.
RigidBodyInertia: Rigid body spatial inertia matrix implementing Featherstone’s spatial inertia formulation.
SpatialVector: A 6D spatial vector representing either motion or force.
SymmetricMat3: A memory-efficient 3×3 symmetric matrix for spatial algebra operations.

Constants§

FRAC_PI_2
PI
VEC3_ZERO

Traits§

SpatialVec: Trait defining the core interface for spatial vectors in Featherstone’s spatial algebra.

Functions§

unit_quat
unit_quat_unchecked
unit_vec3
unit_vec3_unchecked
vec3
vec4
vec6

Type Aliases§

DMatrix
DMatrixView
DMatrixViewMut
DVector
DVectorView
DVectorViewMut
Mat3
Matrix6xN: Dynamic matrix that has 6 rows and N columns.
MatrixView
MatrixViewMut
Quat
Real
SMatrix
SVector: Static alloc vector type alias.
SpatialForceVector
SpatialMotionVector
UnitQuat
UnitVec3
Vec3
Vec4
Vec6