tmx/
geo.rs

1//! Transverse Mercator SRS -> ECEF, in a local ENU frame at the scene origin so
2//! glTF vertices stay small (float32-safe). TM params come from the scene WKT,
3//! so any TM grid works. Inverse-TM is the Redfearn series, validated against
4//! PROJ to sub-cm (see tests). Datum shift is assumed zero.
5
6#[derive(Clone, Copy, Debug)]
7pub struct TmParams {
8    pub a: f64,
9    pub f: f64,
10    pub lat0: f64,
11    pub lon0: f64,
12    pub k0: f64,
13    pub fe: f64,
14    pub fnorth: f64,
15}
16
17impl TmParams {
18    fn e2(&self) -> f64 {
19        self.f * (2.0 - self.f)
20    }
21
22    fn meridian_arc(&self, phi: f64) -> f64 {
23        let e2 = self.e2();
24        let e4 = e2 * e2;
25        let e6 = e4 * e2;
26        self.a
27            * ((1.0 - e2 / 4.0 - 3.0 * e4 / 64.0 - 5.0 * e6 / 256.0) * phi
28                - (3.0 * e2 / 8.0 + 3.0 * e4 / 32.0 + 45.0 * e6 / 1024.0) * (2.0 * phi).sin()
29                + (15.0 * e4 / 256.0 + 45.0 * e6 / 1024.0) * (4.0 * phi).sin()
30                - (35.0 * e6 / 3072.0) * (6.0 * phi).sin())
31    }
32
33    pub fn tm_inverse(&self, easting: f64, northing: f64) -> (f64, f64) {
34        let e2 = self.e2();
35        let ep2 = e2 / (1.0 - e2);
36        let m = (northing - self.fnorth) / self.k0 + self.meridian_arc(self.lat0);
37        let mu = m / (self.a * (1.0 - e2 / 4.0 - 3.0 * e2 * e2 / 64.0 - 5.0 * e2.powi(3) / 256.0));
38        let e1 = (1.0 - (1.0 - e2).sqrt()) / (1.0 + (1.0 - e2).sqrt());
39
40        let phi1 = mu
41            + (3.0 * e1 / 2.0 - 27.0 * e1.powi(3) / 32.0) * (2.0 * mu).sin()
42            + (21.0 * e1 * e1 / 16.0 - 55.0 * e1.powi(4) / 32.0) * (4.0 * mu).sin()
43            + (151.0 * e1.powi(3) / 96.0) * (6.0 * mu).sin()
44            + (1097.0 * e1.powi(4) / 512.0) * (8.0 * mu).sin();
45
46        let sp = phi1.sin();
47        let cp = phi1.cos();
48        let tp = phi1.tan();
49        let c1 = ep2 * cp * cp;
50        let t1 = tp * tp;
51        let n1 = self.a / (1.0 - e2 * sp * sp).sqrt();
52        let r1 = self.a * (1.0 - e2) / (1.0 - e2 * sp * sp).powf(1.5);
53        let d = (easting - self.fe) / (n1 * self.k0);
54
55        let lat = phi1
56            - (n1 * tp / r1)
57                * (d * d / 2.0
58                    - (5.0 + 3.0 * t1 + 10.0 * c1 - 4.0 * c1 * c1 - 9.0 * ep2) * d.powi(4) / 24.0
59                    + (61.0 + 90.0 * t1 + 298.0 * c1 + 45.0 * t1 * t1 - 252.0 * ep2 - 3.0 * c1 * c1)
60                        * d.powi(6)
61                        / 720.0);
62
63        let lon = self.lon0
64            + (d - (1.0 + 2.0 * t1 + c1) * d.powi(3) / 6.0
65                + (5.0 - 2.0 * c1 + 28.0 * t1 - 3.0 * c1 * c1 + 8.0 * ep2 + 24.0 * t1 * t1)
66                    * d.powi(5)
67                    / 120.0)
68                / cp;
69
70        (lat, lon)
71    }
72
73    pub fn geodetic_to_ecef(&self, lat: f64, lon: f64, h: f64) -> [f64; 3] {
74        let e2 = self.e2();
75        let n = self.a / (1.0 - e2 * lat.sin() * lat.sin()).sqrt();
76        [
77            (n + h) * lat.cos() * lon.cos(),
78            (n + h) * lat.cos() * lon.sin(),
79            (n * (1.0 - e2) + h) * lat.sin(),
80        ]
81    }
82
83    pub fn proj_to_ecef(&self, easting: f64, northing: f64, h: f64) -> [f64; 3] {
84        let (lat, lon) = self.tm_inverse(easting, northing);
85        self.geodetic_to_ecef(lat, lon, h)
86    }
87}
88
89const DEG: f64 = std::f64::consts::PI / 180.0;
90
91pub fn parse_srs(srs: &str) -> Result<TmParams, String> {
92    let srs = srs.trim();
93    if let Some(code) = srs.strip_prefix("EPSG:").or_else(|| srs.strip_prefix("epsg:")) {
94        return epsg_tm(code.trim());
95    }
96    if !srs.contains("Transverse_Mercator") {
97        return Err(format!(
98            "unsupported SRS: only Transverse Mercator is handled (got: {:.80})",
99            srs
100        ));
101    }
102    let p = |name: &str| wkt_param(srs, name);
103    let (a, inv_f) = wkt_spheroid(srs)?;
104    let k0 = if srs.contains("PARAMETER[\"scale_factor\"") {
105        p("scale_factor")?
106    } else {
107        1.0
108    };
109    Ok(TmParams {
110        a,
111        f: 1.0 / inv_f,
112        lat0: p("latitude_of_origin")? * DEG,
113        lon0: p("central_meridian")? * DEG,
114        k0,
115        fe: p("false_easting")?,
116        fnorth: p("false_northing")?,
117    })
118}
119
120const GRS80_A: f64 = 6_378_137.0;
121const GRS80_INVF: f64 = 298.257222101;
122
123fn epsg_tm(code: &str) -> Result<TmParams, String> {
124    let korea = |lon0: f64| TmParams {
125        a: GRS80_A,
126        f: 1.0 / GRS80_INVF,
127        lat0: 38.0 * DEG,
128        lon0: lon0 * DEG,
129        k0: 1.0,
130        fe: 200_000.0,
131        fnorth: 600_000.0,
132    };
133    Ok(match code {
134        "5185" => korea(125.0),
135        "5186" => korea(127.0),
136        "5187" => korea(129.0),
137        "5188" => korea(131.0),
138        _ => {
139            return Err(format!(
140                "EPSG:{code} not in the built-in TM table; provide a full WKT SRS"
141            ))
142        }
143    })
144}
145
146fn wkt_param(wkt: &str, name: &str) -> Result<f64, String> {
147    // Anchored to PARAMETER[" so a name appearing elsewhere (a CRS/datum name)
148    // can't be mistaken for the value.
149    let needle = format!("PARAMETER[\"{name}\"");
150    let i = wkt
151        .find(&needle)
152        .ok_or_else(|| format!("SRS missing parameter {name}"))?;
153    let after = &wkt[i + needle.len()..];
154    let comma = after.find(',').ok_or_else(|| format!("malformed {name}"))?;
155    let rest = &after[comma + 1..];
156    let end = rest.find([',', ']']).unwrap_or(rest.len());
157    rest[..end]
158        .trim()
159        .parse()
160        .map_err(|_| format!("bad value for {name}"))
161}
162
163fn wkt_spheroid(wkt: &str) -> Result<(f64, f64), String> {
164    let i = wkt.find("SPHEROID").ok_or("SRS missing SPHEROID")?;
165    let after = &wkt[i..];
166    let q1 = after.find('"').ok_or("malformed SPHEROID")?;
167    let q2 = after[q1 + 1..].find('"').ok_or("malformed SPHEROID")? + q1 + 1;
168    let nums = &after[q2 + 1..];
169    let nums = &nums[nums.find(',').ok_or("malformed SPHEROID")? + 1..];
170    let mut it = nums.split(',');
171    let a: f64 = it
172        .next()
173        .and_then(|s| s.trim().parse().ok())
174        .ok_or("bad SPHEROID semi-major axis")?;
175    let invf_raw = it.next().ok_or("bad SPHEROID inverse flattening")?;
176    let end = invf_raw.find([']']).unwrap_or(invf_raw.len());
177    let inv_f: f64 = invf_raw[..end]
178        .trim()
179        .parse()
180        .map_err(|_| "bad SPHEROID inverse flattening".to_string())?;
181    Ok((a, if inv_f == 0.0 { f64::INFINITY } else { inv_f }))
182}
183
184pub struct EnuFrame {
185    params: TmParams,
186    origin: [f64; 3],
187    ecef_o: [f64; 3],
188    east: [f64; 3],
189    north: [f64; 3],
190    up: [f64; 3],
191}
192
193impl EnuFrame {
194    pub fn new(origin: [f64; 3], params: TmParams) -> Self {
195        let (lat, lon) = params.tm_inverse(origin[0], origin[1]);
196        let ecef_o = params.geodetic_to_ecef(lat, lon, origin[2]);
197        let (sl, cl) = (lon.sin(), lon.cos());
198        let (sp, cp) = (lat.sin(), lat.cos());
199        EnuFrame {
200            params,
201            origin,
202            ecef_o,
203            east: [-sl, cl, 0.0],
204            north: [-sp * cl, -sp * sl, cp],
205            up: [cp * cl, cp * sl, sp],
206        }
207    }
208
209    pub fn local_to_enu(&self, local: [f32; 3]) -> [f32; 3] {
210        let e = self.origin[0] + local[0] as f64;
211        let n = self.origin[1] + local[1] as f64;
212        let h = self.origin[2] + local[2] as f64;
213        // ECEF subtraction in f64 (cancels the ~6.3e6 m magnitude), only the
214        // small ENU result is cast to f32. Heights are treated as ellipsoidal.
215        let p = self.params.proj_to_ecef(e, n, h);
216        let d = [p[0] - self.ecef_o[0], p[1] - self.ecef_o[1], p[2] - self.ecef_o[2]];
217        [
218            dot(&self.east, &d) as f32,
219            dot(&self.north, &d) as f32,
220            dot(&self.up, &d) as f32,
221        ]
222    }
223
224    pub fn root_transform(&self) -> [f64; 16] {
225        [
226            self.east[0], self.east[1], self.east[2], 0.0,
227            self.north[0], self.north[1], self.north[2], 0.0,
228            self.up[0], self.up[1], self.up[2], 0.0,
229            self.ecef_o[0], self.ecef_o[1], self.ecef_o[2], 1.0,
230        ]
231    }
232}
233
234fn dot(a: &[f64; 3], b: &[f64; 3]) -> f64 {
235    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
236}
237
238#[cfg(test)]
239mod tests {
240    use super::*;
241    const D: f64 = 180.0 / std::f64::consts::PI;
242
243    fn p5186() -> TmParams {
244        parse_srs("EPSG:5186").unwrap()
245    }
246
247    #[test]
248    fn tm_inverse_matches_proj() {
249        let (lat, lon) = p5186().tm_inverse(155915.0, 102269.0);
250        assert!((lat * D - 33.51319702).abs() < 1e-7, "lat {}", lat * D);
251        assert!((lon * D - 126.52550273).abs() < 1e-7, "lon {}", lon * D);
252    }
253
254    #[test]
255    fn ecef_matches_proj() {
256        let p = p5186().proj_to_ecef(155840.0, 102213.0, 66.1);
257        let r = [-3168292.8836, 4277844.2800, 3501544.2965];
258        for k in 0..3 {
259            assert!((p[k] - r[k]).abs() < 0.01, "axis {k}: {} vs {}", p[k], r[k]);
260        }
261    }
262
263    #[test]
264    fn parse_wkt_matches_epsg() {
265        let wkt = "PROJCS[\"Korea 2000 / Central Belt 2010\",GEOGCS[\"Korea 2000\",\
266            DATUM[\"x\",SPHEROID[\"GRS 1980\",6378137,298.257222101]],PRIMEM[\"Greenwich\",0]],\
267            PROJECTION[\"Transverse_Mercator\"],PARAMETER[\"latitude_of_origin\",38],\
268            PARAMETER[\"central_meridian\",127],PARAMETER[\"scale_factor\",1],\
269            PARAMETER[\"false_easting\",200000],PARAMETER[\"false_northing\",600000],\
270            UNIT[\"metre\",1]]";
271        let w = parse_srs(wkt).unwrap();
272        let (lat, lon) = w.tm_inverse(155915.0, 102269.0);
273        assert!((lat * D - 33.51319702).abs() < 1e-7);
274        assert!((lon * D - 126.52550273).abs() < 1e-7);
275        assert!((w.a - 6378137.0).abs() < 1e-6 && (w.k0 - 1.0).abs() < 1e-9);
276    }
277
278    #[test]
279    fn rejects_non_tm() {
280        assert!(parse_srs("PROJCS[\"x\",PROJECTION[\"Lambert_Conformal_Conic_2SP\"]]").is_err());
281    }
282
283    #[test]
284    fn enu_roundtrip() {
285        let frame = EnuFrame::new([155915.0, 102269.0, 0.0], p5186());
286        let local = [272.85f32, -404.1, 72.29];
287        let enu = frame.local_to_enu(local);
288        let m = frame.root_transform();
289        let v = [enu[0] as f64, enu[1] as f64, enu[2] as f64];
290        let world = [
291            m[0] * v[0] + m[4] * v[1] + m[8] * v[2] + m[12],
292            m[1] * v[0] + m[5] * v[1] + m[9] * v[2] + m[13],
293            m[2] * v[0] + m[6] * v[1] + m[10] * v[2] + m[14],
294        ];
295        let truth = p5186().proj_to_ecef(155915.0 + 272.85, 102269.0 - 404.1, 72.29);
296        for k in 0..3 {
297            assert!((world[k] - truth[k]).abs() < 0.01, "axis {k}");
298        }
299    }
300}
tmx/geo.rs

tmx/
geo.rs
