mxmxmx2tiles 0.1.0

//! Transverse Mercator SRS -> ECEF, in a local ENU frame at the scene origin so
//! glTF vertices stay small (float32-safe). TM params come from the scene WKT,
//! so any TM grid works. Inverse-TM is the Redfearn series, validated against
//! PROJ to sub-cm (see tests). Datum shift is assumed zero.

#[derive(Clone, Copy, Debug)]
pub struct TmParams {
    pub a: f64,
    pub f: f64,
    pub lat0: f64,
    pub lon0: f64,
    pub k0: f64,
    pub fe: f64,
    pub fnorth: f64,
}

impl TmParams {
    fn e2(&self) -> f64 {
        self.f * (2.0 - self.f)
    }

    fn meridian_arc(&self, phi: f64) -> f64 {
        let e2 = self.e2();
        let e4 = e2 * e2;
        let e6 = e4 * e2;
        self.a
            * ((1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0) * phi
                - (3.0 * e2 / 8.0 + 3.0 * e4 / 32.0 + 45.0 * e6 / 1024.0) * (2.0 * phi).sin()
                + (15.0 * e4 / 256.0 + 45.0 * e6 / 1024.0) * (4.0 * phi).sin()
                - (35.0 * e6 / 3072.0) * (6.0 * phi).sin())
    }

    pub fn tm_inverse(&self, easting: f64, northing: f64) -> (f64, f64) {
        let e2 = self.e2();
        let ep2 = e2 / (1.0 - e2);
        let m = (northing - self.fnorth) / self.k0 + self.meridian_arc(self.lat0);
        let mu = m / (self.a * (1.0 - e2 / 4.0 - 3.0 * e2 * e2 / 64.0 - 5.0 * e2.powi(3) / 256.0));
        let e1 = (1.0 - (1.0 - e2).sqrt()) / (1.0 + (1.0 - e2).sqrt());

        let phi1 = mu
            + (3.0 * e1 / 2.0 - 27.0 * e1.powi(3) / 32.0) * (2.0 * mu).sin()
            + (21.0 * e1 * e1 / 16.0 - 55.0 * e1.powi(4) / 32.0) * (4.0 * mu).sin()
            + (151.0 * e1.powi(3) / 96.0) * (6.0 * mu).sin()
            + (1097.0 * e1.powi(4) / 512.0) * (8.0 * mu).sin();

        let sp = phi1.sin();
        let cp = phi1.cos();
        let tp = phi1.tan();
        let c1 = ep2 * cp * cp;
        let t1 = tp * tp;
        let n1 = self.a / (1.0 - e2 * sp * sp).sqrt();
        let r1 = self.a * (1.0 - e2) / (1.0 - e2 * sp * sp).powf(1.5);
        let d = (easting - self.fe) / (n1 * self.k0);

        let lat = phi1
            - (n1 * tp / r1)
                * (d * d / 2.0
                    - (5.0 + 3.0 * t1 + 10.0 * c1 - 4.0 * c1 * c1 - 9.0 * ep2) * d.powi(4) / 24.0
                    + (61.0 + 90.0 * t1 + 298.0 * c1 + 45.0 * t1 * t1 - 252.0 * ep2 - 3.0 * c1 * c1)
                        * d.powi(6)
                        / 720.0);

        let lon = self.lon0
            + (d - (1.0 + 2.0 * t1 + c1) * d.powi(3) / 6.0
                + (5.0 - 2.0 * c1 + 28.0 * t1 - 3.0 * c1 * c1 + 8.0 * ep2 + 24.0 * t1 * t1)
                    * d.powi(5)
                    / 120.0)
                / cp;

        (lat, lon)
    }

    pub fn geodetic_to_ecef(&self, lat: f64, lon: f64, h: f64) -> [f64; 3] {
        let e2 = self.e2();
        let n = self.a / (1.0 - e2 * lat.sin() * lat.sin()).sqrt();
        [
            (n + h) * lat.cos() * lon.cos(),
            (n + h) * lat.cos() * lon.sin(),
            (n * (1.0 - e2) + h) * lat.sin(),
        ]
    }

    pub fn proj_to_ecef(&self, easting: f64, northing: f64, h: f64) -> [f64; 3] {
        let (lat, lon) = self.tm_inverse(easting, northing);
        self.geodetic_to_ecef(lat, lon, h)
    }
}

const DEG: f64 = std::f64::consts::PI / 180.0;

