geo3d 0.3.2

//! Optional conversions between the coordinate types and the common math / GIS crates.
//!
//! Each block is behind its own default-off feature, so the crate stays dependency-free unless you
//! ask for a bridge:
//!
//! - **`mint`** — [`From`]/[`Into`] with `mint::Vector3<f64>` (all vector types) and `mint::Point3<f64>`
//!   (the position types), [`Cov3`](crate::Cov3) with `mint::RowMatrix3<f64>` and
//!   `mint::ColumnMatrix3<f64>`, and [`Quaternion`](crate::Quaternion) with `mint::Quaternion<f64>`. `mint` is the Math-Interoperability-Types lingua franca, so this
//!   one bridge also reaches `glam`, `nalgebra` and `cgmath`, which all speak `mint`.
//! - **`glam`** — with `glam::DVec3`, [`Cov3`](crate::Cov3) with `glam::DMat3`, and
//!   [`Quaternion`](crate::Quaternion) with `glam::DQuat`.
//! - **`nalgebra`** — with `nalgebra::Vector3<f64>` and `nalgebra::Point3<f64>`, and
//!   [`Cov3`](crate::Cov3) with `nalgebra::Matrix3<f64>`, and [`Quaternion`](crate::Quaternion)
//!   with `nalgebra::Quaternion<f64>` (and from `UnitQuaternion<f64>`).
//! - **`geo-types`** — [`Geodetic`](crate::Geodetic) ↔ `geo_types::Point<f64>` / `Coord<f64>` for 2D
//!   GIS pipelines. **Longitude is `x`, latitude is `y`** (the `geo` convention) and the ellipsoidal
//!   height is dropped on the way out and taken as `0` on the way in.
//!
//! ```
//! # #[cfg(feature = "glam")] {
//! use geo3d::Ecef;
//! let e = Ecef::new(1.0, 2.0, 3.0);
//! let v: glam::DVec3 = e.into();
//! assert_eq!(Ecef::from(v), e);
//! # }
//! ```

// The macros below only expand under a feature, so they are otherwise-unused without one.
#![allow(unused_macros)]

/// `From`/`Into` both ways between a geo3d 3-component type and a foreign 3-vector type. The forward
/// direction reads the named fields; the reverse always builds through `Type::new(x, y, z)`.
macro_rules! vec3_bridge {
    ($geo:ident { $a:ident, $b:ident, $c:ident } <=> $foreign:ty : $ctor:expr, |$v:ident| $rev:expr) => {
        impl From<crate::$geo> for $foreign {
            #[inline]
            fn from(g: crate::$geo) -> Self {
                let ctor = $ctor;
                ctor(g.$a, g.$b, g.$c)
            }
        }
        impl From<$foreign> for crate::$geo {
            #[inline]
            fn from($v: $foreign) -> Self {
                let (x, y, z) = $rev;
                crate::$geo::new(x, y, z)
            }
        }
    };
}

#[cfg(feature = "mint")]
mod mint_impls {
    vec3_bridge!(Vec3  { x, y, z }            <=> mint::Vector3<f64> : |x, y, z| mint::Vector3 { x, y, z }, |v| (v.x, v.y, v.z));
    vec3_bridge!(Enu   { east, north, up }    <=> mint::Vector3<f64> : |x, y, z| mint::Vector3 { x, y, z }, |v| (v.x, v.y, v.z));
    vec3_bridge!(Ned   { north, east, down }  <=> mint::Vector3<f64> : |x, y, z| mint::Vector3 { x, y, z }, |v| (v.x, v.y, v.z));
    vec3_bridge!(Frd   { forward, right, down } <=> mint::Vector3<f64> : |x, y, z| mint::Vector3 { x, y, z }, |v| (v.x, v.y, v.z));
    vec3_bridge!(Ecef  { x, y, z }            <=> mint::Vector3<f64> : |x, y, z| mint::Vector3 { x, y, z }, |v| (v.x, v.y, v.z));
    vec3_bridge!(Eci   { x, y, z }            <=> mint::Vector3<f64> : |x, y, z| mint::Vector3 { x, y, z }, |v| (v.x, v.y, v.z));
    // The position types also convert to/from `Point3`.
    vec3_bridge!(Ecef  { x, y, z }            <=> mint::Point3<f64>  : |x, y, z| mint::Point3 { x, y, z },  |v| (v.x, v.y, v.z));
    vec3_bridge!(Eci   { x, y, z }            <=> mint::Point3<f64>  : |x, y, z| mint::Point3 { x, y, z },  |v| (v.x, v.y, v.z));

