From 757457bb839f9da2fcd9f555841596b104180626 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sun, 9 Aug 2026 13:57:59 +0300 Subject: [PATCH] math: add fixed-size flight dynamics primitives --- libs/kaptan-math/Cargo.toml | 10 + libs/kaptan-math/src/lib.rs | 898 ++++++++++++++++++++++++++++++++++++ 2 files changed, 908 insertions(+) create mode 100644 libs/kaptan-math/Cargo.toml create mode 100644 libs/kaptan-math/src/lib.rs diff --git a/libs/kaptan-math/Cargo.toml b/libs/kaptan-math/Cargo.toml new file mode 100644 index 0000000..863bc31 --- /dev/null +++ b/libs/kaptan-math/Cargo.toml @@ -0,0 +1,10 @@ +[package] +name = "kaptan-math" +edition.workspace = true +license.workspace = true +repository.workspace = true +rust-version.workspace = true +version.workspace = true + +[lints] +workspace = true diff --git a/libs/kaptan-math/src/lib.rs b/libs/kaptan-math/src/lib.rs new file mode 100644 index 0000000..18b3ba0 --- /dev/null +++ b/libs/kaptan-math/src/lib.rs @@ -0,0 +1,898 @@ +//! Minimal `f32` math shared by Kaptan navigation and control. +//! +//! This crate deliberately exposes only fixed-size vectors and a unit +//! quaternion. Frame markers carry no runtime data and prevent accidental +//! body/local-vector interchange at public navigation and control boundaries. + +#![no_std] + +use core::marker::PhantomData; + +/// Failure reported by a checked mathematical operation. +/// +/// The flight path must treat one of these as a rejected input or an estimator +/// failure. In particular, no quaternion constructor converts an error into +/// the identity rotation. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum MathError { + /// An operand or intermediate value was not finite. + NonFinite, + /// A vector or quaternion did not have a representable non-zero norm. + ZeroNorm, + /// A small-angle quaternion was requested outside its reviewed domain. + RotationIncrementOutOfRange, + /// Rotation-matrix axes were not a finite, right-handed orthonormal basis. + NonOrthonormalAxes, +} + +/// Local north-east-down navigation frame. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum Ned {} +/// Aircraft forward-right-down body frame. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum Frd {} +/// A physical sensor's locally declared coordinate frame. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum Sensor {} + +/// Three `f32` components expressed in one explicit frame. +#[derive(Debug, PartialEq)] +pub struct Vector3 { + x: f32, + y: f32, + z: f32, + _frame: PhantomData, +} + +impl Copy for Vector3 {} +impl Clone for Vector3 { + fn clone(&self) -> Self { + *self + } +} + +impl Vector3 { + /// Creates a vector whose components use the caller's documented unit. + pub const fn new(x: f32, y: f32, z: f32) -> Self { + Self { + x, + y, + z, + _frame: PhantomData, + } + } + + /// Returns the x component. + pub const fn x(self) -> f32 { + self.x + } + /// Returns the y component. + pub const fn y(self) -> f32 { + self.y + } + /// Returns the z component. + pub const fn z(self) -> f32 { + self.z + } + + /// Returns the zero vector. + pub const fn zero() -> Self { + Self::new(0.0, 0.0, 0.0) + } + + /// Returns whether every component is finite. + pub fn is_finite(self) -> bool { + self.x.is_finite() & self.y.is_finite() & self.z.is_finite() + } + + /// Adds vectors in the same frame and unit. + pub const fn add(self, other: Self) -> Self { + Self::new(self.x + other.x, self.y + other.y, self.z + other.z) + } + + /// Subtracts vectors in the same frame and unit. + pub const fn sub(self, other: Self) -> Self { + Self::new(self.x - other.x, self.y - other.y, self.z - other.z) + } + + /// Scales a vector without changing its frame. + pub const fn scale(self, scalar: f32) -> Self { + Self::new(self.x * scalar, self.y * scalar, self.z * scalar) + } + + /// Dot product of vectors in the same frame. + pub const fn dot(self, other: Self) -> f32 { + self.x * other.x + self.y * other.y + self.z * other.z + } + + /// Cross product of vectors in the same frame. + pub const fn cross(self, other: Self) -> Self { + Self::new( + self.y * other.z - self.z * other.y, + self.z * other.x - self.x * other.z, + self.x * other.y - self.y * other.x, + ) + } + + /// Squared Euclidean norm. + pub const fn norm_squared(self) -> f32 { + self.dot(self) + } + + /// Euclidean norm using a bounded Newton iteration. + pub fn norm(self) -> f32 { + sqrt(self.norm_squared()) + } + + /// Returns a unit vector, rejecting zero and non-finite input. + /// + /// Scaling before normalization prevents an otherwise finite, large vector + /// from overflowing while its direction is still perfectly representable. + pub fn normalized(self) -> Option { + if !self.is_finite() { + return None; + } + let scale = self.x.abs().max(self.y.abs()).max(self.z.abs()); + if scale <= 1.0e-8 { + return None; + } + let scaled = self.scale(1.0 / scale); + let norm = sqrt(scaled.norm_squared()); + // `scale` is a finite non-zero component, so `scaled` contains one + // component with magnitude one. Its squared norm is therefore finite + // and at least one; keep that proof local rather than carrying an + // unreachable fallback after the public input validation above. + debug_assert!(norm.is_finite()); + debug_assert!(norm > 0.0); + let normalized = scaled.scale(1.0 / norm); + normalized.is_finite().then_some(normalized) + } +} + +/// Two `f32` components expressed in one explicit frame. +#[derive(Debug, PartialEq)] +pub struct Vector2 { + x: f32, + y: f32, + _frame: PhantomData, +} + +impl Copy for Vector2 {} +impl Clone for Vector2 { + fn clone(&self) -> Self { + *self + } +} + +impl Vector2 { + /// Creates a two-dimensional vector. + pub const fn new(x: f32, y: f32) -> Self { + Self { + x, + y, + _frame: PhantomData, + } + } + /// Returns the x component. + pub const fn x(self) -> f32 { + self.x + } + /// Returns the y component. + pub const fn y(self) -> f32 { + self.y + } + /// Returns whether both components are finite. + pub fn is_finite(self) -> bool { + self.x.is_finite() && self.y.is_finite() + } +} + +/// A normalized Hamilton rotation from `From` into `To`. +/// +/// Source and destination frames are zero-sized type parameters, so a caller +/// cannot apply a valid quaternion to a vector in an unrelated frame. The +/// navigation convention is body-to-NED. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct UnitQuaternion { + w: f32, + x: f32, + y: f32, + z: f32, + _frames: PhantomData To>, +} + +impl UnitQuaternion { + /// Identity-component rotation. Calling this for different frame types is + /// an explicit assertion that their declared axes are aligned. + pub const fn identity() -> Self { + Self { + w: 1.0, + x: 0.0, + y: 0.0, + z: 0.0, + _frames: PhantomData, + } + } + + /// Normalizes raw Hamilton components, rejecting non-finite or zero input. + /// + /// Values are scaled before the norm is evaluated, so valid directions + /// with very large finite components do not overflow to an identity-like + /// result. + pub fn from_components(w: f32, x: f32, y: f32, z: f32) -> Result { + if !w.is_finite() | !x.is_finite() | !y.is_finite() | !z.is_finite() { + return Err(MathError::NonFinite); + } + let scale = w.abs().max(x.abs()).max(y.abs()).max(z.abs()); + if scale <= 1.0e-8 { + return Err(MathError::ZeroNorm); + } + let sw = w / scale; + let sx = x / scale; + let sy = y / scale; + let sz = z / scale; + let norm = sqrt(sw * sw + sx * sx + sy * sy + sz * sz); + // A finite non-zero maximum component makes one scaled component + // exactly +/-1. The normalization norm is consequently finite and + // strictly positive after the checks above. + debug_assert!(norm.is_finite()); + debug_assert!(norm > 0.0); + Ok(Self { + w: sw / norm, + x: sx / norm, + y: sy / norm, + z: sz / norm, + _frames: PhantomData, + }) + } + + /// Returns the scalar component. + pub const fn w(self) -> f32 { + self.w + } + /// Returns the x imaginary component. + pub const fn x(self) -> f32 { + self.x + } + /// Returns the y imaginary component. + pub const fn y(self) -> f32 { + self.y + } + /// Returns the z imaginary component. + pub const fn z(self) -> f32 { + self.z + } + + /// Returns whether components are finite and normalized within a small + /// floating-point tolerance. + pub fn is_normalized(self) -> bool { + let squared_norm_error = + (self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z - 1.0).abs(); + self.w.is_finite() + & self.x.is_finite() + & self.y.is_finite() + & self.z.is_finite() + & (squared_norm_error < 2.0e-4) + } + + /// Composes rotations: the returned rotation applies `right` then `self`. + pub fn compose( + self, + right: UnitQuaternion, + ) -> Result, MathError> { + UnitQuaternion::from_components( + self.w * right.w - self.x * right.x - self.y * right.y - self.z * right.z, + self.w * right.x + self.x * right.w + self.y * right.z - self.z * right.y, + self.w * right.y - self.x * right.z + self.y * right.w + self.z * right.x, + self.w * right.z + self.x * right.y - self.y * right.x + self.z * right.w, + ) + } + + /// Inverts a unit rotation. + pub const fn inverse(self) -> UnitQuaternion { + UnitQuaternion { + w: self.w, + x: -self.x, + y: -self.y, + z: -self.z, + _frames: PhantomData, + } + } + + /// Rotates a vector into this rotation's destination frame. + pub fn rotate(self, vector: Vector3) -> Vector3 { + let q = Vector3::::new(self.x, self.y, self.z); + let v = Vector3::::new(vector.x, vector.y, vector.z); + let twice_cross = q.cross(v).scale(2.0); + v.add(twice_cross.scale(self.w)).add(q.cross(twice_cross)) + } +} + +impl UnitQuaternion { + /// Builds an incremental same-frame rotation from a rotation vector in + /// radians. The flight estimator admits increments no larger than two + /// radians before calling this bounded polynomial, avoiding a hidden + /// `libm` dependency in the rate path. + pub fn from_small_rotation_vector(rotation: Vector3) -> Result { + if !rotation.is_finite() { + return Err(MathError::NonFinite); + } + let rotation_squared = rotation.norm_squared(); + if !rotation_squared.is_finite() { + return Err(MathError::NonFinite); + } + if rotation_squared > 4.0 { + return Err(MathError::RotationIncrementOutOfRange); + } + let half_squared = 0.25 * rotation_squared; + let sin_over_angle = 0.5 * (1.0 - half_squared / 6.0 + half_squared * half_squared / 120.0); + let cosine = 1.0 - half_squared / 2.0 + half_squared * half_squared / 24.0 + - half_squared * half_squared * half_squared / 720.0; + // The expression is close to unit length for its stated domain. A + // normalization removes accumulated polynomial/rounding error. + Self::from_components( + cosine, + rotation.x * sin_over_angle, + rotation.y * sin_over_angle, + rotation.z * sin_over_angle, + ) + } +} + +impl UnitQuaternion { + /// Builds a body-to-NED rotation from orthonormal body x/y/z axes + /// expressed in NED. + pub fn from_body_axes_in_ned( + body_x: Vector3, + body_y: Vector3, + body_z: Vector3, + ) -> Result { + if !body_x.is_finite() || !body_y.is_finite() || !body_z.is_finite() { + return Err(MathError::NonFinite); + } + const ORTHONORMAL_TOLERANCE: f32 = 2.0e-3; + let unit = |axis: Vector3| (axis.norm_squared() - 1.0).abs() <= ORTHONORMAL_TOLERANCE; + let handedness = body_x.cross(body_y).dot(body_z); + if !unit(body_x) + || !unit(body_y) + || !unit(body_z) + || body_x.dot(body_y).abs() > ORTHONORMAL_TOLERANCE + || body_x.dot(body_z).abs() > ORTHONORMAL_TOLERANCE + || body_y.dot(body_z).abs() > ORTHONORMAL_TOLERANCE + || (handedness - 1.0).abs() > ORTHONORMAL_TOLERANCE + { + return Err(MathError::NonOrthonormalAxes); + } + let m00 = body_x.x; + let m01 = body_y.x; + let m02 = body_z.x; + let m10 = body_x.y; + let m11 = body_y.y; + let m12 = body_z.y; + let m20 = body_x.z; + let m21 = body_y.z; + let m22 = body_z.z; + let trace = m00 + m11 + m22; + let (w, x, y, z) = if trace > 0.0 { + let s = 2.0 * sqrt(trace + 1.0); + (0.25 * s, (m21 - m12) / s, (m02 - m20) / s, (m10 - m01) / s) + } else if m00 > m11 && m00 > m22 { + let s = 2.0 * sqrt(1.0 + m00 - m11 - m22); + ((m21 - m12) / s, 0.25 * s, (m01 + m10) / s, (m02 + m20) / s) + } else if m11 > m22 { + let s = 2.0 * sqrt(1.0 + m11 - m00 - m22); + ((m02 - m20) / s, (m01 + m10) / s, 0.25 * s, (m12 + m21) / s) + } else { + let s = 2.0 * sqrt(1.0 + m22 - m00 - m11); + ((m10 - m01) / s, (m02 + m20) / s, (m12 + m21) / s, 0.25 * s) + }; + Self::from_components(w, x, y, z) + } + + /// Rotates a body vector into NED using a body-to-NED quaternion. + pub fn body_to_ned(self, vector: Vector3) -> Vector3 { + self.rotate(vector) + } + + /// Rotates an NED vector into body using a body-to-NED quaternion. + pub fn ned_to_body(self, vector: Vector3) -> Vector3 { + self.inverse().rotate(vector) + } +} + +/// Bounded square root for positive finite `f32` values. +/// +/// A scaled exponent estimate plus four Newton iterations provides bounded +/// vector/quaternion normalization precision across the finite `f32` range. +/// Negative/non-finite input is rejected as `NaN` rather than silently +/// converted. +pub fn sqrt(value: f32) -> f32 { + if !value.is_finite() || value < 0.0 { + return f32::NAN; + } + if value == 0.0 { + return value; + } + // The exponent shortcut below assumes a normal input. Scale a + // subnormal by an exact power of two, then undo half that exponent after + // the same bounded iteration. This keeps the smallest finite positive + // `f32` in the reviewed accuracy envelope without a libm dependency. + if value < f32::MIN_POSITIVE { + return sqrt(value * 16_777_216.0) / 4_096.0; + } + // A bit-level exponent estimate makes the fixed iteration count accurate + // for subnormal through maximum finite inputs. It avoids linking a + // platform libm implementation in the embedded rate path. + let mut estimate = f32::from_bits((value.to_bits() >> 1) + 0x1fc0_0000); + let mut iteration = 0; + while iteration < 4 { + estimate = 0.5 * (estimate + value / estimate); + iteration += 1; + } + estimate +} + +/// Applies the SO(3) right Jacobian to a vector in one frame. +/// +/// The same bounded two-radian increment domain as +/// [`UnitQuaternion::from_small_rotation_vector`] avoids a hidden `libm` +/// dependency. The coefficients are fixed Taylor polynomials for +/// `(1 - cos(theta)) / theta²` and `(theta - sin(theta)) / theta³`. +pub fn right_jacobian_apply( + rotation: Vector3, + vector: Vector3, +) -> Result, MathError> { + jacobian_apply(rotation, vector, -1.0) +} + +/// Applies the SO(3) left Jacobian to a vector in one frame. +/// +/// For the same rotation, the left and right Jacobians differ only in the +/// sign of the first cross-product term. +pub fn left_jacobian_apply( + rotation: Vector3, + vector: Vector3, +) -> Result, MathError> { + jacobian_apply(rotation, vector, 1.0) +} + +fn jacobian_apply( + rotation: Vector3, + vector: Vector3, + first_cross_sign: f32, +) -> Result, MathError> { + if !rotation.is_finite() | !vector.is_finite() { + return Err(MathError::NonFinite); + } + let theta_squared = rotation.norm_squared(); + if !theta_squared.is_finite() { + return Err(MathError::NonFinite); + } + if theta_squared > 4.0 { + return Err(MathError::RotationIncrementOutOfRange); + } + let theta_fourth = theta_squared * theta_squared; + let theta_sixth = theta_fourth * theta_squared; + let theta_eighth = theta_fourth * theta_fourth; + let first_coefficient = 0.5 - theta_squared / 24.0 + theta_fourth / 720.0 + - theta_sixth / 40_320.0 + + theta_eighth / 3_628_800.0; + let second_coefficient = 1.0 / 6.0 - theta_squared / 120.0 + theta_fourth / 5_040.0 + - theta_sixth / 362_880.0 + + theta_eighth / 39_916_800.0; + let cross = rotation.cross(vector); + let result = vector + .add(cross.scale(first_cross_sign * first_coefficient)) + .add(rotation.cross(cross).scale(second_coefficient)); + result + .is_finite() + .then_some(result) + .ok_or(MathError::NonFinite) +} + +#[cfg(test)] +mod tests { + use core::marker::PhantomData; + + use super::{ + Frd, MathError, Ned, UnitQuaternion, Vector3, left_jacobian_apply, right_jacobian_apply, + sqrt, + }; + + #[test] + fn frame_rotation_preserves_norm() { + let q = UnitQuaternion::::identity(); + let vector = Vector3::::new(3.0, -4.0, 2.0); + assert!((q.body_to_ned(vector).norm() - vector.norm()).abs() < 1.0e-3); + } + + #[test] + fn composed_rotations_normalize() { + let delta = + UnitQuaternion::from_small_rotation_vector(Vector3::::new(0.0, 0.0, 0.001)) + .unwrap(); + let mut q = UnitQuaternion::::identity(); + for _ in 0..10_000 { + q = q.compose(delta).unwrap(); + } + assert!(q.is_normalized()); + let forward = q.body_to_ned(Vector3::::new(1.0, 0.0, 0.0)); + assert!((forward.x() + 0.839).abs() < 0.02); + } + + #[test] + fn matrix_identity() { + let q = UnitQuaternion::from_body_axes_in_ned( + Vector3::::new(1.0, 0.0, 0.0), + Vector3::::new(0.0, 1.0, 0.0), + Vector3::::new(0.0, 0.0, 1.0), + ) + .unwrap(); + assert_eq!(q, UnitQuaternion::::identity()); + } + + #[test] + fn sqrt_accuracy() { + assert!((sqrt(2.0) - core::f32::consts::SQRT_2).abs() < 1.0e-5); + for value in [ + f32::from_bits(1), + f32::from_bits(2), + f32::from_bits(0x007f_ffff), + ] { + let root = sqrt(value); + assert!(root.is_finite()); + assert!(root > 0.0); + assert!(((root * root - value) / value).abs() < 2.0e-5); + } + } + + #[test] + fn so3_jacobians_match_their_bounded_series_reference() { + let rotation = Vector3::::new(0.0, 0.0, 0.5); + let vector = Vector3::::new(1.0, 0.0, 0.0); + let right = right_jacobian_apply(rotation, vector).unwrap(); + let left = left_jacobian_apply(rotation, vector).unwrap(); + let theta = 0.5_f32; + let expected_x = theta.sin() / theta; + let expected_y = (1.0 - theta.cos()) / theta; + assert!((right.x() - expected_x).abs() < 1.0e-6); + assert!((right.y() + expected_y).abs() < 1.0e-6); + assert!((left.x() - expected_x).abs() < 1.0e-6); + assert!((left.y() - expected_y).abs() < 1.0e-6); + assert_eq!( + right_jacobian_apply(Vector3::new(2.1, 0.0, 0.0), vector), + Err(MathError::RotationIncrementOutOfRange) + ); + assert_eq!( + left_jacobian_apply(Vector3::new(f32::NAN, 0.0, 0.0), vector), + Err(MathError::NonFinite) + ); + assert_eq!( + right_jacobian_apply(Vector3::new(f32::MAX, 0.0, 0.0), vector), + Err(MathError::NonFinite) + ); + } + + #[test] + fn quaternion_validation() { + assert_eq!( + UnitQuaternion::::from_components(0.0, 0.0, 0.0, 0.0), + Err(MathError::ZeroNorm) + ); + assert_eq!( + UnitQuaternion::::from_components(f32::NAN, 0.0, 0.0, 0.0), + Err(MathError::NonFinite) + ); + assert_eq!( + UnitQuaternion::::from_small_rotation_vector(Vector3::new(2.01, 0.0, 0.0)), + Err(MathError::RotationIncrementOutOfRange) + ); + assert_eq!( + UnitQuaternion::from_body_axes_in_ned( + Vector3::::new(1.0, 0.0, 0.0), + Vector3::::new(1.0, 0.0, 0.0), + Vector3::::new(0.0, 0.0, 1.0), + ), + Err(MathError::NonOrthonormalAxes) + ); + } + + #[test] + fn quaternion_known_answers() { + let half = core::f32::consts::FRAC_1_SQRT_2; + let q = UnitQuaternion::::from_components(half, 0.0, 0.0, half).unwrap(); + let east = q.body_to_ned(Vector3::new(1.0, 0.0, 0.0)); + assert!(east.x().abs() < 2.0e-6); + assert!((east.y() - 1.0).abs() < 2.0e-6); + + let huge = + UnitQuaternion::::from_components(f32::MAX, -f32::MAX, f32::MAX, -f32::MAX) + .unwrap(); + assert!(huge.is_normalized()); + let recovered = q.ned_to_body(east); + assert!((recovered.x() - 1.0).abs() < 2.0e-6); + assert!(recovered.y().abs() < 2.0e-6); + } + + #[test] + fn sqrt_rejects_invalid_input() { + assert!(sqrt(f32::NAN).is_nan()); + assert!(sqrt(f32::INFINITY).is_nan()); + assert!(sqrt(-1.0).is_nan()); + assert_eq!(sqrt(-0.0).to_bits(), (-0.0_f32).to_bits()); + + for value in [ + f32::from_bits(1), + f32::from_bits(0x007f_ffff), + f32::MIN_POSITIVE, + 1.0e-20, + 1.0e-6, + 2.0, + 123.456, + f32::MAX, + ] { + let reference = (value as f64).sqrt() as f32; + let actual = sqrt(value); + assert!(((actual - reference) / reference).abs() < 3.0e-6, "{value}"); + } + } + + #[test] + fn vector_validation() { + let vector = Vector3::::new(3.0, 4.0, 0.0); + assert_eq!(vector.clone(), vector); + assert_eq!((vector.x(), vector.y(), vector.z()), (3.0, 4.0, 0.0)); + assert_eq!(vector.normalized().unwrap(), Vector3::new(0.6, 0.8, 0.0)); + assert!( + Vector3::::new(f32::NAN, 0.0, 0.0) + .normalized() + .is_none() + ); + assert!(Vector3::::zero().normalized().is_none()); + assert_eq!( + vector.sub(Vector3::new(1.0, -2.0, 3.0)), + Vector3::new(2.0, 6.0, -3.0) + ); + for invalid in [ + Vector3::::new(f32::NAN, 0.0, 0.0), + Vector3::::new(0.0, f32::INFINITY, 0.0), + Vector3::::new(0.0, 0.0, f32::NEG_INFINITY), + ] { + assert!(!invalid.is_finite()); + } + + let two = super::Vector2::::new(1.0, -2.0); + assert_eq!(two.clone(), two); + assert_eq!((two.x(), two.y()), (1.0, -2.0)); + assert!(two.is_finite()); + assert!(!super::Vector2::::new(f32::NAN, 0.0).is_finite()); + } + + #[test] + fn quaternion_accessors() { + let quaternion = UnitQuaternion::::from_components(1.0, 2.0, 3.0, 4.0).unwrap(); + assert_eq!(quaternion.clone(), quaternion); + assert!(quaternion.w() > 0.0); + assert!(quaternion.x() > 0.0); + assert!(quaternion.y() > 0.0); + assert!(quaternion.z() > 0.0); + assert!( + UnitQuaternion::::identity() + .inverse() + .is_normalized() + ); + assert_eq!( + UnitQuaternion::::from_small_rotation_vector(Vector3::new( + f32::INFINITY, + 0.0, + 0.0 + )), + Err(MathError::NonFinite) + ); + assert_eq!( + UnitQuaternion::::from_small_rotation_vector(Vector3::new( + f32::MAX, + 0.0, + 0.0 + )), + Err(MathError::NonFinite) + ); + assert_eq!( + UnitQuaternion::::from_body_axes_in_ned( + Vector3::new(f32::NAN, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ), + Err(MathError::NonFinite) + ); + } + + #[test] + fn matrix_diagonals() { + let axes = [ + // 180 degree rotations about body x, y, and z respectively. + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(0.0, -1.0, 0.0), + Vector3::new(0.0, 0.0, -1.0), + ], + [ + Vector3::::new(-1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, -1.0), + ], + [ + Vector3::::new(-1.0, 0.0, 0.0), + Vector3::new(0.0, -1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ], + ]; + for [x, y, z] in axes { + assert!( + UnitQuaternion::from_body_axes_in_ned(x, y, z) + .unwrap() + .is_normalized() + ); + } + } + + #[test] + fn validation_operands() { + for vector in [ + Vector3::::new(f32::NAN, 0.0, 0.0), + Vector3::new(0.0, f32::NAN, 0.0), + Vector3::new(0.0, 0.0, f32::NAN), + ] { + assert!(!vector.is_finite()); + } + for components in [ + (f32::NAN, 1.0, 0.0, 0.0), + (1.0, f32::NAN, 0.0, 0.0), + (1.0, 0.0, f32::NAN, 0.0), + (1.0, 0.0, 0.0, f32::NAN), + ] { + assert_eq!( + UnitQuaternion::::from_components( + components.0, + components.1, + components.2, + components.3, + ), + Err(MathError::NonFinite) + ); + } + assert_eq!( + UnitQuaternion::::from_components(f32::INFINITY, 0.0, 0.0, 0.0), + Err(MathError::NonFinite) + ); + assert_eq!( + UnitQuaternion::::from_components(1.0, f32::INFINITY, 0.0, 0.0), + Err(MathError::NonFinite) + ); + assert_eq!( + UnitQuaternion::::from_components(1.0, 0.0, f32::INFINITY, 0.0), + Err(MathError::NonFinite) + ); + assert_eq!( + UnitQuaternion::::from_components(1.0, 0.0, 0.0, f32::INFINITY), + Err(MathError::NonFinite) + ); + for quaternion in [ + UnitQuaternion:: { + w: f32::NAN, + x: 0.0, + y: 0.0, + z: 0.0, + _frames: PhantomData, + }, + UnitQuaternion:: { + w: 1.0, + x: f32::NAN, + y: 0.0, + z: 0.0, + _frames: PhantomData, + }, + UnitQuaternion:: { + w: 1.0, + x: 0.0, + y: f32::NAN, + z: 0.0, + _frames: PhantomData, + }, + UnitQuaternion:: { + w: 1.0, + x: 0.0, + y: 0.0, + z: f32::NAN, + _frames: PhantomData, + }, + UnitQuaternion:: { + w: 2.0, + x: 0.0, + y: 0.0, + z: 0.0, + _frames: PhantomData, + }, + ] { + assert!(!quaternion.is_normalized()); + } + for axes in [ + [ + Vector3::::new(2.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ], + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 2.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ], + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 2.0), + ], + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(0.1, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ], + [ + Vector3::::new(1.0, 0.0, 0.1), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ], + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.1), + Vector3::new(0.0, 0.0, 1.0), + ], + ] { + assert_eq!( + UnitQuaternion::from_body_axes_in_ned(axes[0], axes[1], axes[2]), + Err(MathError::NonOrthonormalAxes) + ); + } + for axes in [ + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(f32::NAN, 0.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ], + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(f32::NAN, 0.0, 0.0), + ], + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(0.01, 0.999_95, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ], + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.01, 0.0, 0.999_95), + ], + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.01, 0.999_95), + ], + [ + Vector3::::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, -1.0), + ], + ] { + assert!(UnitQuaternion::from_body_axes_in_ned(axes[0], axes[1], axes[2]).is_err()); + } + let diagonal_tie = UnitQuaternion::from_body_axes_in_ned( + Vector3::::new(0.0, 0.0, 1.0), + Vector3::new(0.0, -1.0, 0.0), + Vector3::new(1.0, 0.0, 0.0), + ) + .unwrap(); + assert!(diagonal_tie.is_normalized()); + } +}