oxiproj-transformations 0.1.2

//! `topocentric` conversion (ECEF geocentric Cartesian X,Y,Z <-> ENU
//! topocentric East,North,Up about an origin).
//!
//! Ported from PROJ `src/conversions/topocentric.cpp`. The conversion declares
//! `left = CARTESIAN` and `right = CARTESIAN`: the geocentric Cartesian
//! coordinates on the left are mapped to local East/North/Up coordinates
//! centred on a fixed origin on the right.
//!
//! The origin may be supplied either as geocentric Cartesian coordinates
//! (`+X_0 +Y_0 +Z_0`) or as geographic coordinates (`+lon_0 +lat_0 +h_0`).
//! From the origin we cache the geocentric position `(X0, Y0, Z0)` together
//! with the sine/cosine of its longitude and latitude, which drive the rotation
//! between the geocentric and topocentric frames (see `src/conversions/topocentric.cpp`).

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

/// State for the `topocentric` conversion.
///
/// Mirrors the opaque struct of PROJ `src/conversions/topocentric.cpp`: it holds
/// the geocentric origin `(x0, y0, z0)` and the trigonometric terms of the
/// origin longitude (`sinlam0`, `coslam0`) and latitude (`sinphi0`, `cosphi0`)
/// used to rotate between the geocentric and local ENU frames.
#[derive(Debug)]
struct Topocentric {
    x0: f64,
    y0: f64,
    z0: f64,
    sinphi0: f64,
    cosphi0: f64,
    sinlam0: f64,
    coslam0: f64,
}

impl Operation for Topocentric {
    /// Forward: geocentric Cartesian `(X, Y, Z)` -> topocentric ENU.
    ///
    /// The `left = CARTESIAN` input arrives in the `Lpz` slots, so the
    /// geocentric `x`, `y`, `z` are carried in `lpz.lam`, `lpz.phi`, `lpz.z`
    /// respectively. Ported from `src/conversions/topocentric.cpp`
    /// (`topocentric_fwd`).
    fn forward_3d(&self, lpz: Lpz) -> ProjResult<Xyz> {
        let x = lpz.lam;
        let y = lpz.phi;
        let z = lpz.z;

        let dx = x - self.x0;
        let dy = y - self.y0;
        let dz = z - self.z0;

        let e = -dx * self.sinlam0 + dy * self.coslam0;
        let n = -dx * self.sinphi0 * self.coslam0 - dy * self.sinphi0 * self.sinlam0
            + dz * self.cosphi0;
        let u =
            dx * self.cosphi0 * self.coslam0 + dy * self.cosphi0 * self.sinlam0 + dz * self.sinphi0;

        Ok(Xyz::new(e, n, u))
    }

    /// Inverse: topocentric ENU -> geocentric Cartesian `(X, Y, Z)`.
    ///
    /// The input `xyz.x`, `xyz.y`, `xyz.z` are the East, North and Up
    /// components. The recovered geocentric coordinates are returned in the
    /// `Lpz` slots (`lam`, `phi`, `z`) to keep the `CARTESIAN` convention.
    /// Ported from `src/conversions/topocentric.cpp` (`topocentric_inv`).
    fn inverse_3d(&self, xyz: Xyz) -> ProjResult<Lpz> {
        let e = xyz.x;
        let n = xyz.y;
        let u = xyz.z;

        let gx = self.x0 - e * self.sinlam0 - n * self.sinphi0 * self.coslam0
            + u * self.cosphi0 * self.coslam0;
        let gy = self.y0 + e * self.coslam0 - n * self.sinphi0 * self.sinlam0
            + u * self.cosphi0 * self.sinlam0;
        let gz = self.z0 + n * self.cosphi0 + u * self.sinphi0;

        Ok(Lpz::new(gx, gy, gz))
    }

    /// Forward 4D: spatially applies [`forward_3d`](Operation::forward_3d) and
    /// passes the temporal coordinate through unchanged.
    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]))
    }

    /// Inverse 4D: spatially applies [`inverse_3d`](Operation::inverse_3d) and
    /// passes the temporal coordinate through unchanged.
    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 `topocentric` conversion.