    use crate::Cov3;
    use mint::{ColumnMatrix3, RowMatrix3, Vector3};

    fn v([x, y, z]: [f64; 3]) -> Vector3<f64> {
        Vector3 { x, y, z }
    }
    impl From<Cov3> for RowMatrix3<f64> {
        fn from(c: Cov3) -> Self {
            let [x, y, z] = c.0.map(v);
            RowMatrix3 { x, y, z }
        }
    }
    impl From<RowMatrix3<f64>> for Cov3 {
        fn from(m: RowMatrix3<f64>) -> Self {
            Cov3([m.x, m.y, m.z].map(|r| [r.x, r.y, r.z]))
        }
    }
    impl From<Cov3> for ColumnMatrix3<f64> {
        fn from(c: Cov3) -> Self {
            let m = c.0;
            let col = |j: usize| v([m[0][j], m[1][j], m[2][j]]);
            ColumnMatrix3 {
                x: col(0),
                y: col(1),
                z: col(2),
            }
        }
    }
    impl From<crate::Quaternion> for mint::Quaternion<f64> {
        fn from(q: crate::Quaternion) -> Self {
            mint::Quaternion {
                s: q.w,
                v: v([q.x, q.y, q.z]),
            }
        }
    }
    impl From<mint::Quaternion<f64>> for crate::Quaternion {
        fn from(q: mint::Quaternion<f64>) -> Self {
            crate::Quaternion::new(q.s, q.v.x, q.v.y, q.v.z)
        }
    }
    impl From<ColumnMatrix3<f64>> for Cov3 {
        fn from(m: ColumnMatrix3<f64>) -> Self {
            Cov3([[m.x.x, m.y.x, m.z.x], [m.x.y, m.y.y, m.z.y], [m.x.z, m.y.z, m.z.z]])
        }
    }
}

#[cfg(feature = "glam")]
mod glam_impls {
    vec3_bridge!(Vec3  { x, y, z }            <=> glam::DVec3 : |x, y, z| glam::DVec3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Enu   { east, north, up }    <=> glam::DVec3 : |x, y, z| glam::DVec3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Ned   { north, east, down }  <=> glam::DVec3 : |x, y, z| glam::DVec3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Frd   { forward, right, down } <=> glam::DVec3 : |x, y, z| glam::DVec3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Ecef  { x, y, z }            <=> glam::DVec3 : |x, y, z| glam::DVec3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Eci   { x, y, z }            <=> glam::DVec3 : |x, y, z| glam::DVec3::new(x, y, z), |v| (v.x, v.y, v.z));

    use crate::Cov3;
    use glam::DMat3;

    // glam stores columns; Cov3 is row by row.
    impl From<Cov3> for DMat3 {
        fn from(c: Cov3) -> Self {
            DMat3::from_cols_array_2d(&c.0).transpose()
        }
    }
    impl From<DMat3> for Cov3 {
        fn from(m: DMat3) -> Self {
            Cov3(m.transpose().to_cols_array_2d())
        }
    }
    // glam stores the scalar last.
    impl From<crate::Quaternion> for glam::DQuat {
        fn from(q: crate::Quaternion) -> Self {
            glam::DQuat::from_xyzw(q.x, q.y, q.z, q.w)
        }
    }
    impl From<glam::DQuat> for crate::Quaternion {
        fn from(q: glam::DQuat) -> Self {
            crate::Quaternion::new(q.w, q.x, q.y, q.z)
        }
    }
}

