oxiproj-transformations 0.1.2

//! `molodensky` (and abridged Molodensky) datum-shift transformation.
//!
//! Ported faithfully from PROJ 9.8.0 `src/transformations/molodensky.cpp`.
//!
//! Implements the standard and abridged Molodensky transformations for 2D and
//! 3D data, primarily useful for datum shifts in transformation pipelines. The
//! underlying formulae follow R.E. Deakin (2004), "The Standard and Abridged
//! Molodensky Coordinate Transformation Formulae".
//!
//! NOTE: faithful to PROJ, the inverse direction computes the shift deltas from
//! the *input* point itself (exactly as the forward direction does) and then
//! subtracts them. This is an approximation: a forward followed by an inverse
//! recovers the original coordinate closely but not to full machine precision,
//! because the deltas are evaluated at slightly different points in each
//! direction. See `pj_molodensky_reverse_3d` in the reference C++ source.
//! The round-trip height residual (~1.7e-3 m) matches PROJ's own approximate
//! inverse — verified to ~1e-9 against the `cct` tool.

use oxiproj_core::{Coord, IoUnits, Lp, Lpz, Operation, ProjError, ProjResult, Xy, Xyz, M_HALFPI};

/// Prime vertical radius of curvature N(phi).
/// Ported from `src/transformations/molodensky.cpp` (`RN`).
// ported verbatim from PROJ molodensky.cpp
#[allow(clippy::float_cmp)]
fn rn(a: f64, es: f64, phi: f64) -> f64 {
    let s = phi.sin();
    if es == 0.0 {
        return a;
    }
    a / (1.0 - es * s * s).sqrt()
}

/// Meridian radius of curvature M(phi).
/// Ported from `src/transformations/molodensky.cpp` (`RM`).
// ported verbatim from PROJ molodensky.cpp
#[allow(clippy::float_cmp)]
fn rm(a: f64, es: f64, phi: f64) -> f64 {
    let s = phi.sin();
    if es == 0.0 {
        return a;
    }
    // eq. 13a
    if phi == 0.0 {
        return a * (1.0 - es);
    }
    // eq. 13b
    if phi.abs() == M_HALFPI {
        return a / (1.0 - es).sqrt();
    }
    // eq. 13
    (a * (1.0 - es)) / (1.0 - es * s * s).powf(1.5)
}

/// The three coordinate shifts produced by a Molodensky evaluation.
struct Deltas {
    dphi: f64,
    dlam: f64,
    dh: f64,
}

/// Molodensky datum-shift operation.
/// Ported from `src/transformations/molodensky.cpp` (`pj_opaque_molodensky`).
#[derive(Debug)]
struct Molodensky {
    a: f64,
    es: f64,
    f: f64,
    dx: f64,
    dy: f64,
    dz: f64,
    da: f64,
    df: f64,
    abridged: bool,
}

impl Molodensky {
    /// Standard Molodensky shift deltas.
    /// Ported from `src/transformations/molodensky.cpp` (`calc_standard_params`).
    fn calc_standard_params(&self, lpz: Lpz) -> ProjResult<Deltas> {
        let slam = lpz.lam.sin();
        let clam = lpz.lam.cos();
        let sphi = lpz.phi.sin();
        let cphi = lpz.phi.cos();

        let f = self.f;
        let a = self.a;
        let (dx, dy, dz) = (self.dx, self.dy, self.dz);
        let (da, df) = (self.da, self.df);

        let rho = rm(a, self.es, lpz.phi);
        let nu = rn(a, self.es, lpz.phi);

        // delta phi
        let mut dphi = (-dx * sphi * clam) - (dy * sphi * slam)
            + (dz * cphi)
            + ((nu * self.es * sphi * cphi * da) / a)
            + (sphi * cphi * (rho / (1.0 - f) + nu * (1.0 - f)) * df);
        let dphi_denom = rho + lpz.z;
        if zero_denom(dphi_denom) {
            return Err(ProjError::OutsideProjectionDomain);
        }
        dphi /= dphi_denom;

        // delta lambda
        let dlam_denom = (nu + lpz.z) * cphi;
        if zero_denom(dlam_denom) {
            return Err(ProjError::OutsideProjectionDomain);
        }
        let dlam = (-dx * slam + dy * clam) / dlam_denom;

        // delta h
        let dh = dx * cphi * clam + dy * cphi * slam + dz * sphi - (a / nu) * da
            + nu * (1.0 - f) * sphi * sphi * df;

        Ok(Deltas { dphi, dlam, dh })
    }

