math: add fixed-size flight dynamics primitives
This commit is contained in:
Onur Özkan committed
1 parent
b0d22b61d8
commit
757457bb83
2 files changed
+908
No files matched your search
@@ -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
|
||||
@@ -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<Frame> {
|
||||
x: f32,
|
||||
y: f32,
|
||||
z: f32,
|
||||
_frame: PhantomData<Frame>,
|
||||
}
|
||||
|
||||
impl<Frame> Copy for Vector3<Frame> {}
|
||||
impl<Frame> Clone for Vector3<Frame> {
|
||||
fn clone(&self) -> Self {
|
||||
*self
|
||||
}
|
||||
}
|
||||
|
||||
impl<Frame> Vector3<Frame> {
|
||||
/// 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<Self> {
|
||||
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<Frame> {
|
||||
x: f32,
|
||||
y: f32,
|
||||
_frame: PhantomData<Frame>,
|
||||
}
|
||||
|
||||
impl<Frame> Copy for Vector2<Frame> {}
|
||||
impl<Frame> Clone for Vector2<Frame> {
|
||||
fn clone(&self) -> Self {
|
||||
*self
|
||||
}
|
||||
}
|
||||
|
||||
impl<Frame> Vector2<Frame> {
|
||||
/// 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<From, To> {
|
||||
w: f32,
|
||||
x: f32,
|
||||
y: f32,
|
||||
z: f32,
|
||||
_frames: PhantomData<fn(From) -> To>,
|
||||
}
|
||||
|
||||
impl<From, To> UnitQuaternion<From, To> {
|
||||
/// 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<Self, MathError> {
|
||||
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<RightFrom>(
|
||||
self,
|
||||
right: UnitQuaternion<RightFrom, From>,
|
||||
) -> Result<UnitQuaternion<RightFrom, To>, 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<To, From> {
|
||||
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<From>) -> Vector3<To> {
|
||||
let q = Vector3::<To>::new(self.x, self.y, self.z);
|
||||
let v = Vector3::<To>::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<Frame> UnitQuaternion<Frame, Frame> {
|
||||
/// 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<Frame>) -> Result<Self, MathError> {
|
||||
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<Frd, Ned> {
|
||||
/// 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<Ned>,
|
||||
body_y: Vector3<Ned>,
|
||||
body_z: Vector3<Ned>,
|
||||
) -> Result<Self, MathError> {
|
||||
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<Ned>| (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<Frd>) -> Vector3<Ned> {
|
||||
self.rotate(vector)
|
||||
}
|
||||
|
||||
/// Rotates an NED vector into body using a body-to-NED quaternion.
|
||||
pub fn ned_to_body(self, vector: Vector3<Ned>) -> Vector3<Frd> {
|
||||
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<Frame>(
|
||||
rotation: Vector3<Frame>,
|
||||
vector: Vector3<Frame>,
|
||||
) -> Result<Vector3<Frame>, 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<Frame>(
|
||||
rotation: Vector3<Frame>,
|
||||
vector: Vector3<Frame>,
|
||||
) -> Result<Vector3<Frame>, MathError> {
|
||||
jacobian_apply(rotation, vector, 1.0)
|
||||
}
|
||||
|
||||
fn jacobian_apply<Frame>(
|
||||
rotation: Vector3<Frame>,
|
||||
vector: Vector3<Frame>,
|
||||
first_cross_sign: f32,
|
||||
) -> Result<Vector3<Frame>, 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::<Frd, Ned>::identity();
|
||||
let vector = Vector3::<Frd>::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::<Frd>::new(0.0, 0.0, 0.001))
|
||||
.unwrap();
|
||||
let mut q = UnitQuaternion::<Frd, Ned>::identity();
|
||||
for _ in 0..10_000 {
|
||||
q = q.compose(delta).unwrap();
|
||||
}
|
||||
assert!(q.is_normalized());
|
||||
let forward = q.body_to_ned(Vector3::<Frd>::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::<Ned>::new(1.0, 0.0, 0.0),
|
||||
Vector3::<Ned>::new(0.0, 1.0, 0.0),
|
||||
Vector3::<Ned>::new(0.0, 0.0, 1.0),
|
||||
)
|
||||
.unwrap();
|
||||
assert_eq!(q, UnitQuaternion::<Frd, Ned>::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::<Frd>::new(0.0, 0.0, 0.5);
|
||||
let vector = Vector3::<Frd>::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::<Frd, Ned>::from_components(0.0, 0.0, 0.0, 0.0),
|
||||
Err(MathError::ZeroNorm)
|
||||
);
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Ned>::from_components(f32::NAN, 0.0, 0.0, 0.0),
|
||||
Err(MathError::NonFinite)
|
||||
);
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Frd>::from_small_rotation_vector(Vector3::new(2.01, 0.0, 0.0)),
|
||||
Err(MathError::RotationIncrementOutOfRange)
|
||||
);
|
||||
assert_eq!(
|
||||
UnitQuaternion::from_body_axes_in_ned(
|
||||
Vector3::<Ned>::new(1.0, 0.0, 0.0),
|
||||
Vector3::<Ned>::new(1.0, 0.0, 0.0),
|
||||
Vector3::<Ned>::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::<Frd, Ned>::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::<Frd, Ned>::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::<Frd>::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::<Frd>::new(f32::NAN, 0.0, 0.0)
|
||||
.normalized()
|
||||
.is_none()
|
||||
);
|
||||
assert!(Vector3::<Frd>::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::<Frd>::new(f32::NAN, 0.0, 0.0),
|
||||
Vector3::<Frd>::new(0.0, f32::INFINITY, 0.0),
|
||||
Vector3::<Frd>::new(0.0, 0.0, f32::NEG_INFINITY),
|
||||
] {
|
||||
assert!(!invalid.is_finite());
|
||||
}
|
||||
|
||||
let two = super::Vector2::<Frd>::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::<Frd>::new(f32::NAN, 0.0).is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn quaternion_accessors() {
|
||||
let quaternion = UnitQuaternion::<Frd, Ned>::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::<Frd, Ned>::identity()
|
||||
.inverse()
|
||||
.is_normalized()
|
||||
);
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Frd>::from_small_rotation_vector(Vector3::new(
|
||||
f32::INFINITY,
|
||||
0.0,
|
||||
0.0
|
||||
)),
|
||||
Err(MathError::NonFinite)
|
||||
);
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Frd>::from_small_rotation_vector(Vector3::new(
|
||||
f32::MAX,
|
||||
0.0,
|
||||
0.0
|
||||
)),
|
||||
Err(MathError::NonFinite)
|
||||
);
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Ned>::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::<Ned>::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(0.0, -1.0, 0.0),
|
||||
Vector3::new(0.0, 0.0, -1.0),
|
||||
],
|
||||
[
|
||||
Vector3::<Ned>::new(-1.0, 0.0, 0.0),
|
||||
Vector3::new(0.0, 1.0, 0.0),
|
||||
Vector3::new(0.0, 0.0, -1.0),
|
||||
],
|
||||
[
|
||||
Vector3::<Ned>::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::<Frd>::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::<Frd, Ned>::from_components(
|
||||
components.0,
|
||||
components.1,
|
||||
components.2,
|
||||
components.3,
|
||||
),
|
||||
Err(MathError::NonFinite)
|
||||
);
|
||||
}
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Ned>::from_components(f32::INFINITY, 0.0, 0.0, 0.0),
|
||||
Err(MathError::NonFinite)
|
||||
);
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Ned>::from_components(1.0, f32::INFINITY, 0.0, 0.0),
|
||||
Err(MathError::NonFinite)
|
||||
);
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Ned>::from_components(1.0, 0.0, f32::INFINITY, 0.0),
|
||||
Err(MathError::NonFinite)
|
||||
);
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Ned>::from_components(1.0, 0.0, 0.0, f32::INFINITY),
|
||||
Err(MathError::NonFinite)
|
||||
);
|
||||
for quaternion in [
|
||||
UnitQuaternion::<Frd, Ned> {
|
||||
w: f32::NAN,
|
||||
x: 0.0,
|
||||
y: 0.0,
|
||||
z: 0.0,
|
||||
_frames: PhantomData,
|
||||
},
|
||||
UnitQuaternion::<Frd, Ned> {
|
||||
w: 1.0,
|
||||
x: f32::NAN,
|
||||
y: 0.0,
|
||||
z: 0.0,
|
||||
_frames: PhantomData,
|
||||
},
|
||||
UnitQuaternion::<Frd, Ned> {
|
||||
w: 1.0,
|
||||
x: 0.0,
|
||||
y: f32::NAN,
|
||||
z: 0.0,
|
||||
_frames: PhantomData,
|
||||
},
|
||||
UnitQuaternion::<Frd, Ned> {
|
||||
w: 1.0,
|
||||
x: 0.0,
|
||||
y: 0.0,
|
||||
z: f32::NAN,
|
||||
_frames: PhantomData,
|
||||
},
|
||||
UnitQuaternion::<Frd, Ned> {
|
||||
w: 2.0,
|
||||
x: 0.0,
|
||||
y: 0.0,
|
||||
z: 0.0,
|
||||
_frames: PhantomData,
|
||||
},
|
||||
] {
|
||||
assert!(!quaternion.is_normalized());
|
||||
}
|
||||
for axes in [
|
||||
[
|
||||
Vector3::<Ned>::new(2.0, 0.0, 0.0),
|
||||
Vector3::new(0.0, 1.0, 0.0),
|
||||
Vector3::new(0.0, 0.0, 1.0),
|
||||
],
|
||||
[
|
||||
Vector3::<Ned>::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(0.0, 2.0, 0.0),
|
||||
Vector3::new(0.0, 0.0, 1.0),
|
||||
],
|
||||
[
|
||||
Vector3::<Ned>::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(0.0, 1.0, 0.0),
|
||||
Vector3::new(0.0, 0.0, 2.0),
|
||||
],
|
||||
[
|
||||
Vector3::<Ned>::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(0.1, 1.0, 0.0),
|
||||
Vector3::new(0.0, 0.0, 1.0),
|
||||
],
|
||||
[
|
||||
Vector3::<Ned>::new(1.0, 0.0, 0.1),
|
||||
Vector3::new(0.0, 1.0, 0.0),
|
||||
Vector3::new(0.0, 0.0, 1.0),
|
||||
],
|
||||
[
|
||||
Vector3::<Ned>::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::<Ned>::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(f32::NAN, 0.0, 0.0),
|
||||
Vector3::new(0.0, 0.0, 1.0),
|
||||
],
|
||||
[
|
||||
Vector3::<Ned>::new(1.0, 0.0, 0.0),
|
||||
Vector3::new(0.0, 1.0, 0.0),
|
||||
Vector3::new(f32::NAN, 0.0, 0.0),
|
||||
],
|
||||
[
|
||||
Vector3::<Ned>::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::<Ned>::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::<Ned>::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::<Ned>::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::<Ned>::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());
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user