#[cfg(feature = "nalgebra")]
mod nalgebra_impls {
    vec3_bridge!(Vec3  { x, y, z }            <=> nalgebra::Vector3<f64> : |x, y, z| nalgebra::Vector3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Enu   { east, north, up }    <=> nalgebra::Vector3<f64> : |x, y, z| nalgebra::Vector3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Ned   { north, east, down }  <=> nalgebra::Vector3<f64> : |x, y, z| nalgebra::Vector3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Frd   { forward, right, down } <=> nalgebra::Vector3<f64> : |x, y, z| nalgebra::Vector3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Ecef  { x, y, z }            <=> nalgebra::Vector3<f64> : |x, y, z| nalgebra::Vector3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Eci   { x, y, z }            <=> nalgebra::Vector3<f64> : |x, y, z| nalgebra::Vector3::new(x, y, z), |v| (v.x, v.y, v.z));
    vec3_bridge!(Ecef  { x, y, z }            <=> nalgebra::Point3<f64>  : |x, y, z| nalgebra::Point3::new(x, y, z),  |v| (v.coords.x, v.coords.y, v.coords.z));
    vec3_bridge!(Eci   { x, y, z }            <=> nalgebra::Point3<f64>  : |x, y, z| nalgebra::Point3::new(x, y, z),  |v| (v.coords.x, v.coords.y, v.coords.z));

    use crate::Cov3;
    use nalgebra::Matrix3;

    impl From<Cov3> for Matrix3<f64> {
        fn from(c: Cov3) -> Self {
            Matrix3::from_fn(|r, col| c.0[r][col])
        }
    }
    impl From<Matrix3<f64>> for Cov3 {
        fn from(m: Matrix3<f64>) -> Self {
            Cov3(core::array::from_fn(|r| core::array::from_fn(|col| m[(r, col)])))
        }
    }
    impl From<crate::Quaternion> for nalgebra::Quaternion<f64> {
        fn from(q: crate::Quaternion) -> Self {
            nalgebra::Quaternion::new(q.w, q.x, q.y, q.z)
        }
    }
    impl From<nalgebra::Quaternion<f64>> for crate::Quaternion {
        fn from(q: nalgebra::Quaternion<f64>) -> Self {
            crate::Quaternion::new(q.w, q.i, q.j, q.k)
        }
    }
    impl From<nalgebra::UnitQuaternion<f64>> for crate::Quaternion {
        fn from(q: nalgebra::UnitQuaternion<f64>) -> Self {
            crate::Quaternion::from(q.into_inner())
        }
    }
}

#[cfg(feature = "geo-types")]
mod geo_types_impls {
    use crate::Geodetic;

    // The `geo` convention is (x = longitude, y = latitude). Height is dropped going out, 0 coming in.
    impl From<Geodetic> for geo_types::Point<f64> {
        #[inline]
        fn from(g: Geodetic) -> Self {
            geo_types::Point::new(g.lon_deg, g.lat_deg)
        }
    }
    impl From<Geodetic> for geo_types::Coord<f64> {
        #[inline]
        fn from(g: Geodetic) -> Self {
            geo_types::Coord {
                x: g.lon_deg,
                y: g.lat_deg,
            }
        }
    }
    impl From<geo_types::Point<f64>> for Geodetic {
        #[inline]
        fn from(p: geo_types::Point<f64>) -> Self {
            Geodetic::new(p.y(), p.x(), 0.0)
        }
    }
    impl From<geo_types::Coord<f64>> for Geodetic {
        #[inline]
        fn from(c: geo_types::Coord<f64>) -> Self {
            Geodetic::new(c.y, c.x, 0.0)
        }
    }
}

