oxiproj-transformations 0.1.2

//! Covariance/uncertainty propagation through datum transforms.
//!
//! Implements Σ_out = J·Σ_in·Jᵀ where J is the transform Jacobian.

use oxiproj_core::CovMatrix2x2;

/// Propagate 2D coordinate covariance through a 2D affine transform.
///
/// The transform is: x' = a00*x + a01*y + b0, y' = a10*x + a11*y + b1
/// The Jacobian is [[a00, a01], [a10, a11]].
pub fn propagate_through_affine_2d(
    sigma: &CovMatrix2x2,
    a00: f64,
    a01: f64,
    a10: f64,
    a11: f64,
) -> CovMatrix2x2 {
    sigma.propagate(a00, a01, a10, a11)
}

/// Propagate 2D (XY) coordinate covariance through a 7-parameter Helmert transform.
///
/// Uses only the XY block of the full 3D rotation matrix:
/// R_xy = (1+scale) * [[1, -rot_z], [rot_z, 1]]
/// (rot_x, rot_y affect Z only to first order in 2D projection)
pub fn propagate_through_helmert_7_xy(
    sigma: &CovMatrix2x2,
    rot_z: f64,
    scale: f64,
) -> CovMatrix2x2 {
    let s1 = 1.0 + scale;
    sigma.propagate(s1, -rot_z * s1, rot_z * s1, s1)
}

/// Propagate 2D coordinate covariance through a 3-parameter (translation-only) Helmert.
///
/// Translations don't change the uncertainty; returns sigma unchanged.
pub fn propagate_through_helmert_3(sigma: &CovMatrix2x2) -> CovMatrix2x2 {
    // Jacobian is identity: Σ_out = I·Σ_in·Iᵀ = Σ_in
    *sigma
}

#[cfg(test)]
mod tests {
    use super::*;
    use oxiproj_core::CovMatrix2x2;

    const EPS: f64 = 1e-12;

    fn approx_eq(a: f64, b: f64, eps: f64) -> bool {
        (a - b).abs() < eps
    }

    fn cov_approx_eq(a: &CovMatrix2x2, b: &CovMatrix2x2, eps: f64) -> bool {
        approx_eq(a.var_x, b.var_x, eps)
            && approx_eq(a.var_y, b.var_y, eps)
            && approx_eq(a.cov_xy, b.cov_xy, eps)
    }

    /// Affine identity: a00=1, a01=0, a10=0, a11=1 → Σ_out == Σ_in
    #[test]
    fn affine_identity_preserves_covariance() {
        let sigma = CovMatrix2x2::new(4.0, 9.0, 2.0);
        let out = propagate_through_affine_2d(&sigma, 1.0, 0.0, 0.0, 1.0);
        assert!(
            cov_approx_eq(&out, &sigma, EPS),
            "Identity affine should preserve covariance: got {:?}, want {:?}",
            out,
            sigma
        );
    }

    /// Affine rotation by 90°: diagonal input → diagonal output with swapped variances
    /// Rotation by 90°: x' = -y, y' = x → Jacobian [[0,-1],[1,0]]
    #[test]
    fn affine_rotation_90_swaps_variances() {
        let sigma = CovMatrix2x2::diagonal(4.0, 9.0);
        // Rotation by 90 deg: x' = -y => a00=0, a01=-1; y' = x => a10=1, a11=0
        let out = propagate_through_affine_2d(&sigma, 0.0, -1.0, 1.0, 0.0);
        // Expected: var_x_out = var_y_in = 9, var_y_out = var_x_in = 4, cov_xy_out = 0
        let expected = CovMatrix2x2::diagonal(9.0, 4.0);
        assert!(
            cov_approx_eq(&out, &expected, EPS),
            "90° rotation should swap diagonal variances: got {:?}, want {:?}",
            out,
            expected
        );
    }

    /// Helmert-3: Σ_out == Σ_in (translations don't affect uncertainty)
    #[test]
    fn helmert_3_preserves_covariance() {
        let sigma = CovMatrix2x2::new(4.0, 9.0, 2.0);
        let out = propagate_through_helmert_3(&sigma);
        assert!(
            cov_approx_eq(&out, &sigma, EPS),
            "Translation-only Helmert should preserve covariance: got {:?}, want {:?}",
            out,
            sigma
        );
    }

    /// Helmert-7 with rot_z=1e-5 rad and scale=0: output is PSD
    #[test]
    fn helmert_7_typical_rotation_is_psd() {
        let sigma = CovMatrix2x2::new(4.0, 9.0, 1.0);
        let rot_z = 1e-5_f64; // ~2 arc-second rotation
        let scale = 0.0;
        let out = propagate_through_helmert_7_xy(&sigma, rot_z, scale);
        assert!(
            out.is_psd(),
            "Output covariance should be PSD for typical Helmert rotation: {:?}",
            out
        );
    }

    /// Comparison with full propagation formula: compute J·Σ·Jᵀ by hand
    /// for a specific affine case and verify.
    ///
    /// J = [[2, 1], [0, 3]], Σ = [[4, 1], [1, 9]]
    /// J·Σ = [[2*4+1*1, 2*1+1*9], [0*4+3*1, 0*1+3*9]] = [[9, 11], [3, 27]]
    /// J·Σ·Jᵀ = [[9*2+11*1, 9*0+11*3], [3*2+27*1, 3*0+27*3]]
    ///         = [[18+11, 0+33], [6+27, 0+81]]
    ///         = [[29, 33], [33, 81]]
    /// So: var_x=29, var_y=81, cov_xy=33
    #[test]
    fn affine_manual_propagation_matches() {
        let sigma = CovMatrix2x2::new(4.0, 9.0, 1.0);
        let out = propagate_through_affine_2d(&sigma, 2.0, 1.0, 0.0, 3.0);
        assert!(
            approx_eq(out.var_x, 29.0, EPS),
            "var_x should be 29, got {}",
            out.var_x
        );
        assert!(
            approx_eq(out.var_y, 81.0, EPS),
            "var_y should be 81, got {}",
            out.var_y
        );
        assert!(
            approx_eq(out.cov_xy, 33.0, EPS),
            "cov_xy should be 33, got {}",
            out.cov_xy
        );
    }

    /// Helmert-7 identity (rot_z=0, scale=0) should preserve covariance exactly.
    #[test]
    fn helmert_7_identity_preserves_covariance() {
        let sigma = CovMatrix2x2::new(4.0, 9.0, 2.0);
        let out = propagate_through_helmert_7_xy(&sigma, 0.0, 0.0);
        assert!(
            cov_approx_eq(&out, &sigma, EPS),
            "Helmert-7 identity should preserve covariance: got {:?}, want {:?}",
            out,
            sigma
        );
    }
}