pub fn parse_srs(srs: &str) -> Result<TmParams, String> {
    let srs = srs.trim();
    if let Some(code) = srs.strip_prefix("EPSG:").or_else(|| srs.strip_prefix("epsg:")) {
        return epsg_tm(code.trim());
    }
    if !srs.contains("Transverse_Mercator") {
        return Err(format!(
            "unsupported SRS: only Transverse Mercator is handled (got: {:.80})",
            srs
        ));
    }
    let p = |name: &str| wkt_param(srs, name);
    let (a, inv_f) = wkt_spheroid(srs)?;
    let k0 = if srs.contains("PARAMETER[\"scale_factor\"") {
        p("scale_factor")?
    } else {
        1.0
    };
    Ok(TmParams {
        a,
        f: 1.0 / inv_f,
        lat0: p("latitude_of_origin")? * DEG,
        lon0: p("central_meridian")? * DEG,
        k0,
        fe: p("false_easting")?,
        fnorth: p("false_northing")?,
    })
}

const GRS80_A: f64 = 6_378_137.0;
const GRS80_INVF: f64 = 298.257222101;

fn epsg_tm(code: &str) -> Result<TmParams, String> {
    let korea = |lon0: f64| TmParams {
        a: GRS80_A,
        f: 1.0 / GRS80_INVF,
        lat0: 38.0 * DEG,
        lon0: lon0 * DEG,
        k0: 1.0,
        fe: 200_000.0,
        fnorth: 600_000.0,
    };
    Ok(match code {
        "5185" => korea(125.0),
        "5186" => korea(127.0),
        "5187" => korea(129.0),
        "5188" => korea(131.0),
        _ => {
            return Err(format!(
                "EPSG:{code} not in the built-in TM table; provide a full WKT SRS"
            ))
        }
    })
}

fn wkt_param(wkt: &str, name: &str) -> Result<f64, String> {
    // Anchored to PARAMETER[" so a name appearing elsewhere (a CRS/datum name)
    // can't be mistaken for the value.
    let needle = format!("PARAMETER[\"{name}\"");
    let i = wkt
        .find(&needle)
        .ok_or_else(|| format!("SRS missing parameter {name}"))?;
    let after = &wkt[i + needle.len()..];
    let comma = after.find(',').ok_or_else(|| format!("malformed {name}"))?;
    let rest = &after[comma + 1..];
    let end = rest.find([',', ']']).unwrap_or(rest.len());
    rest[..end]
        .trim()
        .parse()
        .map_err(|_| format!("bad value for {name}"))
}

fn wkt_spheroid(wkt: &str) -> Result<(f64, f64), String> {
    let i = wkt.find("SPHEROID").ok_or("SRS missing SPHEROID")?;
    let after = &wkt[i..];
    let q1 = after.find('"').ok_or("malformed SPHEROID")?;
    let q2 = after[q1 + 1..].find('"').ok_or("malformed SPHEROID")? + q1 + 1;
    let nums = &after[q2 + 1..];
    let nums = &nums[nums.find(',').ok_or("malformed SPHEROID")? + 1..];
    let mut it = nums.split(',');
    let a: f64 = it
        .next()
        .and_then(|s| s.trim().parse().ok())
        .ok_or("bad SPHEROID semi-major axis")?;
    let invf_raw = it.next().ok_or("bad SPHEROID inverse flattening")?;
    let end = invf_raw.find([']']).unwrap_or(invf_raw.len());
    let inv_f: f64 = invf_raw[..end]
        .trim()
        .parse()
        .map_err(|_| "bad SPHEROID inverse flattening".to_string())?;
    Ok((a, if inv_f == 0.0 { f64::INFINITY } else { inv_f }))
}

pub struct EnuFrame {
    params: TmParams,
    origin: [f64; 3],
    ecef_o: [f64; 3],
    east: [f64; 3],
    north: [f64; 3],
    up: [f64; 3],
}

impl EnuFrame {
    pub fn new(origin: [f64; 3], params: TmParams) -> Self {
        let (lat, lon) = params.tm_inverse(origin[0], origin[1]);
        let ecef_o = params.geodetic_to_ecef(lat, lon, origin[2]);
        let (sl, cl) = (lon.sin(), lon.cos());
        let (sp, cp) = (lat.sin(), lat.cos());
        EnuFrame {
            params,
            origin,
            ecef_o,
            east: [-sl, cl, 0.0],
            north: [-sp * cl, -sp * sl, cp],
            up: [cp * cl, cp * sl, sp],
        }
    }