#[cfg(all(
    test,
    any(feature = "mint", feature = "glam", feature = "nalgebra", feature = "geo-types")
))]
mod tests {
    #[allow(unused_imports)]
    use crate::{Ecef, Eci, Enu, Geodetic, Ned, Vec3};

    #[cfg(feature = "mint")]
    #[test]
    fn mint_round_trips_and_orders_fields() {
        let v = Vec3::new(1.0, 2.0, 3.0);
        let m: mint::Vector3<f64> = v.into();
        assert_eq!((m.x, m.y, m.z), (1.0, 2.0, 3.0));
        assert_eq!(Vec3::from(m), v);
        // ENU field order maps east→x, north→y, up→z, and round-trips.
        let enu = Enu::new(4.0, 5.0, 6.0);
        let me: mint::Vector3<f64> = enu.into();
        assert_eq!((me.x, me.y, me.z), (4.0, 5.0, 6.0));
        assert_eq!(Enu::from(me), enu);
        // Ned maps north→x, east→y, down→z.
        let ned = Ned::new(7.0, 8.0, 9.0);
        let mn: mint::Vector3<f64> = ned.into();
        assert_eq!((mn.x, mn.y, mn.z), (7.0, 8.0, 9.0));
        assert_eq!(Ned::from(mn), ned);
        // Positions also bridge to Point3.
        let e = Ecef::new(10.0, 20.0, 30.0);
        let p: mint::Point3<f64> = e.into();
        assert_eq!(Ecef::from(p), e);
        let i = Eci::new(-1.0, -2.0, -3.0);
        let pi: mint::Point3<f64> = i.into();
        assert_eq!(Eci::from(pi), i);
    }

    #[cfg(feature = "glam")]
    #[test]
    fn glam_round_trips() {
        let e = Ecef::new(1.5, -2.5, 3.5);
        let g: glam::DVec3 = e.into();
        assert_eq!((g.x, g.y, g.z), (1.5, -2.5, 3.5));
        assert_eq!(Ecef::from(g), e);
        let enu = Enu::new(1.0, 2.0, 3.0);
        assert_eq!(Enu::from(glam::DVec3::from(enu)), enu);
    }

    #[cfg(feature = "nalgebra")]
    #[test]
    fn nalgebra_round_trips_vector_and_point() {
        let v = Vec3::new(1.0, 2.0, 3.0);
        let nv: nalgebra::Vector3<f64> = v.into();
        assert_eq!((nv.x, nv.y, nv.z), (1.0, 2.0, 3.0));
        assert_eq!(Vec3::from(nv), v);
        let e = Ecef::new(10.0, 20.0, 30.0);
        let np: nalgebra::Point3<f64> = e.into();
        assert_eq!(Ecef::from(np), e);
    }

