feat(firmware): validate configuration and migrate sensor-driven flight APIs
Validate configuration through its owning schemas and keep board declarations separate from shared F722 resource ownership. Bound USB transitions, isolate transport failures and retain restart-only activation. Migrate navigation and flight consumers with the sensor and schema contracts. Require explicit installation data and synchronous output authority; board motor outputs remain inhibited. Validation: assembled-prefix host tests, strict Clippy, platform/USB tests and both ARM release builds. Final-source coverage and scoped Miri/Kani evidence are retained separately from hardware qualification.
This commit is contained in:
Onur Özkan committed
1 parent
fb1b8af2d1
commit
f9a61a086f
211 files changed
+45657
-12740
No files matched your search
@@ -0,0 +1,26 @@
|
||||
//! Classification of the production square root, not an accuracy proof.
|
||||
|
||||
use super::sqrt;
|
||||
|
||||
#[kani::proof]
|
||||
#[kani::unwind(5)]
|
||||
fn sqrt_preserves_domain_and_zero_sign() {
|
||||
let value = f32::from_bits(kani::any());
|
||||
let root = sqrt(value);
|
||||
|
||||
if !value.is_finite() || value < 0.0 {
|
||||
assert!(root.is_nan());
|
||||
} else if value == 0.0 {
|
||||
assert_eq!(root.to_bits(), value.to_bits());
|
||||
} else {
|
||||
assert!(root.is_finite());
|
||||
assert!(root > 0.0);
|
||||
}
|
||||
|
||||
kani::cover!(value.is_nan());
|
||||
kani::cover!(value == f32::INFINITY);
|
||||
kani::cover!(value < 0.0 && value.is_finite());
|
||||
kani::cover!(value.to_bits() == (-0.0_f32).to_bits());
|
||||
kani::cover!(value > 0.0 && value < f32::MIN_POSITIVE);
|
||||
kani::cover!(value == f32::MAX);
|
||||
}
|
||||
@@ -1,19 +1,18 @@
|
||||
//! 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.
|
||||
//! Fixed-size `f32` vectors and unit quaternions with typed coordinate frames.
|
||||
|
||||
#![no_std]
|
||||
#![forbid(unsafe_code)]
|
||||
|
||||
use core::marker::PhantomData;
|
||||
use kaptan_err_reportable::KaptanError;
|
||||
|
||||
#[cfg(kani)]
|
||||
mod kani_proofs;
|
||||
|
||||
/// Squared-radian domain of the bounded SO(3) exponential and Jacobians.
|
||||
pub const MAXIMUM_ROTATION_VECTOR_NORM_SQUARED: f32 = 4.0;
|
||||
|
||||
/// 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, KaptanError)]
|
||||
pub enum MathError {
|
||||
/// An operand or intermediate value was not finite.
|
||||
@@ -22,7 +21,7 @@ pub enum MathError {
|
||||
/// A vector or quaternion did not have a representable non-zero norm.
|
||||
#[kaptan_error(id = 6002)]
|
||||
ZeroNorm,
|
||||
/// A small-angle quaternion was requested outside its reviewed domain.
|
||||
/// A small-angle quaternion was requested outside its supported domain.
|
||||
#[kaptan_error(id = 6003)]
|
||||
RotationIncrementOutOfRange,
|
||||
/// Rotation-matrix axes were not a finite, right-handed orthonormal basis.
|
||||
@@ -36,11 +35,16 @@ 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.
|
||||
///
|
||||
/// Different coordinate frames cannot be added implicitly.
|
||||
/// ```compile_fail,E0308
|
||||
/// use kaptan_math::{Frd, Ned, Vector3};
|
||||
/// let body = Vector3::<Frd>::zero();
|
||||
/// let local = Vector3::<Ned>::zero();
|
||||
/// let _ = body.add(local);
|
||||
/// ```
|
||||
#[derive(Debug, PartialEq)]
|
||||
pub struct Vector3<Frame> {
|
||||
x: f32,
|
||||
@@ -67,20 +71,16 @@ impl<Frame> Vector3<Frame> {
|
||||
}
|
||||
}
|
||||
|
||||
/// 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)
|
||||
}
|
||||
@@ -124,79 +124,56 @@ impl<Frame> Vector3<Frame> {
|
||||
self.dot(self)
|
||||
}
|
||||
|
||||
/// Euclidean norm using a bounded Newton iteration.
|
||||
/// Euclidean norm, scaled to avoid intermediate overflow and underflow.
|
||||
pub fn norm(self) -> f32 {
|
||||
sqrt(self.norm_squared())
|
||||
if !self.is_finite() {
|
||||
return f32::NAN;
|
||||
}
|
||||
let scale = self.x.abs().max(self.y.abs()).max(self.z.abs());
|
||||
if scale == 0.0 {
|
||||
return 0.0;
|
||||
}
|
||||
let scaled = Self::new(self.x / scale, self.y / scale, self.z / scale);
|
||||
scale * sqrt(scaled.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.
|
||||
/// Scaling prevents overflow when the direction is 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 {
|
||||
if scale == 0.0 {
|
||||
return None;
|
||||
}
|
||||
let scaled = self.scale(1.0 / scale);
|
||||
let scaled = Self::new(self.x / scale, self.y / scale, self.z / 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.
|
||||
// component with magnitude one. The squared norm is in [1, 3], so
|
||||
// multiplying components in [-1, 1] by its reciprocal root is finite.
|
||||
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()
|
||||
debug_assert!(normalized.is_finite());
|
||||
Some(normalized)
|
||||
}
|
||||
}
|
||||
|
||||
/// 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.
|
||||
/// A rotation accepts vectors in its source frame.
|
||||
/// ```compile_fail,E0308
|
||||
/// use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3};
|
||||
/// let body_to_local = UnitQuaternion::<Frd, Ned>::identity();
|
||||
/// let _ = body_to_local.rotate(Vector3::<Ned>::zero());
|
||||
/// ```
|
||||
/// Composition requires the inner destination to match the outer source.
|
||||
/// ```compile_fail,E0308
|
||||
/// use kaptan_math::{Frd, Ned, UnitQuaternion};
|
||||
/// let body_to_local = UnitQuaternion::<Frd, Ned>::identity();
|
||||
/// let _ = body_to_local.compose(body_to_local);
|
||||
/// ```
|
||||
#[derive(Clone, Copy, Debug, PartialEq)]
|
||||
pub struct UnitQuaternion<From, To> {
|
||||
w: f32,
|
||||
@@ -220,16 +197,13 @@ impl<From, To> UnitQuaternion<From, To> {
|
||||
}
|
||||
|
||||
/// 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.
|
||||
/// Scaling prevents intermediate norm overflow.
|
||||
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 {
|
||||
if scale == 0.0 {
|
||||
return Err(MathError::ZeroNorm);
|
||||
}
|
||||
let sw = w / scale;
|
||||
@@ -314,10 +288,8 @@ impl<From, To> UnitQuaternion<From, To> {
|
||||
}
|
||||
|
||||
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.
|
||||
/// Builds a same-frame rotation using a polynomial valid up to two radians.
|
||||
/// Larger rotation vectors are rejected.
|
||||
pub fn from_small_rotation_vector(rotation: Vector3<Frame>) -> Result<Self, MathError> {
|
||||
if !rotation.is_finite() {
|
||||
return Err(MathError::NonFinite);
|
||||
@@ -326,13 +298,21 @@ impl<Frame> UnitQuaternion<Frame, Frame> {
|
||||
if !rotation_squared.is_finite() {
|
||||
return Err(MathError::NonFinite);
|
||||
}
|
||||
if rotation_squared > 4.0 {
|
||||
if rotation_squared > MAXIMUM_ROTATION_VECTOR_NORM_SQUARED {
|
||||
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 half_fourth = half_squared * half_squared;
|
||||
let half_sixth = half_fourth * half_squared;
|
||||
let half_eighth = half_fourth * half_fourth;
|
||||
let sin_over_angle = 0.5
|
||||
* (1.0 - half_squared / 6.0 + half_squared * half_squared / 120.0
|
||||
- half_sixth / 5_040.0
|
||||
+ half_eighth / 362_880.0);
|
||||
let cosine = 1.0 - half_squared / 2.0 + half_squared * half_squared / 24.0
|
||||
- half_squared * half_squared * half_squared / 720.0;
|
||||
- half_squared * half_squared * half_squared / 720.0
|
||||
+ half_eighth / 40_320.0
|
||||
- half_eighth * half_squared / 3_628_800.0;
|
||||
// The expression is close to unit length for its stated domain. A
|
||||
// normalization removes accumulated polynomial/rounding error.
|
||||
Self::from_components(
|
||||
@@ -407,10 +387,8 @@ impl UnitQuaternion<Frd, Ned> {
|
||||
|
||||
/// 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.
|
||||
/// Uses an exponent estimate and four Newton iterations. Returns `NaN` for
|
||||
/// negative or non-finite input.
|
||||
pub fn sqrt(value: f32) -> f32 {
|
||||
if !value.is_finite() || value < 0.0 {
|
||||
return f32::NAN;
|
||||
@@ -418,16 +396,11 @@ pub fn sqrt(value: f32) -> f32 {
|
||||
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.
|
||||
// The exponent estimate requires normal input. Scale subnormals by 2^24
|
||||
// and divide their square root by 2^12.
|
||||
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 {
|
||||
@@ -439,10 +412,9 @@ pub fn sqrt(value: f32) -> f32 {
|
||||
|
||||
/// 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³`.
|
||||
/// Uses the two-radian domain of [`UnitQuaternion::from_small_rotation_vector`]
|
||||
/// and Taylor polynomials for `(1 - cos(theta)) / theta²` and
|
||||
/// `(theta - sin(theta)) / theta³`.
|
||||
pub fn right_jacobian_apply<Frame>(
|
||||
rotation: Vector3<Frame>,
|
||||
vector: Vector3<Frame>,
|
||||
@@ -473,18 +445,21 @@ fn jacobian_apply<Frame>(
|
||||
if !theta_squared.is_finite() {
|
||||
return Err(MathError::NonFinite);
|
||||
}
|
||||
if theta_squared > 4.0 {
|
||||
if theta_squared > MAXIMUM_ROTATION_VECTOR_NORM_SQUARED {
|
||||
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 theta_tenth = theta_eighth * theta_squared;
|
||||
let first_coefficient = 0.5 - theta_squared / 24.0 + theta_fourth / 720.0
|
||||
- theta_sixth / 40_320.0
|
||||
+ theta_eighth / 3_628_800.0;
|
||||
+ theta_eighth / 3_628_800.0
|
||||
- theta_tenth / 479_001_600.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;
|
||||
+ theta_eighth / 39_916_800.0
|
||||
- theta_tenth / 6_227_020_800.0;
|
||||
let cross = rotation.cross(vector);
|
||||
let result = vector
|
||||
.add(cross.scale(first_cross_sign * first_coefficient))
|
||||
@@ -665,12 +640,6 @@ mod tests {
|
||||
] {
|
||||
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]
|
||||
@@ -750,75 +719,35 @@ mod tests {
|
||||
] {
|
||||
assert!(!vector.is_finite());
|
||||
}
|
||||
for components in [
|
||||
for (w, x, y, z) 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),
|
||||
(f32::INFINITY, 0.0, 0.0, 0.0),
|
||||
(1.0, f32::INFINITY, 0.0, 0.0),
|
||||
(1.0, 0.0, f32::INFINITY, 0.0),
|
||||
(1.0, 0.0, 0.0, f32::INFINITY),
|
||||
] {
|
||||
assert_eq!(
|
||||
UnitQuaternion::<Frd, Ned>::from_components(
|
||||
components.0,
|
||||
components.1,
|
||||
components.2,
|
||||
components.3,
|
||||
),
|
||||
UnitQuaternion::<Frd, Ned>::from_components(w, x, y, z),
|
||||
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,
|
||||
},
|
||||
for (w, x, y, z) in [
|
||||
(f32::NAN, 0.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),
|
||||
(2.0, 0.0, 0.0, 0.0),
|
||||
] {
|
||||
let quaternion = UnitQuaternion::<Frd, Ned> {
|
||||
w,
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
_frames: PhantomData,
|
||||
};
|
||||
assert!(!quaternion.is_normalized());
|
||||
}
|
||||
for axes in [
|
||||
|
||||
@@ -0,0 +1,122 @@
|
||||
use kaptan_math::{Frd, UnitQuaternion, Vector3, left_jacobian_apply, right_jacobian_apply};
|
||||
|
||||
fn exp(v: [f64; 3]) -> [f64; 4] {
|
||||
let norm = v.iter().map(|x| x * x).sum::<f64>().sqrt();
|
||||
let scale = if norm == 0.0 {
|
||||
0.5
|
||||
} else {
|
||||
(norm / 2.0).sin() / norm
|
||||
};
|
||||
[(norm / 2.0).cos(), v[0] * scale, v[1] * scale, v[2] * scale]
|
||||
}
|
||||
|
||||
fn multiply(a: [f64; 4], b: [f64; 4]) -> [f64; 4] {
|
||||
[
|
||||
a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3],
|
||||
a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2],
|
||||
a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1],
|
||||
a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0],
|
||||
]
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn bounded_exponential_and_jacobians_match_f64_group_differentials() {
|
||||
// Axial and oblique directions across the admitted domain. Central
|
||||
// differences of Exp(phi + d) Exp(-phi) independently fix the left sign.
|
||||
for direction in [
|
||||
[1.0_f64, 0.0, 0.0],
|
||||
[0.0, 0.0, -1.0],
|
||||
[2.0 / 3.0, -1.0 / 3.0, 2.0 / 3.0],
|
||||
] {
|
||||
for magnitude in [0.0_f64, 1.0e-7, 0.001, 0.2, 1.0, 1.5, 1.999, 2.0] {
|
||||
let phi = direction.map(|x| (x * magnitude) as f32);
|
||||
let phi64 = phi.map(f64::from);
|
||||
let expected = exp(phi64);
|
||||
let rotation = Vector3::<Frd>::new(phi[0], phi[1], phi[2]);
|
||||
let actual = UnitQuaternion::from_small_rotation_vector(rotation).unwrap();
|
||||
for (actual, expected) in [actual.w(), actual.x(), actual.y(), actual.z()]
|
||||
.into_iter()
|
||||
.zip(expected)
|
||||
{
|
||||
// Below two f32 ulps, including the polynomial endpoint error.
|
||||
assert!((f64::from(actual) - expected).abs() < 1.5e-7, "{phi:?}");
|
||||
}
|
||||
let inverse = [expected[0], -expected[1], -expected[2], -expected[3]];
|
||||
for axis in 0..3 {
|
||||
let mut basis = [0.0; 3];
|
||||
basis[axis] = 1.0;
|
||||
let basis = Vector3::new(basis[0], basis[1], basis[2]);
|
||||
let mut plus = phi64;
|
||||
let mut minus = phi64;
|
||||
const STEP: f64 = 1.0e-6;
|
||||
plus[axis] += STEP;
|
||||
minus[axis] -= STEP;
|
||||
for (actual, positive, negative) in [
|
||||
(
|
||||
left_jacobian_apply(rotation, basis).unwrap(),
|
||||
multiply(exp(plus), inverse),
|
||||
multiply(exp(minus), inverse),
|
||||
),
|
||||
(
|
||||
right_jacobian_apply(rotation, basis).unwrap(),
|
||||
multiply(inverse, exp(plus)),
|
||||
multiply(inverse, exp(minus)),
|
||||
),
|
||||
] {
|
||||
for (row, value) in [actual.x(), actual.y(), actual.z()].into_iter().enumerate()
|
||||
{
|
||||
// The infinitesimal rotation is twice quaternion xyz.
|
||||
let reference = (positive[row + 1] - negative[row + 1]) / STEP;
|
||||
assert!(
|
||||
(f64::from(value) - reference).abs() < 2.5e-6,
|
||||
"{phi:?}, axis {axis}, row {row}"
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn normalization_is_scale_invariant_across_f32_exponents() {
|
||||
for scale in [
|
||||
f32::from_bits(1),
|
||||
f32::MIN_POSITIVE,
|
||||
1.0e-30,
|
||||
1.0e-9,
|
||||
1.0,
|
||||
1.0e30,
|
||||
f32::MAX,
|
||||
] {
|
||||
let vector = Vector3::<Frd>::new(scale, -scale, scale);
|
||||
let normalized = vector.normalized().unwrap();
|
||||
for component in [normalized.x(), -normalized.y(), normalized.z()] {
|
||||
assert!((f64::from(component) - 1.0 / 3.0_f64.sqrt()).abs() < 1.0e-7);
|
||||
}
|
||||
let quaternion =
|
||||
UnitQuaternion::<Frd, Frd>::from_components(scale, scale, -scale, -scale).unwrap();
|
||||
assert_eq!(
|
||||
[
|
||||
quaternion.w(),
|
||||
quaternion.x(),
|
||||
quaternion.y(),
|
||||
quaternion.z()
|
||||
],
|
||||
[0.5, 0.5, -0.5, -0.5]
|
||||
);
|
||||
assert_eq!(Vector3::<Frd>::new(scale, 0.0, 0.0).norm(), scale);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn norm_distinguishes_zero_invalid_input_and_unrepresentable_magnitude() {
|
||||
assert_eq!(Vector3::<Frd>::zero().norm(), 0.0);
|
||||
for invalid in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY] {
|
||||
assert!(Vector3::<Frd>::new(1.0, invalid, 2.0).norm().is_nan());
|
||||
}
|
||||
let large = Vector3::<Frd>::new(f32::MAX, f32::MAX, f32::MAX);
|
||||
assert_eq!(large.norm(), f32::INFINITY);
|
||||
// The direction is still representable when the magnitude is not.
|
||||
assert!((f64::from(large.normalized().unwrap().norm()) - 1.0).abs() < 2.0e-7);
|
||||
}
|
||||
Reference in new issue
Block a user