multicalc 0.10.0

Math for real-time embedded systems, in stable no_std Rust: state estimation, control, kinematics, Lie groups, autodiff, and linear algebra — from 64-bit servers to bare-metal microcontrollers
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73

//! Spatial math: rotations, Lie groups, and spatial-algebra types.
//!
//! - [`Quaternion`] — unit-quaternion rotations.
//! - [`SO2`] / [`SO3`] / [`SE2`] / [`SE3`] — 2D/3D rotation and rigid-transform Lie groups.
//! - [`Twist`] / [`Wrench`] — spatial velocity and force in `[v; ω]` / `[force; torque]` ordering.
//! - [`SpatialInertia`] — a body's mass, centre of mass, and rotational inertia.
//! - [`FreeJointState`] — the pose and velocity of a body free to move in all six directions.

use crate::scalar::Numeric;

mod free_joint;
mod inertia;
mod lie;
mod quaternion;
mod twist;
mod wrench;

pub use free_joint::FreeJointState;
pub use inertia::SpatialInertia;
pub use lie::{SE2, SE3, SO2, SO3};
pub use quaternion::Quaternion;
pub use twist::Twist;
pub use wrench::Wrench;

/// Angle threshold below which trig ratios switch to their Taylor series.
/// Also used for eps-scale proximity checks (e.g. Euler gimbal lock vs ±1).
/// Scaled as `30 · EPSILON` so each scalar type gets a type-appropriate cutoff.
#[inline]
#[must_use]
pub(crate) fn small_angle<T: Numeric>() -> T {
    T::EPSILON_X30
}

/// Squared small-angle threshold, `(30 · EPSILON)²`, for branches on θ² / ‖v‖² before
/// `sqrt`.
#[inline]
#[must_use]
pub(crate) fn small_angle_sq<T: Numeric>() -> T {
    small_angle::<T>() * small_angle::<T>()
}

/// Small-angle thresholds for left jacobian so3
/// t1 = (360*epsilon)^(1/6)
/// t2 = (2520*epsilon)^(1/6)
#[inline]
#[must_use]
pub(crate) fn small_angle_so3_sq<T: Numeric>() -> (T, T) {
    let t1 = (T::from_f64(360.0) * T::EPSILON).cbrt();
    let t2 = (T::from_f64(2520.0) * T::EPSILON).cbrt();
    (t1, t2)
}

/// Small-angle threshold for inverse left jacobian so3
/// t = (16*945*epsilon)^(1/6)
#[inline]
#[must_use]
pub(crate) fn small_angle_inverse_so3_sq<T: Numeric>() -> T {
    (T::from_f64(15_120.0) * T::EPSILON).cbrt()
}

/// Small-angle threshold for q matrix se3
/// The function has two thresholds, take the largest
/// t2 = (2520*espilon)^(1/6)
/// t3 = (12*1680*espilon)^(1/8)
/// t5 = (0.5*9!*epsilon)^(1/8)
#[inline]
#[must_use]
pub(crate) fn small_angle_se3_sq<T: Numeric>() -> (T, T, T) {
    let t2 = (T::from_f64(2520.0) * T::EPSILON).cbrt();
    let t3 = (T::from_f64(20_160.0) * T::EPSILON).sqrt().sqrt();
    let t5 = (T::from_f64(181_440.0) * T::EPSILON).sqrt().sqrt();
    (t2, t3, t5)
}