oxiproj-transformations 0.1.2

//! `affine` and `geogoffset` transformations.
//!
//! Ported from PROJ 9.8.0 `src/transformations/affine.cpp`
//! (`PJ_TRANSFORMATION(affine)`, `PJ_TRANSFORMATION(geogoffset)`, and
//! `computeReverseParameters`).

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

/// Arcsecond to radians. Ported from PROJ `affine.cpp` (`#define ARCSEC_TO_RAD`).
const ARCSEC_TO_RAD: f64 = DEG_TO_RAD / 3600.0;

/// Affine / geographic-offset operation.
///
/// Holds the constant offsets, the forward 3x3 scale/rotation matrix plus time
/// scale, the precomputed reverse 3x3 matrix plus reverse time scale, and an
/// invertibility flag. Ported from PROJ `affine.cpp` (`pj_opaque_affine` and
/// `pj_affine_coeffs`).
#[derive(Debug)]
struct Affine {
    xoff: f64,
    yoff: f64,
    zoff: f64,
    toff: f64,
    s11: f64,
    s12: f64,
    s13: f64,
    s21: f64,
    s22: f64,
    s23: f64,
    s31: f64,
    s32: f64,
    s33: f64,
    tscale: f64,
    r11: f64,
    r12: f64,
    r13: f64,
    r21: f64,
    r22: f64,
    r23: f64,
    r31: f64,
    r32: f64,
    r33: f64,
    rev_tscale: f64,
    invertible: bool,
}

/// Inverse of the forward 3x3 matrix plus reverse time scale and invertibility.
struct ReverseMatrix {
    r11: f64,
    r12: f64,
    r13: f64,
    r21: f64,
    r22: f64,
    r23: f64,
    r31: f64,
    r32: f64,
    r33: f64,
    rev_tscale: f64,
    invertible: bool,
}

/// Compute the reverse (inverse) matrix from the forward coefficients.
///
/// Ported verbatim from PROJ `affine.cpp` (`computeReverseParameters`). When the
/// forward matrix is singular (`det == 0`) or the time scale is zero, the
/// operation is flagged non-invertible and the returned reverse matrix is left
/// as an identity placeholder (never used because `invertible` is false).
// ported verbatim from PROJ affine.cpp
#[allow(clippy::float_cmp)]
fn compute_reverse(m: [f64; 9], tscale: f64) -> ReverseMatrix {
    let [s11, s12, s13, s21, s22, s23, s31, s32, s33] = m;
    // cf
    // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices
    let a = s11;
    let b = s12;
    let c = s13;
    let d = s21;
    let e = s22;
    let f = s23;
    let g = s31;
    let h = s32;
    let i = s33;
    let big_a = e * i - f * h;
    let big_b = -(d * i - f * g);
    let big_c = d * h - e * g;
    let big_d = -(b * i - c * h);
    let big_e = a * i - c * g;
    let big_f = -(a * h - b * g);
    let big_g = b * f - c * e;
    let big_h = -(a * f - c * d);
    let big_i = a * e - b * d;
    let det = a * big_a + b * big_b + c * big_c;
    if det == 0.0 || tscale == 0.0 {
        ReverseMatrix {
            r11: 1.0,
            r12: 0.0,
            r13: 0.0,
            r21: 0.0,
            r22: 1.0,
            r23: 0.0,
            r31: 0.0,
            r32: 0.0,
            r33: 1.0,
            rev_tscale: 1.0,
            invertible: false,
        }
    } else {
        ReverseMatrix {
            r11: big_a / det,
            r12: big_d / det,
            r13: big_g / det,
            r21: big_b / det,
            r22: big_e / det,
            r23: big_h / det,
            r31: big_c / det,
            r32: big_f / det,
            r33: big_i / det,
            rev_tscale: 1.0 / tscale,
            invertible: true,
        }
    }
}

impl Affine {
    /// Assemble an `Affine` from offsets and forward coefficients, computing the
    /// reverse matrix and invertibility flag via [`compute_reverse`].
    #[allow(clippy::too_many_arguments)]
    fn assemble(
        xoff: f64,
        yoff: f64,
        zoff: f64,
        toff: f64,
        s11: f64,
        s12: f64,
        s13: f64,
        s21: f64,
        s22: f64,
        s23: f64,
        s31: f64,
        s32: f64,
        s33: f64,
        tscale: f64,
    ) -> Affine {
        let rev = compute_reverse([s11, s12, s13, s21, s22, s23, s31, s32, s33], tscale);
        Affine {
            xoff,
            yoff,
            zoff,
            toff,
            s11,
            s12,
            s13,
            s21,
            s22,
            s23,
            s31,
            s32,
            s33,
            tscale,
            r11: rev.r11,
            r12: rev.r12,
            r13: rev.r13,
            r21: rev.r21,
            r22: rev.r22,
            r23: rev.r23,
            r31: rev.r31,
            r32: rev.r32,
            r33: rev.r33,
            rev_tscale: rev.rev_tscale,
            invertible: rev.invertible,
        }
    }
}

