astrodynamics 0.12.0

//! Position-covariance modeling for conjunction and orbit analysis.
//!
//! Owns the authoritative RTN->ECI frame transform of a 3x3 position
//! covariance and the symmetric positive-semidefinite (PSD) validation used to
//! reject ill-formed covariances. The orbis Elixir binding is a thin
//! marshaling and structural-validation layer over this module; no frame or
//! PSD formula lives there.
//!
//! The covariance is transformed but never rescaled here, so it carries the
//! squared units of whatever position vectors it was formed from.

use crate::math::mat3::{self, Mat3};
use crate::math::vec3;

/// Position magnitudes below this are treated as a degenerate (zero) position
/// vector, for which the RTN frame is undefined.
const ZERO_POSITION_EPS: f64 = 1.0e-30;
/// Orbit-normal magnitudes below this mean position and velocity are parallel,
/// so the RTN frame normal (and thus the frame) is undefined.
const PARALLEL_RV_EPS: f64 = 1.0e-30;
/// Diagonal covariance entries are allowed to dip to this (negative) bound
/// before the PSD check rejects them, absorbing float round-off.
const PSD_DIAGONAL_EPS: f64 = 1.0e-15;
/// Second- and third-order principal minors are allowed to dip to this
/// (negative) bound before the PSD check rejects them.
const PSD_MINOR_EPS: f64 = 1.0e-12;
/// Off-diagonal pairs differing by more than this are treated as asymmetric.
const SYMMETRY_EPS: f64 = 1.0e-12;

/// Reason an RTN->ECI transform could not be built from an orbit state.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum RtnFrameError {
    /// The position vector is effectively zero.
    ZeroPosition,
    /// Position and velocity are parallel, leaving the orbit normal undefined.
    ParallelPositionVelocity,
}

impl RtnFrameError {
    /// Message string matching the historical orbis error verbatim, so the
    /// thin Elixir binding preserves its public `{:error, reason}` shapes.
    pub fn message(self) -> &'static str {
        match self {
            RtnFrameError::ZeroPosition => "zero position vector",
            RtnFrameError::ParallelPositionVelocity => "position and velocity are parallel",
        }
    }
}

/// Build the RTN->ECI rotation whose columns are the radial, transverse, and
/// normal unit vectors of the orbit state `(r, v)`.
///
/// Operation order (magnitude before normalize, division not reciprocal
/// multiply, cross-product component order) is fixed to reproduce the prior
/// Elixir reference bit-for-bit.
fn rtn_to_eci_rotation(r: [f64; 3], v: [f64; 3]) -> Result<Mat3, RtnFrameError> {
    if vec3::norm3(r) < ZERO_POSITION_EPS {
        return Err(RtnFrameError::ZeroPosition);
    }
    let r_hat = vec3::unit3_ref_unchecked(&r);
    let h = vec3::cross3(r, v);
    if vec3::norm3(h) < PARALLEL_RV_EPS {
        return Err(RtnFrameError::ParallelPositionVelocity);
    }
    let n_hat = vec3::unit3_ref_unchecked(&h);
    let t_hat = vec3::cross3(n_hat, r_hat);
    Ok([
        [r_hat[0], t_hat[0], n_hat[0]],
        [r_hat[1], t_hat[1], n_hat[1]],
        [r_hat[2], t_hat[2], n_hat[2]],
    ])
}

/// Transform a 3x3 RTN position covariance to ECI: `C_eci = R * C_rtn * R^T`.
///
/// The triple product materialises the intermediate `R * C_rtn` and applies
/// `R^T` in a second multiply (left-to-right `k` summation), matching the
/// chained Elixir `mat_mul` reduction order rather than a fused Kahan product.
pub fn rtn_to_eci(cov_rtn: &Mat3, r: [f64; 3], v: [f64; 3]) -> Result<Mat3, RtnFrameError> {
    let rot = rtn_to_eci_rotation(r, v)?;
    let rot_t = mat3::inline_tr(&rot);
    Ok(mat3::inline_rxr(&mat3::inline_rxr(&rot, cov_rtn), &rot_t))
}

/// Whether a 3x3 matrix is symmetric within [`SYMMETRY_EPS`].
pub fn symmetric(m: &Mat3) -> bool {
    (m[0][1] - m[1][0]).abs() < SYMMETRY_EPS
        && (m[0][2] - m[2][0]).abs() < SYMMETRY_EPS
        && (m[1][2] - m[2][1]).abs() < SYMMETRY_EPS
}

/// Determinant of a 3x3 matrix via cofactor expansion along the first row,
/// matching the Elixir reference operation order.
fn det3x3(m: &Mat3) -> f64 {
    let (a, b, c) = (m[0][0], m[0][1], m[0][2]);
    let (d, e, f) = (m[1][0], m[1][1], m[1][2]);
    let (g, h, i) = (m[2][0], m[2][1], m[2][2]);
    a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g)
}