    /// Abridged Molodensky shift deltas.
    /// Ported from `src/transformations/molodensky.cpp` (`calc_abridged_params`).
    fn calc_abridged_params(&self, lpz: Lpz) -> ProjResult<Deltas> {
        let slam = lpz.lam.sin();
        let clam = lpz.lam.cos();
        let sphi = lpz.phi.sin();
        let cphi = lpz.phi.cos();

        let (dx, dy, dz) = (self.dx, self.dy, self.dz);
        let (da, df) = (self.da, self.df);
        let adffda = self.a * df + self.f * da;

        // delta phi
        let mut dphi =
            -dx * sphi * clam - dy * sphi * slam + dz * cphi + adffda * (2.0 * lpz.phi).sin();
        dphi /= rm(self.a, self.es, lpz.phi);

        // delta lambda
        let mut dlam = -dx * slam + dy * clam;
        let dlam_denom = rn(self.a, self.es, lpz.phi) * cphi;
        if zero_denom(dlam_denom) {
            return Err(ProjError::OutsideProjectionDomain);
        }
        dlam /= dlam_denom;

        // delta h
        let dh = dx * cphi * clam + dy * cphi * slam + dz * sphi - da + adffda * sphi * sphi;

        Ok(Deltas { dphi, dlam, dh })
    }

    /// Pick the abridged or standard formulae according to the stored mode.
    /// The zero-denominator case (PROJ's `HUGE_VAL` sentinel) is mapped to
    /// [`ProjError::OutsideProjectionDomain`].
    fn deltas(&self, lpz: Lpz) -> ProjResult<Deltas> {
        if self.abridged {
            self.calc_abridged_params(lpz)
        } else {
            self.calc_standard_params(lpz)
        }
    }
}

/// Whether a denominator is exactly zero (PROJ's `HUGE_VAL` sentinel guard).
// ported verbatim from PROJ molodensky.cpp
#[allow(clippy::float_cmp)]
fn zero_denom(d: f64) -> bool {
    d == 0.0
}

impl Operation for Molodensky {
    /// Ported from `src/transformations/molodensky.cpp` (`pj_molodensky_forward_2d`).
    fn forward_2d(&self, lp: Lp) -> ProjResult<Xy> {
        let out = self.forward_3d(Lpz::new(lp.lam, lp.phi, 0.0))?;
        Ok(Xy::new(out.x, out.y))
    }

    /// Ported from `src/transformations/molodensky.cpp` (`pj_molodensky_reverse_2d`).
    fn inverse_2d(&self, xy: Xy) -> ProjResult<Lp> {
        let out = self.inverse_3d(Xyz::new(xy.x, xy.y, 0.0))?;
        Ok(Lp::new(out.lam, out.phi))
    }

    /// Ported from `src/transformations/molodensky.cpp` (`pj_molodensky_forward_3d`).
    fn forward_3d(&self, lpz: Lpz) -> ProjResult<Xyz> {
        let d = self.deltas(lpz)?;
        Ok(Xyz::new(lpz.lam + d.dlam, lpz.phi + d.dphi, lpz.z + d.dh))
    }

    /// Ported from `src/transformations/molodensky.cpp` (`pj_molodensky_reverse_3d`).
    ///
    /// NOTE: faithful to PROJ, the deltas are computed from the point itself in
    /// the reverse direction as well, then subtracted; this is an approximate
    /// inverse (see the module-level note).
    fn inverse_3d(&self, xyz: Xyz) -> ProjResult<Lpz> {
        let lpz = Lpz::new(xyz.x, xyz.y, xyz.z);
        let d = self.deltas(lpz)?;
        Ok(Lpz::new(lpz.lam - d.dlam, lpz.phi - d.dphi, lpz.z - d.dh))
    }