impl Operation for Affine {
    /// Ported from PROJ `affine.cpp` (`forward_4d`).
    fn forward_4d(&self, c: Coord) -> ProjResult<Coord> {
        let v = c.v();
        let (x, y, z, t) = (v[0], v[1], v[2], v[3]);
        let ox = self.xoff + self.s11 * x + self.s12 * y + self.s13 * z;
        let oy = self.yoff + self.s21 * x + self.s22 * y + self.s23 * z;
        let oz = self.zoff + self.s31 * x + self.s32 * y + self.s33 * z;
        let ot = self.toff + self.tscale * t;
        Ok(Coord::new(ox, oy, oz, ot))
    }

    /// Ported from PROJ `affine.cpp` (`reverse_4d`). Returns
    /// [`ProjError::NoInverseOp`] when the matrix is non-invertible (PROJ nulls
    /// out the inverse callbacks in that case).
    fn inverse_4d(&self, c: Coord) -> ProjResult<Coord> {
        if !self.invertible {
            return Err(ProjError::NoInverseOp);
        }
        let v = c.v();
        let x = v[0] - self.xoff;
        let y = v[1] - self.yoff;
        let z = v[2] - self.zoff;
        let ox = self.r11 * x + self.r12 * y + self.r13 * z;
        let oy = self.r21 * x + self.r22 * y + self.r23 * z;
        let oz = self.r31 * x + self.r32 * y + self.r33 * z;
        let ot = self.rev_tscale * (v[3] - self.toff);
        Ok(Coord::new(ox, oy, oz, ot))
    }

    /// Ported from PROJ `affine.cpp` (`forward_3d`): route through `forward_4d`
    /// with `t = 0`.
    fn forward_3d(&self, lpz: Lpz) -> ProjResult<Xyz> {
        let out = self.forward_4d(Coord::new(lpz.lam, lpz.phi, lpz.z, 0.0))?;
        let v = out.v();
        Ok(Xyz::new(v[0], v[1], v[2]))
    }

    /// Ported from PROJ `affine.cpp` (`reverse_3d`): route through `inverse_4d`
    /// with `t = 0`.
    fn inverse_3d(&self, xyz: Xyz) -> ProjResult<Lpz> {
        let out = self.inverse_4d(Coord::new(xyz.x, xyz.y, xyz.z, 0.0))?;
        let v = out.v();
        Ok(Lpz::new(v[0], v[1], v[2]))
    }

    /// Ported from PROJ `affine.cpp` (`forward_2d`): route through `forward_4d`
    /// with `z = 0`, `t = 0`.
    fn forward_2d(&self, lp: Lp) -> ProjResult<Xy> {
        let out = self.forward_4d(Coord::new(lp.lam, lp.phi, 0.0, 0.0))?;
        let v = out.v();
        Ok(Xy::new(v[0], v[1]))
    }

    /// Ported from PROJ `affine.cpp` (`reverse_2d`): route through `inverse_4d`
    /// with `z = 0`, `t = 0`.
    fn inverse_2d(&self, xy: Xy) -> ProjResult<Lp> {
        let out = self.inverse_4d(Coord::new(xy.x, xy.y, 0.0, 0.0))?;
        let v = out.v();
        Ok(Lp::new(v[0], v[1]))
    }

    fn has_inverse(&self) -> bool {
        self.invertible
    }
}

/// Construct the `affine` transformation.
///
/// Ported from PROJ `affine.cpp` (`PJ_TRANSFORMATION(affine)`). Left and right
/// units are both `Whatever`. The diagonal matrix terms (`s11`, `s22`, `s33`)
/// and `tscale` default to 1.0; all other coefficients and offsets default to
/// 0.0. `tshift` is accepted as an alias for `toff`.
pub fn new(p: &crate::TransParams) -> oxiproj_core::ProjResult<crate::TransBuild> {
    let params = p.params;
    let xoff = params.get_f64("xoff").unwrap_or(0.0);
    let yoff = params.get_f64("yoff").unwrap_or(0.0);
    let zoff = params.get_f64("zoff").unwrap_or(0.0);
    let toff = match params.get_f64("toff") {
        Some(v) => v,
        None => params.get_f64("tshift").unwrap_or(0.0),
    };

    let s11 = params.get_f64("s11").unwrap_or(1.0);
    let s12 = params.get_f64("s12").unwrap_or(0.0);
    let s13 = params.get_f64("s13").unwrap_or(0.0);
    let s21 = params.get_f64("s21").unwrap_or(0.0);
    let s22 = params.get_f64("s22").unwrap_or(1.0);
    let s23 = params.get_f64("s23").unwrap_or(0.0);
    let s31 = params.get_f64("s31").unwrap_or(0.0);
    let s32 = params.get_f64("s32").unwrap_or(0.0);
    let s33 = params.get_f64("s33").unwrap_or(1.0);
    let tscale = params.get_f64("tscale").unwrap_or(1.0);

    let op = Affine::assemble(
        xoff, yoff, zoff, toff, s11, s12, s13, s21, s22, s23, s31, s32, s33, tscale,
    );
    Ok(crate::TransBuild::new(
        Box::new(op),
        IoUnits::Whatever,
        IoUnits::Whatever,
    ))
}

