oxiproj-transformations 0.1.2

//! Shared internal geodetic <-> geocentric Cartesian math, ported from PROJ src/conversions/cart.cpp.
//!
//! These helpers back the deferred `cart`, `geocent`, and `topocentric`
//! conversions (v0.2.0); until those ports land they are exercised only by the
//! unit tests, hence the module-level `dead_code` allowance.
#![allow(dead_code)]

use oxiproj_core::M_HALFPI;
use oxiproj_core::{Ellipsoid, Lpz, Xyz};

/// Prime vertical radius of curvature N(phi). Ported from src/conversions/cart.cpp
/// (`normal_radius_of_curvature`).
// ported verbatim from PROJ cart.cpp
#[allow(clippy::float_cmp)]
pub(crate) fn normal_radius_of_curvature(a: f64, es: f64, sinphi: f64) -> f64 {
    if es == 0.0 {
        return a;
    }
    a / (1.0 - es * sinphi * sinphi).sqrt()
}

/// Geocentric radius at latitude phi. Ported from src/conversions/cart.cpp
/// (`geocentric_radius`).
pub(crate) fn geocentric_radius(a: f64, b_div_a: f64, cosphi: f64, sinphi: f64) -> f64 {
    let cosphi_squared = cosphi * cosphi;
    let sinphi_squared = sinphi * sinphi;
    let b_div_a_squared = b_div_a * b_div_a;
    let b_div_a_squared_mul_sinphi_squared = b_div_a_squared * sinphi_squared;
    a * ((cosphi_squared + b_div_a_squared * b_div_a_squared_mul_sinphi_squared)
        / (cosphi_squared + b_div_a_squared_mul_sinphi_squared))
        .sqrt()
}

/// Geographic (lon,lat,h) -> geocentric Cartesian (X,Y,Z).
/// Ported from src/conversions/cart.cpp (`cartesian`).
pub(crate) fn geodetic_to_cartesian(geod: Lpz, ell: &Ellipsoid) -> Xyz {
    let cosphi = geod.phi.cos();
    let sinphi = geod.phi.sin();
    let n = normal_radius_of_curvature(ell.a, ell.es, sinphi);
    let x = (n + geod.z) * cosphi * geod.lam.cos();
    let y = (n + geod.z) * cosphi * geod.lam.sin();
    let z = (n * (1.0 - ell.es) + geod.z) * sinphi;
    Xyz::new(x, y, z)
}

/// Geocentric Cartesian (X,Y,Z) -> geographic (lon,lat,h), Bowring's method.
/// Ported from src/conversions/cart.cpp (`geodetic`).
// ported verbatim from PROJ cart.cpp
#[allow(clippy::float_cmp)]
pub(crate) fn cartesian_to_geodetic(cart: Xyz, ell: &Ellipsoid) -> Lpz {
    let x_div_a = cart.x * ell.ra;
    let y_div_a = cart.y * ell.ra;
    let z_div_a = cart.z * ell.ra;
    let p_div_a = (x_div_a * x_div_a + y_div_a * y_div_a).sqrt();
    let b_div_a = 1.0 - ell.f;
    let p_div_a_b_div_a = p_div_a * b_div_a;
    let norm = (z_div_a * z_div_a + p_div_a_b_div_a * p_div_a_b_div_a).sqrt();
    let (c, s) = if norm != 0.0 {
        let inv_norm = 1.0 / norm;
        (p_div_a_b_div_a * inv_norm, z_div_a * inv_norm)
    } else {
        (1.0, 0.0)
    };
    let y_phi = z_div_a + ell.e2s * b_div_a * s * s * s;
    let x_phi = p_div_a - ell.es * c * c * c;
    let norm_phi = (y_phi * y_phi + x_phi * x_phi).sqrt();
    let (mut cosphi, mut sinphi) = if norm_phi != 0.0 {
        let inv_norm_phi = 1.0 / norm_phi;
        (x_phi * inv_norm_phi, y_phi * inv_norm_phi)
    } else {
        (1.0, 0.0)
    };
    let phi = if x_phi <= 0.0 {
        cosphi = 0.0;
        sinphi = if cart.z >= 0.0 { 1.0 } else { -1.0 };
        if cart.z >= 0.0 {
            M_HALFPI
        } else {
            -M_HALFPI
        }
    } else {
        (y_phi / x_phi).atan()
    };
    let lam = y_div_a.atan2(x_div_a);
    let z = if cosphi < 1e-6 {
        let r = geocentric_radius(ell.a, b_div_a, cosphi, sinphi);
        cart.z.abs() - r
    } else {
        let n = normal_radius_of_curvature(ell.a, ell.es, sinphi);
        ell.a * p_div_a / cosphi - n
    };
    Lpz::new(lam, phi, z)
}

#[cfg(test)]
mod tests {
    use super::*;
    use oxiproj_core::{Ellipsoid, Lpz, DEG_TO_RAD};

    fn wgs84() -> Ellipsoid {
        Ellipsoid::named("WGS84").unwrap()
    }

    #[test]
    fn cart_round_trip_wgs84() {
        let ell = wgs84();
        let lam = 12.0 * DEG_TO_RAD;
        let phi = 55.0 * DEG_TO_RAD;
        let h = 100.0;
        let geod = Lpz::new(lam, phi, h);
        let xyz = geodetic_to_cartesian(geod, &ell);
        assert!((xyz.x - 3586525.761017917).abs() < 1e-4, "X = {}", xyz.x);
        assert!((xyz.y - 762339.584102928).abs() < 1e-4, "Y = {}", xyz.y);
        assert!((xyz.z - 5201465.438406702).abs() < 1e-4, "Z = {}", xyz.z);
        let back = cartesian_to_geodetic(xyz, &ell);
        assert!((back.lam - lam).abs() < 1e-12, "lam = {}", back.lam);
        assert!((back.phi - phi).abs() < 1e-12, "phi = {}", back.phi);
        assert!((back.z - h).abs() < 1e-4, "z = {}", back.z);
    }
}