rustial_engine/math/

geodesic.rs

//! Geodesic calculations on an ellipsoid using Vincenty formulae.
//!
//! This module solves the two classical geodesic problems on an oblate
//! ellipsoid:
//!
//! | Problem | Function | Description |
//! |---------|----------|-------------|
//! | **Inverse** | [`geodesic_distance`] | Distance and azimuths between two points. |
//! | **Direct** | [`geodesic_destination`] | Destination given start, azimuth, distance. |
//!
//! The default helpers operate on WGS-84; the `_on` variants accept any
//! [`Ellipsoid`].
//!
//! # Algorithm
//!
//! Both solvers implement the iterative Vincenty formulae as described in:
//!
//! > T. Vincenty, "Direct and Inverse Solutions of Geodesics on the Ellipsoid
//! > with Application of Nested Equations", *Survey Review* **23**(176),
//! > April 1975, pp. 88–93.
//!
//! # Numerical behavior
//!
//! - **Accuracy:** sub-millimeter for all well-conditioned pairs on WGS-84.
//! - **Convergence:** up to [`MAX_ITERATIONS`] (200) iterations with a
//!   tolerance of 1 × 10⁻¹² radians (~0.006 mm on Earth).
//! - **Near-antipodal failure:** Vincenty can fail to converge when the two
//!   points are nearly diametrically opposite.  In those cases a
//!   [`VincentyConvergenceError`] is returned instead of panicking.
//!   Callers needing guaranteed convergence for arbitrary inputs should
//!   consider a Karney-class algorithm as a future enhancement.
//! - **Coincident points:** detected early (sin σ < 10⁻¹⁵) and return
//!   distance = 0, azimuths = 0.
//! - **Equatorial paths:** the cos²α ≈ 0 guard prevents division-by-zero
//!   when both points lie on the equator.
//!
//! # Altitude
//!
//! Altitude (`alt`) is **not** used in the geodesic computation.  The direct
//! solver copies the start point's altitude to the destination unchanged.

use crate::coord::GeoCoord;
use crate::ellipsoid::Ellipsoid;
use std::f64::consts::PI;
use thiserror::Error;

// ---------------------------------------------------------------------------
// Error and result types
// ---------------------------------------------------------------------------

/// Error returned when a geodesic computation fails to converge.
///
/// This typically occurs for near-antipodal point pairs where the Vincenty
/// iteration oscillates without settling.
#[derive(Debug, Clone, PartialEq, Eq, Error)]
#[error("Vincenty iteration did not converge after {0} iterations (near-antipodal points)")]
pub struct VincentyConvergenceError(u32);

impl VincentyConvergenceError {
    /// Number of iterations that were attempted before giving up.
    #[inline]
    pub fn iterations(&self) -> u32 {
        self.0
    }
}

/// Result of the inverse geodesic problem.
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct GeodesicResult {
    /// Distance between the two points in meters.
    pub distance: f64,
    /// Forward azimuth at the start point, in radians (0 = north, clockwise).
    pub azimuth_start: f64,
    /// Forward azimuth at the end point, in radians.
    pub azimuth_end: f64,
}

// ---------------------------------------------------------------------------
// Iteration parameters
// ---------------------------------------------------------------------------

/// Maximum Vincenty iterations before declaring non-convergence.
const MAX_ITERATIONS: u32 = 200;

/// Convergence threshold in radians (~0.006 mm on Earth's surface).
const CONVERGENCE: f64 = 1e-12;

// ---------------------------------------------------------------------------
// Longitude normalization helpers
// ---------------------------------------------------------------------------

/// Normalize a radian delta-longitude into `[-π, π]`.
///
/// Ensures the inverse solve always follows the shortest (antimeridian-aware)
/// great-elliptic path between the two points.
#[inline]
fn normalize_delta_lon_radians(mut lambda: f64) -> f64 {
    while lambda > PI {
        lambda -= 2.0 * PI;
    }
    while lambda < -PI {
        lambda += 2.0 * PI;
    }
    lambda
}