    pub fn local_to_enu(&self, local: [f32; 3]) -> [f32; 3] {
        let e = self.origin[0] + local[0] as f64;
        let n = self.origin[1] + local[1] as f64;
        let h = self.origin[2] + local[2] as f64;
        // ECEF subtraction in f64 (cancels the ~6.3e6 m magnitude), only the
        // small ENU result is cast to f32. Heights are treated as ellipsoidal.
        let p = self.params.proj_to_ecef(e, n, h);
        let d = [p[0] - self.ecef_o[0], p[1] - self.ecef_o[1], p[2] - self.ecef_o[2]];
        [
            dot(&self.east, &d) as f32,
            dot(&self.north, &d) as f32,
            dot(&self.up, &d) as f32,
        ]
    }

    pub fn root_transform(&self) -> [f64; 16] {
        [
            self.east[0], self.east[1], self.east[2], 0.0,
            self.north[0], self.north[1], self.north[2], 0.0,
            self.up[0], self.up[1], self.up[2], 0.0,
            self.ecef_o[0], self.ecef_o[1], self.ecef_o[2], 1.0,
        ]
    }
}

fn dot(a: &[f64; 3], b: &[f64; 3]) -> f64 {
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}

#[cfg(test)]
mod tests {
    use super::*;
    const D: f64 = 180.0 / std::f64::consts::PI;

    fn p5186() -> TmParams {
        parse_srs("EPSG:5186").unwrap()
    }

    #[test]
    fn tm_inverse_matches_proj() {
        let (lat, lon) = p5186().tm_inverse(155915.0, 102269.0);
        assert!((lat * D - 33.51319702).abs() < 1e-7, "lat {}", lat * D);
        assert!((lon * D - 126.52550273).abs() < 1e-7, "lon {}", lon * D);
    }

    #[test]
    fn ecef_matches_proj() {
        let p = p5186().proj_to_ecef(155840.0, 102213.0, 66.1);
        let r = [-3168292.8836, 4277844.2800, 3501544.2965];
        for k in 0..3 {
            assert!((p[k] - r[k]).abs() < 0.01, "axis {k}: {} vs {}", p[k], r[k]);
        }
    }

    #[test]
    fn parse_wkt_matches_epsg() {
        let wkt = "PROJCS[\"Korea 2000 / Central Belt 2010\",GEOGCS[\"Korea 2000\",\
            DATUM[\"x\",SPHEROID[\"GRS 1980\",6378137,298.257222101]],PRIMEM[\"Greenwich\",0]],\
            PROJECTION[\"Transverse_Mercator\"],PARAMETER[\"latitude_of_origin\",38],\
            PARAMETER[\"central_meridian\",127],PARAMETER[\"scale_factor\",1],\
            PARAMETER[\"false_easting\",200000],PARAMETER[\"false_northing\",600000],\
            UNIT[\"metre\",1]]";
        let w = parse_srs(wkt).unwrap();
        let (lat, lon) = w.tm_inverse(155915.0, 102269.0);
        assert!((lat * D - 33.51319702).abs() < 1e-7);
        assert!((lon * D - 126.52550273).abs() < 1e-7);
        assert!((w.a - 6378137.0).abs() < 1e-6 && (w.k0 - 1.0).abs() < 1e-9);
    }

    #[test]
    fn rejects_non_tm() {
        assert!(parse_srs("PROJCS[\"x\",PROJECTION[\"Lambert_Conformal_Conic_2SP\"]]").is_err());
    }

    #[test]
    fn enu_roundtrip() {
        let frame = EnuFrame::new([155915.0, 102269.0, 0.0], p5186());
        let local = [272.85f32, -404.1, 72.29];
        let enu = frame.local_to_enu(local);
        let m = frame.root_transform();
        let v = [enu[0] as f64, enu[1] as f64, enu[2] as f64];
        let world = [
            m[0] * v[0] + m[4] * v[1] + m[8] * v[2] + m[12],
            m[1] * v[0] + m[5] * v[1] + m[9] * v[2] + m[13],
            m[2] * v[0] + m[6] * v[1] + m[10] * v[2] + m[14],
        ];
        let truth = p5186().proj_to_ecef(155915.0 + 272.85, 102269.0 - 404.1, 72.29);
        for k in 0..3 {
            assert!((world[k] - truth[k]).abs() < 0.01, "axis {k}");
        }
    }
}