    /// Not a real covariance (it isn't symmetric), so a row/column mix-up shows.
    #[allow(dead_code)]
    const M: crate::Cov3 = crate::Cov3([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]]);

    #[cfg(feature = "mint")]
    #[test]
    fn mint_matrices_keep_rows_and_columns() {
        let r: mint::RowMatrix3<f64> = M.into();
        assert_eq!((r.x.y, r.y.x), (2.0, 4.0)); // row 0, column 1 and row 1, column 0
        assert_eq!(crate::Cov3::from(r), M);
        let c: mint::ColumnMatrix3<f64> = M.into();
        assert_eq!((c.y.x, c.x.y), (2.0, 4.0)); // column 1's first entry is row 0, column 1
        assert_eq!(crate::Cov3::from(c), M);
    }

    #[cfg(feature = "glam")]
    #[test]
    fn glam_matrix_keeps_rows_and_columns() {
        let g: glam::DMat3 = M.into();
        assert_eq!(g.row(0), glam::DVec3::new(1.0, 2.0, 3.0));
        assert_eq!(crate::Cov3::from(g), M);
    }

    #[cfg(feature = "nalgebra")]
    #[test]
    fn nalgebra_matrix_keeps_rows_and_columns() {
        let n: nalgebra::Matrix3<f64> = M.into();
        assert_eq!(n[(0, 1)], 2.0);
        assert_eq!(crate::Cov3::from(n), M);
    }

    /// Every quaternion bridge keeps the scalar part as the scalar, and glam's and nalgebra's own
    /// yaw, pitch and roll give the same quaternion as geo3d's (an independent check of the
    /// convention: Hamilton, scalar first, body to NED).
    #[allow(dead_code)]
    fn same_rotation(a: crate::Quaternion, b: crate::Quaternion) -> bool {
        let d = |s: f64| {
            (a.w - s * b.w)
                .abs()
                .max((a.x - s * b.x).abs())
                .max((a.y - s * b.y).abs())
                .max((a.z - s * b.z).abs())
        };
        d(1.0).min(d(-1.0)) < 1e-15
    }

    #[cfg(feature = "mint")]
    #[test]
    fn mint_quaternion_keeps_the_scalar() {
        let q = crate::Quaternion::new(0.5, 0.1, 0.2, 0.3);
        let m: mint::Quaternion<f64> = q.into();
        assert_eq!((m.s, m.v.x, m.v.y, m.v.z), (0.5, 0.1, 0.2, 0.3));
        assert_eq!(crate::Quaternion::from(m), q);
    }

    #[cfg(feature = "glam")]
    #[test]
    fn glam_quaternion_agrees() {
        let q = crate::Quaternion::new(0.5, 0.1, 0.2, 0.3);
        let g: glam::DQuat = q.into();
        assert_eq!((g.w, g.x, g.y, g.z), (0.5, 0.1, 0.2, 0.3));
        assert_eq!(crate::Quaternion::from(g), q);
        for (y, p, r) in [(30.0f64, 10.0f64, -5.0f64), (250.0, -80.0, 170.0), (0.0, 89.9, 45.0)] {
            let theirs = glam::DQuat::from_euler(glam::EulerRot::ZYX, y.to_radians(), p.to_radians(), r.to_radians());
            assert!(
                same_rotation(crate::Attitude::new(y, p, r).to_quaternion(), theirs.into()),
                "{y} {p} {r}"
            );
        }
    }

    #[cfg(feature = "nalgebra")]
    #[test]
    fn nalgebra_quaternion_agrees() {
        let q = crate::Quaternion::new(0.5, 0.1, 0.2, 0.3);
        let n: nalgebra::Quaternion<f64> = q.into();
        assert_eq!((n.w, n.i, n.j, n.k), (0.5, 0.1, 0.2, 0.3));
        assert_eq!(crate::Quaternion::from(n), q);
        for (y, p, r) in [(30.0f64, 10.0f64, -5.0f64), (250.0, -80.0, 170.0), (0.0, 89.9, 45.0)] {
            let theirs = nalgebra::UnitQuaternion::from_euler_angles(r.to_radians(), p.to_radians(), y.to_radians());
            assert!(
                same_rotation(crate::Attitude::new(y, p, r).to_quaternion(), theirs.into()),
                "{y} {p} {r}"
            );
        }
    }

    #[cfg(feature = "geo-types")]
    #[test]
    fn geo_types_uses_lon_x_lat_y_and_drops_height() {
        let g = Geodetic::new(-33.87, 151.21, 20.0); // lat, lon, height
        let p: geo_types::Point<f64> = g.into();
        assert_eq!((p.x(), p.y()), (151.21, -33.87)); // x = lon, y = lat
        let back = Geodetic::from(p);
        assert_eq!((back.lat_deg, back.lon_deg, back.height_m), (-33.87, 151.21, 0.0));
        let c: geo_types::Coord<f64> = g.into();
        assert_eq!((c.x, c.y), (151.21, -33.87));
        assert_eq!(Geodetic::from(c).lat_deg, -33.87);
    }
}