/// Wrap a degree longitude into `[-180, 180]`.
///
/// Used by the direct solver to keep the output coordinate in the canonical
/// geographic range.
#[inline]
fn wrap_lon_degrees(mut lon: f64) -> f64 {
    while lon > 180.0 {
        lon -= 360.0;
    }
    while lon < -180.0 {
        lon += 360.0;
    }
    lon
}

// ---------------------------------------------------------------------------
// Inverse problem (distance + azimuths)
// ---------------------------------------------------------------------------

/// Compute the geodesic distance and azimuths between two geographic
/// coordinates on the WGS-84 ellipsoid using the Vincenty inverse formula.
///
/// Returns [`Err`] only for near-antipodal pairs where the iteration
/// fails to converge.
pub fn geodesic_distance(
    from: &GeoCoord,
    to: &GeoCoord,
) -> Result<GeodesicResult, VincentyConvergenceError> {
    geodesic_distance_on(from, to, &Ellipsoid::WGS84)
}

/// Compute the geodesic distance on an arbitrary ellipsoid.
///
/// See [`geodesic_distance`] for behavioral details.
pub fn geodesic_distance_on(
    from: &GeoCoord,
    to: &GeoCoord,
    ellipsoid: &Ellipsoid,
) -> Result<GeodesicResult, VincentyConvergenceError> {
    let a = ellipsoid.a;
    let f = ellipsoid.f;
    let b = ellipsoid.b;

    // --- Reduced latitudes (parametric latitude on the auxiliary sphere) ---
    let u1 = ((1.0 - f) * from.lat.to_radians().tan()).atan();
    let u2 = ((1.0 - f) * to.lat.to_radians().tan()).atan();
    let (sin_u1, cos_u1) = u1.sin_cos();
    let (sin_u2, cos_u2) = u2.sin_cos();

    // Difference in longitude on the auxiliary sphere, normalized to [-π, π].
    let l = normalize_delta_lon_radians((to.lon - from.lon).to_radians());
    let mut lambda = l;

    let mut sin_sigma;
    let mut cos_sigma;
    let mut sigma;
    let mut sin_alpha;
    let mut cos2_alpha;
    let mut cos_2sigma_m;

    // --- Vincenty iteration ---
    for i in 0..MAX_ITERATIONS {
        let (sin_lambda, cos_lambda) = lambda.sin_cos();

        // Eq. (14): sin σ
        sin_sigma = ((cos_u2 * sin_lambda).powi(2)
            + (cos_u1 * sin_u2 - sin_u1 * cos_u2 * cos_lambda).powi(2))
        .sqrt();

        // Coincident-point fast path.
        if sin_sigma < 1e-15 {
            return Ok(GeodesicResult {
                distance: 0.0,
                azimuth_start: 0.0,
                azimuth_end: 0.0,
            });
        }

        // Eq. (15): cos σ
        cos_sigma = sin_u1 * sin_u2 + cos_u1 * cos_u2 * cos_lambda;
        // Eq. (16): σ
        sigma = sin_sigma.atan2(cos_sigma);

        // Eq. (17): sin α  (α = azimuth of the geodesic at the equator)
        sin_alpha = cos_u1 * cos_u2 * sin_lambda / sin_sigma;
        cos2_alpha = 1.0 - sin_alpha * sin_alpha;

        // Eq. (18): cos 2σ_m — guarded for equatorial paths (cos²α ≈ 0).
        cos_2sigma_m = if cos2_alpha.abs() < 1e-15 {
            0.0
        } else {
            cos_sigma - 2.0 * sin_u1 * sin_u2 / cos2_alpha
        };

        // Eq. (10): C
        let c = f / 16.0 * cos2_alpha * (4.0 + f * (4.0 - 3.0 * cos2_alpha));

        // Eq. (11): updated λ
        let lambda_prev = lambda;
        lambda = l
            + (1.0 - c)
                * f
                * sin_alpha
                * (sigma
                    + c * sin_sigma
                        * (cos_2sigma_m
                            + c * cos_sigma * (-1.0 + 2.0 * cos_2sigma_m * cos_2sigma_m)));

        if (lambda - lambda_prev).abs() < CONVERGENCE {
            // --- Converged: compute final distance and azimuths ---

            // Eq. (3): u²
            let u_sq = cos2_alpha * (a * a - b * b) / (b * b);
            // Eq. (3): A
            let cap_a =
                1.0 + u_sq / 16384.0 * (4096.0 + u_sq * (-768.0 + u_sq * (320.0 - 175.0 * u_sq)));
            // Eq. (4): B
            let cap_b = u_sq / 1024.0 * (256.0 + u_sq * (-128.0 + u_sq * (74.0 - 47.0 * u_sq)));

            // Eq. (6): Δσ
            let delta_sigma = cap_b
                * sin_sigma
                * (cos_2sigma_m
                    + cap_b / 4.0
                        * (cos_sigma * (-1.0 + 2.0 * cos_2sigma_m * cos_2sigma_m)
                            - cap_b / 6.0
                                * cos_2sigma_m
                                * (-3.0 + 4.0 * sin_sigma * sin_sigma)
                                * (-3.0 + 4.0 * cos_2sigma_m * cos_2sigma_m)));

            // Eq. (19): geodesic distance
            let distance = b * cap_a * (sigma - delta_sigma);

            // Eq. (20) & (21): forward azimuths, normalized to [0, 2π).
            let (sin_lam, cos_lam) = lambda.sin_cos();
            let az_start = (cos_u2 * sin_lam).atan2(cos_u1 * sin_u2 - sin_u1 * cos_u2 * cos_lam);
            let az_end = (cos_u1 * sin_lam).atan2(-sin_u1 * cos_u2 + cos_u1 * sin_u2 * cos_lam);

            return Ok(GeodesicResult {
                distance,
                azimuth_start: (az_start + 2.0 * PI) % (2.0 * PI),
                azimuth_end: (az_end + 2.0 * PI) % (2.0 * PI),
            });
        }

        if i == MAX_ITERATIONS - 1 {
            return Err(VincentyConvergenceError(MAX_ITERATIONS));
        }
    }

    Err(VincentyConvergenceError(MAX_ITERATIONS))
}

