phyz_math/
lib.rs

1//! Spatial algebra and math primitives for phyz physics engine.
2//!
3//! Implements 6D spatial vectors, Plucker transforms, and spatial inertia
4//! following Featherstone's conventions.
5
6#![warn(missing_docs)]
7
8// Compile the crate README's Rust blocks as doc-tests so the documented API
9// cannot drift from the real one. `cfg(doctest)` keeps it out of rendered docs.
10#[cfg(doctest)]
11#[doc = include_str!("../README.md")]
12pub struct ReadmeDocTests;
13
14pub mod fp;
15pub mod spatial;
16
17pub use spatial::{SpatialInertia, SpatialMat, SpatialTransform, SpatialTransformExt, SpatialVec};
18
19/// Unit quaternion for 3D rotations (scalar `w`, vector `v`).
20pub type Quat = tang::Quat<f64>;
21
22/// 3D vector alias.
23pub type Vec3 = tang::Vec3<f64>;
24/// 3x3 matrix alias.
25pub type Mat3 = tang::Mat3<f64>;
26/// 4x4 matrix alias.
27pub type Mat4 = tang::Mat4<f64>;
28/// Dynamic vector.
29pub type DVec = tang_la::DVec<f64>;
30/// Dynamic matrix.
31pub type DMat = tang_la::DMat<f64>;
32
33pub use tang::{GRAVITY, skew};
34
35/// Exponential map `so(3) → SO(3)`, computed through [`fp`].
36///
37/// Numerically identical to [`tang::Quat::exp`] except that the `sin`/`cos`
38/// come from [`fp::sin_cos`] rather than the platform's libm. Every rotational
39/// configuration update in the engine goes through this function — a free
40/// joint touches it twice per step — so it is the single highest-traffic
41/// transcendental site in a rollout, and the one that most needs to return the
42/// same bits on every machine. See `docs/determinism.md`.
43pub fn quat_exp(omega: &Vec3) -> Quat {
44    let angle = omega.norm();
45    // Same small-angle branch as tang: below one epsilon the series is the
46    // identity plus half the rotation vector, and dividing by `angle` would be
47    // the thing that loses precision rather than the thing that gains it.
48    if angle < f64::EPSILON {
49        return Quat {
50            w: 1.0,
51            v: *omega * 0.5,
52        };
53    }
54    let (s, c) = fp::sin_cos(angle * 0.5);
55    Quat {
56        w: c,
57        v: *omega * (s / angle),
58    }
59}
60
61/// Logarithmic map `SO(3) → so(3)`, computed through [`fp`].
62///
63/// The inverse of [`quat_exp`], and likewise identical to [`tang::Quat::log`]
64/// apart from routing its `atan2` through [`fp::atan2`].
65pub fn quat_log(q: &Quat) -> Vec3 {
66    let norm_v = q.v.norm();
67    if norm_v < f64::EPSILON {
68        return q.v * 2.0;
69    }
70    let angle = 2.0 * fp::atan2(norm_v, q.w);
71    q.v * (angle / norm_v)
72}
73
74#[cfg(test)]
75mod quat_map_tests {
76    use super::*;
77
78    #[test]
79    fn matches_tang_to_a_few_ulp_and_round_trips() {
80        for w in [
81            Vec3::new(0.0, 0.0, 0.0),
82            Vec3::new(1e-18, 0.0, 0.0),
83            Vec3::new(0.3, -0.7, 1.1),
84            Vec3::new(0.0, 0.0, 3.0),
85            Vec3::new(-2.0, 0.5, 0.25),
86        ] {
87            let ours = quat_exp(&w);
88            let theirs = Quat::exp(&w);
89            let tol = 8.0 * f64::EPSILON;
90            assert!((ours.w - theirs.w).abs() <= tol, "exp w for {w:?}");
91            assert!((ours.v - theirs.v).norm() <= tol, "exp v for {w:?}");
92
93            let back = quat_log(&ours);
94            assert!((back - w).norm() <= 1e-12, "round trip for {w:?}");
95            assert!(
96                (quat_log(&theirs) - theirs.log()).norm() <= tol,
97                "log {w:?}"
98            );
99        }
100    }
101}
phyz_math/lib.rs

phyz_math/
lib.rs