///
/// Ported from PROJ `src/conversions/topocentric.cpp` (`PJ_CONVERSION(topocentric)`).
///
/// The origin is specified either as geocentric Cartesian coordinates
/// (`+X_0 +Y_0 +Z_0`) or as geographic coordinates (`+lon_0 +lat_0 +h_0`),
/// the two groups being mutually exclusive. Validation order mirrors PROJ:
///
/// 1. At least one of `X_0` or `lon_0` must be present, else [`ProjError::MissingArg`].
/// 2. The geocentric and geographic groups are mutually exclusive, else
///    [`ProjError::MutuallyExclusiveArgs`].
/// 3. If `X_0` is given, `Y_0` and `Z_0` are required, else [`ProjError::MissingArg`].
/// 4. If `lon_0` is given, `lat_0` is required (`h_0` defaults to `0`), else
///    [`ProjError::MissingArg`].
pub fn new(p: &crate::TransParams) -> oxiproj_core::ProjResult<crate::TransBuild> {
    let ell = p.ellipsoid;

    let has_geocentric = p.params.exists("X_0") || p.params.exists("Y_0") || p.params.exists("Z_0");
    let has_geographic =
        p.params.exists("lon_0") || p.params.exists("lat_0") || p.params.exists("h_0");

    // Rule 1: at least one origin must be specified.
    if !p.params.exists("X_0") && !p.params.exists("lon_0") {
        return Err(ProjError::MissingArg);
    }

    // Rule 2: the two origin groups are mutually exclusive.
    if has_geocentric && has_geographic {
        return Err(ProjError::MutuallyExclusiveArgs);
    }

    let (x0, y0, z0, sinphi0, cosphi0, sinlam0, coslam0) = if p.params.exists("X_0") {
        // Geocentric origin: X_0 implies Y_0 and Z_0 (rule 3).
        if !p.params.exists("Y_0") || !p.params.exists("Z_0") {
            return Err(ProjError::MissingArg);
        }
        let x0 = p.params.get_f64("X_0").ok_or(ProjError::MissingArg)?;
        let y0 = p.params.get_f64("Y_0").ok_or(ProjError::MissingArg)?;
        let z0 = p.params.get_f64("Z_0").ok_or(ProjError::MissingArg)?;

        let lpz = crate::internal::cartesian_to_geodetic(Xyz::new(x0, y0, z0), ell);
        let sinphi0 = lpz.phi.sin();
        let cosphi0 = lpz.phi.cos();
        let sinlam0 = lpz.lam.sin();
        let coslam0 = lpz.lam.cos();

        (x0, y0, z0, sinphi0, cosphi0, sinlam0, coslam0)
    } else {
        // Geographic origin: lon_0 implies lat_0 (rule 4); h_0 defaults to 0.
        if !p.params.exists("lat_0") {
            return Err(ProjError::MissingArg);
        }
        let lam0 = p.params.get_dms("lon_0").ok_or(ProjError::MissingArg)?;
        let phi0 = p.params.get_dms("lat_0").ok_or(ProjError::MissingArg)?;
        let h0 = p.params.get_f64("h_0").unwrap_or(0.0);

        let xyz = crate::internal::geodetic_to_cartesian(Lpz::new(lam0, phi0, h0), ell);
        let x0 = xyz.x;
        let y0 = xyz.y;
        let z0 = xyz.z;

        let sinphi0 = phi0.sin();
        let cosphi0 = phi0.cos();
        let sinlam0 = lam0.sin();
        let coslam0 = lam0.cos();

        (x0, y0, z0, sinphi0, cosphi0, sinlam0, coslam0)
    };

    let op = Topocentric {
        x0,
        y0,
        z0,
        sinphi0,
        cosphi0,
        sinlam0,
        coslam0,
    };

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

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

    #[derive(Default)]
    struct Params {
        x_0: Option<f64>,
        y_0: Option<f64>,
        z_0: Option<f64>,
        lon_0: Option<f64>,
        lat_0: Option<f64>,
        h_0: Option<f64>,
    }

    impl TransParamLookup for Params {
        fn get_dms(&self, key: &str) -> Option<f64> {
            match key {
                "lon_0" => self.lon_0,
                "lat_0" => self.lat_0,
                _ => None,
            }
        }
        fn get_f64(&self, key: &str) -> Option<f64> {
            match key {
                "X_0" => self.x_0,
                "Y_0" => self.y_0,
                "Z_0" => self.z_0,
                "h_0" => self.h_0,
                _ => None,
            }
        }
        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 {
            false
        }
        fn exists(&self, key: &str) -> bool {
            match key {
                "X_0" => self.x_0.is_some(),
                "Y_0" => self.y_0.is_some(),
                "Z_0" => self.z_0.is_some(),
                "lon_0" => self.lon_0.is_some(),
                "lat_0" => self.lat_0.is_some(),
                "h_0" => self.h_0.is_some(),
                _ => false,
            }
        }
    }

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

    fn build(ell: &Ellipsoid, params: &Params) -> ProjResult<crate::TransBuild> {
        new(&TransParams {
            ellipsoid: ell,
            params,
            registry: None,
        })
    }

    // cct reference 1: `+proj=topocentric +ellps=WGS84 +lon_0=2.129 +lat_0=53.809 +h_0=73`.
    // Input geocentric (3771793.97, 140253.34, 5124304.35) -> ENU
    // (36.229659933, 43.901417647, 0.001620194).
    #[test]
    fn geographic_origin_forward_matches_reference() {
        let ell = wgs84();
        let params = Params {
            lon_0: Some(2.129 * DEG_TO_RAD),
            lat_0: Some(53.809 * DEG_TO_RAD),
            h_0: Some(73.0),
            ..Default::default()
        };
        let b = build(&ell, &params).unwrap();

        let out = b
            .operation
            .forward_3d(Lpz::new(3771793.97, 140253.34, 5124304.35))
            .unwrap();
        assert!((out.x - 36.229659933).abs() < 1e-2, "E = {}", out.x);
        assert!((out.y - 43.901417647).abs() < 1e-2, "N = {}", out.y);
        assert!((out.z - 0.001620194).abs() < 1e-2, "U = {}", out.z);

        // Round trip back to geocentric.
        let back = b.operation.inverse_3d(out).unwrap();
        assert!((back.lam - 3771793.97).abs() < 1e-4, "X = {}", back.lam);
        assert!((back.phi - 140253.34).abs() < 1e-4, "Y = {}", back.phi);
        assert!((back.z - 5124304.35).abs() < 1e-4, "Z = {}", back.z);
    }

    // cct reference 2: geocentric origin equal to the point itself yields (0,0,0).
    #[test]
    fn geocentric_origin_forward_is_zero() {
        let ell = wgs84();
        let params = Params {
            x_0: Some(3771793.97),
            y_0: Some(140253.34),
            z_0: Some(5124304.35),
            ..Default::default()
        };
        let b = build(&ell, &params).unwrap();

        let out = b
            .operation
            .forward_3d(Lpz::new(3771793.97, 140253.34, 5124304.35))
            .unwrap();
        assert!(out.x.abs() < 1e-6, "E = {}", out.x);
        assert!(out.y.abs() < 1e-6, "N = {}", out.y);
        assert!(out.z.abs() < 1e-6, "U = {}", out.z);

        // Round trip recovers the original geocentric point.
        let back = b.operation.inverse_3d(out).unwrap();
        assert!((back.lam - 3771793.97).abs() < 1e-6, "X = {}", back.lam);
        assert!((back.phi - 140253.34).abs() < 1e-6, "Y = {}", back.phi);
        assert!((back.z - 5124304.35).abs() < 1e-6, "Z = {}", back.z);
    }

    #[test]
    fn error_no_origin() {
        let ell = wgs84();
        let params = Params::default();
        let err = build(&ell, &params).err();
        assert_eq!(err, Some(ProjError::MissingArg));
    }

    #[test]
    fn error_both_groups() {
        let ell = wgs84();
        let params = Params {
            x_0: Some(3771793.97),
            lon_0: Some(2.129 * DEG_TO_RAD),
            ..Default::default()
        };
        let err = build(&ell, &params).err();
        assert_eq!(err, Some(ProjError::MutuallyExclusiveArgs));
    }

    #[test]
    fn error_x0_without_y0() {
        let ell = wgs84();
        let params = Params {
            x_0: Some(3771793.97),
            ..Default::default()
        };
        let err = build(&ell, &params).err();
        assert_eq!(err, Some(ProjError::MissingArg));
    }

    #[test]
    fn error_lon0_without_lat0() {
        let ell = wgs84();
        let params = Params {
            lon_0: Some(2.129 * DEG_TO_RAD),
            ..Default::default()
        };
        let err = build(&ell, &params).err();
        assert_eq!(err, Some(ProjError::MissingArg));
    }

    #[test]
    fn forward_4d_passes_t_through() {
        let ell = wgs84();
        let params = Params {
            x_0: Some(3771793.97),
            y_0: Some(140253.34),
            z_0: Some(5124304.35),
            ..Default::default()
        };
        let b = build(&ell, &params).unwrap();

        let out = b
            .operation
            .forward_4d(Coord::new(3771793.97, 140253.34, 5124304.35, 2025.5))
            .unwrap();
        let v = out.v();
        assert!((v[3] - 2025.5).abs() < 1e-9, "t = {}", v[3]);
        assert!(v[0].abs() < 1e-6, "E = {}", v[0]);
        assert!(v[1].abs() < 1e-6, "N = {}", v[1]);
        assert!(v[2].abs() < 1e-6, "U = {}", v[2]);
    }
}