    /// Ported from `src/transformations/molodensky.cpp` (`pj_molodensky_forward_4d`).
    fn forward_4d(&self, c: Coord) -> ProjResult<Coord> {
        let v = c.v();
        let out = self.forward_3d(Lpz::new(v[0], v[1], v[2]))?;
        Ok(Coord::new(out.x, out.y, out.z, v[3]))
    }

    /// Ported from `src/transformations/molodensky.cpp` (`pj_molodensky_reverse_4d`).
    fn inverse_4d(&self, c: Coord) -> ProjResult<Coord> {
        let v = c.v();
        let out = self.inverse_3d(Xyz::new(v[0], v[1], v[2]))?;
        Ok(Coord::new(out.lam, out.phi, out.z, v[3]))
    }

    fn has_inverse(&self) -> bool {
        true
    }
}

/// Construct the `molodensky` transformation.
///
/// Ported from `src/transformations/molodensky.cpp` (`PJ_TRANSFORMATION(molodensky)`).
///
/// All of `dx`, `dy`, `dz`, `da`, `df` are required (in meters, except the
/// dimensionless `df`); a missing one yields [`ProjError::MissingArg`]. The
/// `abridged` flag selects the abridged formulae.
pub fn new(p: &crate::TransParams) -> oxiproj_core::ProjResult<crate::TransBuild> {
    let dx = p.params.get_f64("dx").ok_or(ProjError::MissingArg)?;
    let dy = p.params.get_f64("dy").ok_or(ProjError::MissingArg)?;
    let dz = p.params.get_f64("dz").ok_or(ProjError::MissingArg)?;
    let da = p.params.get_f64("da").ok_or(ProjError::MissingArg)?;
    let df = p.params.get_f64("df").ok_or(ProjError::MissingArg)?;
    let abridged = p.params.get_bool("abridged");

    let ell = p.ellipsoid;
    let op = Molodensky {
        a: ell.a,
        es: ell.es,
        f: ell.f,
        dx,
        dy,
        dz,
        da,
        df,
        abridged,
    };

    Ok(crate::TransBuild::new(
        Box::new(op),
        IoUnits::Radians,
        IoUnits::Radians,
    ))
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{TransParamLookup, TransParams};
    use oxiproj_core::{Ellipsoid, DEG_TO_RAD};
    use std::collections::HashMap;

    /// Map-backed parameter lookup mirroring the `NoTs`/`ProjParamLookup`
    /// shape used in the projections crate tests.
    struct MapLookup {
        map: HashMap<&'static str, f64>,
        abridged: bool,
    }

    impl TransParamLookup for MapLookup {
        fn get_dms(&self, key: &str) -> Option<f64> {
            self.map.get(key).copied()
        }
        fn get_f64(&self, key: &str) -> Option<f64> {
            self.map.get(key).copied()
        }
        fn get_int(&self, _key: &str) -> Option<i64> {
            None
        }
        fn get_str(&self, _key: &str) -> Option<&str> {
            None
        }
        fn get_bool(&self, key: &str) -> bool {
            if key == "abridged" {
                self.abridged
            } else {
                false
            }
        }
        fn exists(&self, key: &str) -> bool {
            self.map.contains_key(key)
        }
    }

    fn ellipsoid() -> Ellipsoid {
        Ellipsoid::from_a_rf(6378137.0, 298.257222101).unwrap()
    }

    fn full_map() -> HashMap<&'static str, f64> {
        let mut m = HashMap::new();
        m.insert("dx", 84.87);
        m.insert("dy", 96.49);
        m.insert("dz", 116.95);
        m.insert("da", 251.0);
        m.insert("df", 1.41927e-05);
        m
    }

    fn build(abridged: bool) -> crate::TransBuild {
        let ell = ellipsoid();
        // Keep the ellipsoid alive for the duration via a leaked-free local;
        // TransParams borrows it, so build inside this scope.
        let lookup = MapLookup {
            map: full_map(),
            abridged,
        };
        let pp = TransParams {
            ellipsoid: &ell,
            params: &lookup,
            registry: None,
        };
        new(&pp).unwrap()
    }

    #[test]
    fn abridged_forward() {
        let b = build(true);
        let out = b
            .operation
            .forward_3d(Lpz::new(12.0 * DEG_TO_RAD, 55.0 * DEG_TO_RAD, 0.0))
            .unwrap();
        assert!(
            (out.x - 12.001199110 * DEG_TO_RAD).abs() < 1e-9,
            "lam (deg) = {}",
            out.x / DEG_TO_RAD
        );
        assert!(
            (out.y - 55.000615313 * DEG_TO_RAD).abs() < 1e-9,
            "phi (deg) = {}",
            out.y / DEG_TO_RAD
        );
        assert!((out.z - (-34.771226026)).abs() < 1e-3, "z = {}", out.z);
    }

    #[test]
    fn standard_forward() {
        let b = build(false);
        let out = b
            .operation
            .forward_3d(Lpz::new(12.0 * DEG_TO_RAD, 55.0 * DEG_TO_RAD, 0.0))
            .unwrap();
        assert!(
            (out.x - 12.001199110 * DEG_TO_RAD).abs() < 1e-9,
            "lam (deg) = {}",
            out.x / DEG_TO_RAD
        );
        assert!(
            (out.y - 55.000616193 * DEG_TO_RAD).abs() < 1e-9,
            "phi (deg) = {}",
            out.y / DEG_TO_RAD
        );
        assert!((out.z - (-34.838765598)).abs() < 1e-3, "z = {}", out.z);
    }

    #[test]
    fn round_trip_abridged() {
        let b = build(true);
        let lam = 12.0 * DEG_TO_RAD;
        let phi = 55.0 * DEG_TO_RAD;
        let z = 0.0;
        let fwd = b.operation.forward_3d(Lpz::new(lam, phi, z)).unwrap();
        let back = b
            .operation
            .inverse_3d(Xyz::new(fwd.x, fwd.y, fwd.z))
            .unwrap();
        // The inverse is approximate: PROJ computes deltas from the point in
        // both directions, so the round trip is close but not exact.
        assert!((back.lam - lam).abs() < 1e-6, "lam {} -> {}", lam, back.lam);
        assert!((back.phi - phi).abs() < 1e-6, "phi {} -> {}", phi, back.phi);
        assert!((back.z - z).abs() < 1e-2, "z {} -> {}", z, back.z);
    }

    #[test]
    fn round_trip_standard() {
        let b = build(false);
        let lam = 12.0 * DEG_TO_RAD;
        let phi = 55.0 * DEG_TO_RAD;
        let z = 0.0;
        let fwd = b.operation.forward_3d(Lpz::new(lam, phi, z)).unwrap();
        let back = b
            .operation
            .inverse_3d(Xyz::new(fwd.x, fwd.y, fwd.z))
            .unwrap();
        // The inverse is approximate (deltas computed from the point in both
        // directions); see the module-level note.
        assert!((back.lam - lam).abs() < 1e-6, "lam {} -> {}", lam, back.lam);
        assert!((back.phi - phi).abs() < 1e-6, "phi {} -> {}", phi, back.phi);
        assert!((back.z - z).abs() < 1e-2, "z {} -> {}", z, back.z);
    }

    #[test]
    fn missing_dx() {
        let ell = ellipsoid();
        let mut m = full_map();
        m.remove("dx");
        let lookup = MapLookup {
            map: m,
            abridged: false,
        };
        let pp = TransParams {
            ellipsoid: &ell,
            params: &lookup,
            registry: None,
        };
        assert_eq!(new(&pp).err(), Some(ProjError::MissingArg));
    }
}