kaptan/firmware/libs/kaptan-math/tests/so3_reference.rs

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);
}