/// Whether a symmetric 3x3 matrix is positive semidefinite by Sylvester's
/// criterion: every leading-and-trailing principal minor is non-negative
/// within tolerance. A non-symmetric matrix is rejected.
pub fn positive_semidefinite(m: &Mat3) -> bool {
    if !symmetric(m) {
        return false;
    }

    let m11 = m[0][0];
    let m22 = m[1][1];
    let m33 = m[2][2];
    let m12 = m[0][1];
    let m13 = m[0][2];
    let m23 = m[1][2];

    let det12 = m11 * m22 - m12 * m12;
    let det13 = m11 * m33 - m13 * m13;
    let det23 = m22 * m33 - m23 * m23;
    let det123 = det3x3(m);

    m11 >= -PSD_DIAGONAL_EPS
        && m22 >= -PSD_DIAGONAL_EPS
        && m33 >= -PSD_DIAGONAL_EPS
        && det12 >= -PSD_MINOR_EPS
        && det13 >= -PSD_MINOR_EPS
        && det23 >= -PSD_MINOR_EPS
        && det123 >= -PSD_MINOR_EPS
}

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

    /// Frozen ECI bits from the prior Elixir `Orbis.Covariance.rtn_to_eci`
    /// reference for `r = (7000.123, 1234.5, -250.7)`,
    /// `v = (1.2, 7.4, 0.3)`, and the non-diagonal RTN covariance below.
    /// Row-major; proves cross-language 0-ULP parity, including the last-ULP
    /// off-diagonal asymmetry the chained multiply produces.
    const RTN_TO_ECI_GOLDEN_BITS: [u64; 9] = [
        0x4010077f74cce7ac,
        0xbfd92b0043adb450,
        0x3fe26dc422b0767a,
        0xbfd92b0043adb44a,
        0x402207fb1ad4c218,
        0xbfb9ef5fd1874930,
        0x3fe26dc422b0767a,
        0xbfb9ef5fd1874930,
        0x402ff4452ac4ca0f,
    ];

    #[test]
    fn rtn_to_eci_matches_frozen_elixir_bits() {
        let r = [7000.123, 1234.5, -250.7];
        let v = [1.2, 7.4, 0.3];
        let cov_rtn = [[4.0, 0.5, 0.1], [0.5, 9.0, 0.2], [0.1, 0.2, 16.0]];

        let eci = rtn_to_eci(&cov_rtn, r, v).expect("non-degenerate state");

        let mut flat = [0u64; 9];
        for (idx, slot) in flat.iter_mut().enumerate() {
            *slot = eci[idx / 3][idx % 3].to_bits();
        }
        assert_eq!(flat, RTN_TO_ECI_GOLDEN_BITS);
    }

    #[test]
    fn rtn_to_eci_aligned_state_is_exactly_the_rtn_diagonal() {
        // r along +X, v along +Y -> RTN axes coincide with ECI, so the
        // transform is the identity and the diagonal is reproduced exactly.
        let r = [7000.0, 0.0, 0.0];
        let v = [0.0, 7.5, 0.0];
        let cov_rtn = [[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]];

        let eci = rtn_to_eci(&cov_rtn, r, v).expect("non-degenerate state");

        assert_eq!(eci[0][0].to_bits(), 1.0_f64.to_bits());
        assert_eq!(eci[1][1].to_bits(), 2.0_f64.to_bits());
        assert_eq!(eci[2][2].to_bits(), 3.0_f64.to_bits());
    }

    #[test]
    fn rtn_to_eci_rejects_zero_position() {
        let err = rtn_to_eci(&identity(), [0.0, 0.0, 0.0], [0.0, 7.5, 0.0]).unwrap_err();
        assert_eq!(err, RtnFrameError::ZeroPosition);
        assert_eq!(err.message(), "zero position vector");
    }

    #[test]
    fn rtn_to_eci_rejects_parallel_position_velocity() {
        let err = rtn_to_eci(&identity(), [7000.0, 0.0, 0.0], [1.0, 0.0, 0.0]).unwrap_err();
        assert_eq!(err, RtnFrameError::ParallelPositionVelocity);
        assert_eq!(err.message(), "position and velocity are parallel");
    }

    #[test]
    fn positive_semidefinite_accepts_identity_rejects_negative_and_asymmetric() {
        assert!(positive_semidefinite(&identity()));

        let negative_diag = [[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
        assert!(!positive_semidefinite(&negative_diag));

        let asymmetric = [[1.0, 0.5, 0.0], [0.4, 1.0, 0.0], [0.0, 0.0, 1.0]];
        assert!(!symmetric(&asymmetric));
        assert!(!positive_semidefinite(&asymmetric));
    }

    #[test]
    fn positive_semidefinite_rejects_symmetric_indefinite_matrix() {
        // Symmetric but the 2x2 minor m11*m22 - m12^2 = 1 - 4 < 0.
        let indefinite = [[1.0, 2.0, 0.0], [2.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
        assert!(symmetric(&indefinite));
        assert!(!positive_semidefinite(&indefinite));
    }

    fn identity() -> Mat3 {
        [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
    }
}