/// Construct the `geogoffset` transformation.
///
/// Ported from PROJ `affine.cpp` (`PJ_TRANSFORMATION(geogoffset)`). Left and
/// right units are both `Radians`. `dlon`/`dlat` are arcsecond offsets (scaled
/// to radians); `dh` is a metre height offset. The matrix is the identity, so
/// the result is `(lam + xoff, phi + yoff, z + zoff, t)`.
pub fn new_geogoffset(p: &crate::TransParams) -> oxiproj_core::ProjResult<crate::TransBuild> {
    let params = p.params;
    let xoff = params.get_f64("dlon").unwrap_or(0.0) * ARCSEC_TO_RAD;
    let yoff = params.get_f64("dlat").unwrap_or(0.0) * ARCSEC_TO_RAD;
    let zoff = params.get_f64("dh").unwrap_or(0.0);

    let op = Affine::assemble(
        xoff, yoff, zoff, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0,
    );
    Ok(crate::TransBuild::new(
        Box::new(op),
        IoUnits::Radians,
        IoUnits::Radians,
    ))
}

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

    struct MapLookup {
        map: HashMap<&'static str, f64>,
    }

    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 {
            false
        }
        fn exists(&self, key: &str) -> bool {
            self.map.contains_key(key)
        }
    }

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

    #[test]
    fn affine_forward_inverse() {
        let mut map = HashMap::new();
        map.insert("xoff", 1.0);
        map.insert("yoff", 2.0);
        map.insert("zoff", 3.0);
        map.insert("s11", 1.1);
        map.insert("s12", 0.1);
        map.insert("s22", 1.2);
        map.insert("s33", 1.3);
        map.insert("tscale", 2.0);
        let lookup = MapLookup { map };
        let ell = wgs84();
        let pp = TransParams {
            ellipsoid: &ell,
            params: &lookup,
            registry: None,
        };
        let b = new(&pp).unwrap();

        let out = b
            .operation
            .forward_4d(Coord::new(10.0, 20.0, 30.0, 40.0))
            .unwrap();
        let v = out.v();
        // x = 1 + 1.1*10 + 0.1*20 = 14; y = 2 + 1.2*20 = 26;
        // z = 3 + 1.3*30 = 42; t = 2*40 = 80
        assert!((v[0] - 14.0).abs() < 1e-9, "x = {}", v[0]);
        assert!((v[1] - 26.0).abs() < 1e-9, "y = {}", v[1]);
        assert!((v[2] - 42.0).abs() < 1e-9, "z = {}", v[2]);
        assert!((v[3] - 80.0).abs() < 1e-9, "t = {}", v[3]);

        let back = b
            .operation
            .inverse_4d(Coord::new(14.0, 26.0, 42.0, 80.0))
            .unwrap();
        let r = back.v();
        assert!((r[0] - 10.0).abs() < 1e-9, "x = {}", r[0]);
        assert!((r[1] - 20.0).abs() < 1e-9, "y = {}", r[1]);
        assert!((r[2] - 30.0).abs() < 1e-9, "z = {}", r[2]);
        assert!((r[3] - 40.0).abs() < 1e-9, "t = {}", r[3]);
    }

    #[test]
    fn affine_non_invertible() {
        let mut map = HashMap::new();
        // Force det = 0 by zeroing the diagonal terms that otherwise default to 1.
        map.insert("s11", 0.0);
        map.insert("s22", 0.0);
        map.insert("s33", 0.0);
        let lookup = MapLookup { map };
        let ell = wgs84();
        let pp = TransParams {
            ellipsoid: &ell,
            params: &lookup,
            registry: None,
        };
        let b = new(&pp).unwrap();

        assert!(!b.operation.has_inverse());
        let err = b.operation.inverse_4d(Coord::new(0.0, 0.0, 0.0, 0.0)).err();
        assert_eq!(err, Some(ProjError::NoInverseOp));
    }

    #[test]
    fn geogoffset_offset() {
        let mut map = HashMap::new();
        map.insert("dlon", 3600.0); // 1 degree
        map.insert("dlat", 0.0);
        map.insert("dh", 5.0);
        let lookup = MapLookup { map };
        let ell = wgs84();
        let pp = TransParams {
            ellipsoid: &ell,
            params: &lookup,
            registry: None,
        };
        let b = new_geogoffset(&pp).unwrap();

        let out = b
            .operation
            .forward_4d(Coord::new(12.0 * DEG_TO_RAD, 55.0 * DEG_TO_RAD, 100.0, 0.0))
            .unwrap();
        let v = out.v();
        assert!((v[0] - 13.0 * DEG_TO_RAD).abs() < 1e-12, "lam = {}", v[0]);
        assert!((v[1] - 55.0 * DEG_TO_RAD).abs() < 1e-12, "phi = {}", v[1]);
        assert!((v[2] - 105.0).abs() < 1e-9, "z = {}", v[2]);
    }
}