// ---------------------------------------------------------------------------
// Direct problem (destination from azimuth + distance)
// ---------------------------------------------------------------------------

/// Solve the direct geodesic problem: given a start point, azimuth (radians,
/// 0 = north, clockwise), and distance (meters), compute the destination
/// point on the WGS-84 ellipsoid.
///
/// Returns [`Err`] only if the iteration fails to converge (extremely rare
/// for the direct problem).
pub fn geodesic_destination(
    from: &GeoCoord,
    azimuth: f64,
    distance: f64,
) -> Result<GeoCoord, VincentyConvergenceError> {
    geodesic_destination_on(from, azimuth, distance, &Ellipsoid::WGS84)
}

/// Solve the direct geodesic problem on an arbitrary ellipsoid.
///
/// See [`geodesic_destination`] for behavioral details.
pub fn geodesic_destination_on(
    from: &GeoCoord,
    azimuth: f64,
    distance: f64,
    ellipsoid: &Ellipsoid,
) -> Result<GeoCoord, VincentyConvergenceError> {
    // Zero-distance fast path: return the start point unchanged.
    if distance.abs() < 1e-15 {
        return Ok(*from);
    }

    let a = ellipsoid.a;
    let f = ellipsoid.f;
    let b = ellipsoid.b;

    let (sin_az, cos_az) = azimuth.sin_cos();
    let tan_u1 = (1.0 - f) * from.lat.to_radians().tan();
    let cos_u1 = 1.0 / (1.0 + tan_u1 * tan_u1).sqrt();
    let sin_u1 = tan_u1 * cos_u1;

    // σ₁ = angular distance on the sphere from the equator to the start.
    let sigma1 = tan_u1.atan2(cos_az);
    let sin_alpha = cos_u1 * sin_az;
    let cos2_alpha = 1.0 - sin_alpha * sin_alpha;

    // u² and series coefficients A, B (same as inverse).
    let u_sq = cos2_alpha * (a * a - b * b) / (b * b);
    let cap_a = 1.0 + u_sq / 16384.0 * (4096.0 + u_sq * (-768.0 + u_sq * (320.0 - 175.0 * u_sq)));
    let cap_b = u_sq / 1024.0 * (256.0 + u_sq * (-128.0 + u_sq * (74.0 - 47.0 * u_sq)));

    // Initial estimate of σ (angular distance on the auxiliary sphere).
    let mut sigma = distance / (b * cap_a);

    // --- Vincenty iteration for σ ---
    for i in 0..MAX_ITERATIONS {
        let cos_2sigma_m = (2.0 * sigma1 + sigma).cos();
        let sin_sigma = sigma.sin();
        let cos_sigma = sigma.cos();

        let delta_sigma = cap_b
            * sin_sigma
            * (cos_2sigma_m
                + cap_b / 4.0
                    * (cos_sigma * (-1.0 + 2.0 * cos_2sigma_m * cos_2sigma_m)
                        - cap_b / 6.0
                            * cos_2sigma_m
                            * (-3.0 + 4.0 * sin_sigma * sin_sigma)
                            * (-3.0 + 4.0 * cos_2sigma_m * cos_2sigma_m)));

        let sigma_prev = sigma;
        sigma = distance / (b * cap_a) + delta_sigma;

        if (sigma - sigma_prev).abs() < CONVERGENCE {
            // --- Converged: compute destination lat/lon ---
            let sin_sigma = sigma.sin();
            let cos_sigma = sigma.cos();
            let cos_2sigma_m = (2.0 * sigma1 + sigma).cos();

            let lat2 = (sin_u1 * cos_sigma + cos_u1 * sin_sigma * cos_az).atan2(
                (1.0 - f)
                    * (sin_alpha * sin_alpha
                        + (sin_u1 * sin_sigma - cos_u1 * cos_sigma * cos_az).powi(2))
                    .sqrt(),
            );

            let lambda =
                (sin_sigma * sin_az).atan2(cos_u1 * cos_sigma - sin_u1 * sin_sigma * cos_az);
            let c = f / 16.0 * cos2_alpha * (4.0 + f * (4.0 - 3.0 * cos2_alpha));
            let l = lambda
                - (1.0 - c)
                    * f
                    * sin_alpha
                    * (sigma
                        + c * sin_sigma
                            * (cos_2sigma_m
                                + c * cos_sigma * (-1.0 + 2.0 * cos_2sigma_m * cos_2sigma_m)));

            let lon2 = wrap_lon_degrees((from.lon.to_radians() + l).to_degrees());

            return Ok(GeoCoord::new(lat2.to_degrees(), lon2, from.alt));
        }

        if i == MAX_ITERATIONS - 1 {
            return Err(VincentyConvergenceError(MAX_ITERATIONS));
        }
    }

    Err(VincentyConvergenceError(MAX_ITERATIONS))
}

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

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

    // -- Inverse: basic cases ---------------------------------------------

    #[test]
    fn same_point_zero_distance() {
        let p = GeoCoord::from_lat_lon(45.0, 10.0);
        let result = geodesic_distance(&p, &p).unwrap();
        assert!(result.distance < 1e-6);
    }

    #[test]
    fn known_distance_london_paris() {
        // London (51.5074 N, 0.1278 W) to Paris (48.8566 N, 2.3522 E)
        // Vincenty gives ~343.5 km.
        let london = GeoCoord::from_lat_lon(51.5074, -0.1278);
        let paris = GeoCoord::from_lat_lon(48.8566, 2.3522);
        let result = geodesic_distance(&london, &paris).unwrap();
        assert!((result.distance - 343_500.0).abs() < 1500.0);
    }

    #[test]
    fn equatorial_points() {
        // 1 degree along the equator ~ 111,319 m.
        let a = GeoCoord::from_lat_lon(0.0, 0.0);
        let b = GeoCoord::from_lat_lon(0.0, 1.0);
        let result = geodesic_distance(&a, &b).unwrap();
        assert!((result.distance - 111_319.0).abs() < 100.0);
    }

    // -- Inverse: symmetry ------------------------------------------------

    #[test]
    fn inverse_is_symmetric() {
        let a = GeoCoord::from_lat_lon(40.0, -74.0);
        let b = GeoCoord::from_lat_lon(51.5, -0.1);
        let ab = geodesic_distance(&a, &b).unwrap();
        let ba = geodesic_distance(&b, &a).unwrap();
        assert!((ab.distance - ba.distance).abs() < 1e-6);
    }

    // -- Inverse: antimeridian --------------------------------------------

    #[test]
    fn inverse_crosses_antimeridian_short_path() {
        let a = GeoCoord::from_lat_lon(10.0, 179.0);
        let b = GeoCoord::from_lat_lon(10.0, -179.0);
        let result = geodesic_distance(&a, &b).unwrap();
        // Should follow the short dateline crossing (~220 km), not almost full globe.
        assert!(result.distance > 100_000.0);
        assert!(result.distance < 500_000.0);
    }

    // -- Inverse: near-antipodal convergence failure ----------------------

    #[test]
    fn near_antipodal_returns_error() {
        let a = GeoCoord::from_lat_lon(0.0, 0.0);
        let b = GeoCoord::from_lat_lon(0.5, 179.7);
        // Near-antipodal on the equator: Vincenty may fail to converge.
        let result = geodesic_distance(&a, &b);
        // We accept either a converged result or a convergence error --
        // the important contract is that we never panic.
        if let Ok(r) = result {
            assert!(r.distance > 1e7);
        }
    }

    // -- Direct: basic cases ----------------------------------------------

    #[test]
    fn direct_roundtrip() {
        let start = GeoCoord::from_lat_lon(40.0, -74.0);
        let azimuth = 0.8; // ~45.8 degrees
        let dist = 500_000.0; // 500 km

        let dest = geodesic_destination(&start, azimuth, dist).unwrap();
        let inv = geodesic_distance(&start, &dest).unwrap();
        assert!((inv.distance - dist).abs() < 0.01);
    }

    #[test]
    fn direct_due_north() {
        let start = GeoCoord::from_lat_lon(0.0, 0.0);
        let dest = geodesic_destination(&start, 0.0, 1_000_000.0).unwrap();
        // Should be roughly 9.04 degrees north.
        assert!(dest.lat > 8.0 && dest.lat < 10.0);
        assert!(dest.lon.abs() < 0.001);
    }

    // -- Direct: zero distance --------------------------------------------

    #[test]
    fn direct_zero_distance_returns_start() {
        let start = GeoCoord::from_lat_lon(35.0, 139.0);
        let dest = geodesic_destination(&start, 1.23, 0.0).unwrap();
        assert!((dest.lat - start.lat).abs() < 1e-12);
        assert!((dest.lon - start.lon).abs() < 1e-12);
    }

    // -- Direct: longitude wrapping ---------------------------------------

    #[test]
    fn direct_wraps_longitude_range() {
        let start = GeoCoord::from_lat_lon(0.0, 179.8);
        let dest = geodesic_destination(&start, PI / 2.0, 100_000.0).unwrap();
        assert!((-180.0..=180.0).contains(&dest.lon));
    }

    // -- Altitude passthrough ---------------------------------------------

    #[test]
    fn altitude_passthrough_inverse() {
        let a = GeoCoord::new(0.0, 0.0, 1234.5);
        let b = GeoCoord::new(1.0, 1.0, 9999.0);
        let result = geodesic_distance(&a, &b).unwrap();
        // Altitude must not affect the distance calculation.
        let a0 = GeoCoord::from_lat_lon(0.0, 0.0);
        let b0 = GeoCoord::from_lat_lon(1.0, 1.0);
        let result0 = geodesic_distance(&a0, &b0).unwrap();
        assert!((result.distance - result0.distance).abs() < 1e-6);
    }

    #[test]
    fn altitude_passthrough_direct() {
        let start = GeoCoord::new(0.0, 0.0, 500.0);
        let dest = geodesic_destination(&start, 0.0, 100_000.0).unwrap();
        assert!((dest.alt - 500.0).abs() < 1e-12);
    }
}