cumulus3d_align/
geodesy.rs

1/*
2 * geodesy.rs
3 *
4 * Copyright (c) 2026 ParkSangWoo
5 *
6 * Author(s):
7 *
8 *      ParkSangWoo <dev.parksangwoo@gmail.com>
9 *
10 *
11 * This file is part of cumulus3d.
12 *
13 * Licensed under the Apache License, Version 2.0 (the "License");
14 * you may not use this file except in compliance with the License.
15 * You may obtain a copy of the License at
16 *
17 *      http://www.apache.org/licenses/LICENSE-2.0
18 *
19 * Unless required by applicable law or agreed to in writing, software
20 * distributed under the License is distributed on an "AS IS" BASIS,
21 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22 * See the License for the specific language governing permissions and
23 * limitations under the License.
24 *
25 * Third-party notices: parts of the algorithms, default parameters and data
26 * formats in this file follow other open-source projects. Their copyright
27 * notices and licenses are reproduced in THIRD_PARTY_NOTICES.md.
28 *
29 * SPDX-License-Identifier: Apache-2.0
30 */
31
32//! WGS84 geodetic conversion. Angular inputs and outputs are in degrees.
33
34use cumulus3d_core::{Mat3, Vec3};
35
36/// Semi-major axis a (m).
37pub const WGS84_A: f64 = 6378137.0;
38/// Flattening f.
39pub const WGS84_F: f64 = 1.0 / 298.257223563;
40/// Semi-minor axis b = (1 − f) a (m).
41pub const WGS84_B: f64 = (1.0 - WGS84_F) * WGS84_A;
42/// First eccentricity squared e² = f (2 − f).
43pub const WGS84_E2: f64 = WGS84_F * (2.0 - WGS84_F);
44
45/// Prime vertical radius of curvature N(φ).
46fn prime_vertical_radius(sin_lat: f64) -> f64 {
47    WGS84_A / (1.0 - WGS84_E2 * sin_lat * sin_lat).sqrt()
48}
49
50/// (latitude°, longitude°, ellipsoidal height m) to ECEF (m).
51pub fn lla_to_ecef(lat_deg: f64, lon_deg: f64, alt: f64) -> Vec3 {
52    let (sp, cp) = lat_deg.to_radians().sin_cos();
53    let (sl, cl) = lon_deg.to_radians().sin_cos();
54    let n = prime_vertical_radius(sp);
55    Vec3::new((n + alt) * cp * cl, (n + alt) * cp * sl, (n * (1.0 - WGS84_E2) + alt) * sp)
56}
57
58/// ECEF to (latitude°, longitude°, altitude m). Iterative (at most 100 iterations, stops when the change < 1e−12).
59pub fn ecef_to_lla(p: &Vec3) -> (f64, f64, f64) {
60    let (x, y, z) = (p.x, p.y, p.z);
61    let rho = (x * x + y * y).sqrt();
62    let lon = y.atan2(x);
63    let mut lat = z.atan2(rho);
64    let mut alt = 0.0f64;
65    // Design decision: at the poles (ρ=0) the iteration divides by zero. In that case use the closed-form solution (±90°, |Z| − b).
66    if rho < 1e-9 {
67        let lat = if z >= 0.0 { 90.0 } else { -90.0 };
68        return (lat, lon.to_degrees(), z.abs() - WGS84_B);
69    }
70    for _ in 0..100 {
71        let s = lat.sin();
72        let n = prime_vertical_radius(s);
73        let new_alt = rho / lat.cos() - n;
74        let new_lat = ((z / rho) / (1.0 - WGS84_E2 * n / (n + new_alt))).atan();
75        let done = (new_lat - lat).abs() < 1e-12 && (new_alt - alt).abs() < 1e-12;
76        lat = new_lat;
77        alt = new_alt;
78        if done {
79            break;
80        }
81    }
82    (lat.to_degrees(), lon.to_degrees(), alt)
83}
84
85/// Local ENU coordinate frame (fixed origin). `p_enu = R (p_ecef − r0)`.
86#[derive(Clone, Copy, Debug, PartialEq)]
87pub struct EnuFrame {
88    /// Origin (latitude°, longitude°, altitude m) — input values.
89    pub origin_lla: (f64, f64, f64),
90    /// Origin in ECEF.
91    pub origin_ecef: Vec3,
92    /// ECEF to ENU rotation (rows: E, N, U).
93    pub rotation: Mat3,
94}
95
96impl EnuFrame {
97    /// Creates from the origin LLA. Recomputes the origin latitude/longitude from ECEF iteratively to build the rotation
98    /// (numerically equal to the input; default behavior for bitwise reproducibility of results).
99    pub fn new(lat_deg: f64, lon_deg: f64, alt: f64) -> Self {
100        let r0 = lla_to_ecef(lat_deg, lon_deg, alt);
101        let (lat0, lon0, _) = ecef_to_lla(&r0);
102        let (sp, cp) = lat0.to_radians().sin_cos();
103        let (sl, cl) = lon0.to_radians().sin_cos();
104        #[rustfmt::skip]
105        let rotation = Mat3::new(
106            -sl,       cl,      0.0,
107            -sp * cl, -sp * sl, cp,
108            cp * cl,   cp * sl, sp,
109        );
110        Self { origin_lla: (lat_deg, lon_deg, alt), origin_ecef: r0, rotation }
111    }
112    /// ECEF to ENU of this frame.
113    pub fn ecef_to_enu(&self, p: &Vec3) -> Vec3 {
114        self.rotation * (p - self.origin_ecef)
115    }
116    /// ENU → ECEF.
117    pub fn enu_to_ecef(&self, e: &Vec3) -> Vec3 {
118        self.rotation.transpose() * e + self.origin_ecef
119    }
120    /// (latitude°, longitude°, ellipsoidal height m) to ENU.
121    pub fn lla_to_enu(&self, lat_deg: f64, lon_deg: f64, alt: f64) -> Vec3 {
122        self.ecef_to_enu(&lla_to_ecef(lat_deg, lon_deg, alt))
123    }
124    /// ENU to (latitude°, longitude°, ellipsoidal height m).
125    pub fn enu_to_lla(&self, e: &Vec3) -> (f64, f64, f64) {
126        ecef_to_lla(&self.enu_to_ecef(e))
127    }
128}
129
130#[cfg(test)]
131mod tests {
132    use super::*;
133    use rand::RngExt;
134
135    fn close(a: &Vec3, b: &Vec3, tol: f64) {
136        assert!((a - b).norm() < tol, "{a:?} vs {b:?}");
137    }
138
139    #[test]
140    fn constants() {
141        assert!((WGS84_B - 6356752.314245179).abs() < 1e-8);
142        assert!((WGS84_E2 - 0.0066943799901413165).abs() < 1e-18);
143    }
144
145    #[test]
146    fn lla_to_ecef_known() {
147        close(&lla_to_ecef(0.0, 0.0, 0.0), &Vec3::new(6378137.0, 0.0, 0.0), 1e-4);
148        close(&lla_to_ecef(90.0, 0.0, 0.0), &Vec3::new(0.0, 0.0, 6356752.314245), 1e-4);
149        close(&lla_to_ecef(37.5, 127.0, 50.0), &Vec3::new(-3049062.3651, 4046242.4224, 3861594.5369), 1e-4);
150    }
151
152    #[test]
153    fn enu_known() {
154        let f = EnuFrame::new(37.5, 127.0, 50.0);
155        close(&f.lla_to_enu(37.5, 127.0, 50.0), &Vec3::zeros(), 1e-9);
156        close(&f.lla_to_enu(37.5001, 127.0001, 60.0), &Vec3::new(8.8426, 11.0988, 10.0000), 1e-3);
157    }
158
159    /// Shifting only the longitude by Δ at the same latitude and altitude:
160    /// exact E = (N+h)cosφ sinΔ, N = −sinφ·(N+h)cosφ(cosΔ−1), U = (N+h)cos²φ(cosΔ−1).
161    #[test]
162    fn enu_one_degree_east() {
163        let (lat, lon, h) = (37.5f64, 127.0f64, 50.0f64);
164        let f = EnuFrame::new(lat, lon, h);
165        let e = f.lla_to_enu(lat, lon + 1.0, h);
166        let (sp, cp) = lat.to_radians().sin_cos();
167        let nh = prime_vertical_radius(sp) + h;
168        let d = 1.0f64.to_radians();
169        let expect = Vec3::new(nh * cp * d.sin(), -sp * nh * cp * (d.cos() - 1.0), nh * cp * cp * (d.cos() - 1.0));
170        close(&e, &expect, 1e-6);
171        // Approximation: 1° east ≈ 88.4 km, about −0.6 km up (Earth curvature).
172        assert!((e.x - 88_400.0).abs() < 200.0, "{e:?}");
173        assert!(e.z < 0.0 && e.y > 0.0);
174    }
175
176    #[test]
177    fn ecef_lla_roundtrip() {
178        let mut rng = cumulus3d_core::ransac::make_rng(Some(7));
179        for _ in 0..1000 {
180            let lat = rng.random_range(-89.9..89.9);
181            let lon = rng.random_range(-180.0..180.0);
182            let alt = rng.random_range(-500.0..10000.0);
183            let (la, lo, al) = ecef_to_lla(&lla_to_ecef(lat, lon, alt));
184            assert!((la - lat).abs() < 1e-9 && (lo - lon).abs() < 1e-9 && (al - alt).abs() < 1e-6);
185        }
186    }
187
188    #[test]
189    fn enu_roundtrip() {
190        let f = EnuFrame::new(37.5, 127.0, 50.0);
191        let e = Vec3::new(120.0, -40.0, 33.0);
192        close(&f.lla_to_enu(f.enu_to_lla(&e).0, f.enu_to_lla(&e).1, f.enu_to_lla(&e).2), &e, 1e-6);
193    }
194}
cumulus3d_align/geodesy.rs

cumulus3d_align/
geodesy.rs
