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

1. Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer.
2. 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.