Skip to main content

qualia_core_db/domains/geospatial/geodesy/
wgs84_karney.rs

1// Copyright (c) EarthCraft contributors
2// Adapted under MIT License for QualiaDB.
3//! High-precision WGS84 geodesy and ellipsoidal geometric operations.
4//!
5//! Provides Vincenty/Karney geodesics, Bowring closed-form ECEF/geodetic conversion,
6//! ENU tangent bases, principal radii of curvature, and authalic polygon area.
7//!
8//! Strictly enforces Tier-1 `HotZeroHeap` (zero allocations in all evaluation paths).
9
10use std::f64::consts::PI;
11
12pub const WGS84_A: f64 = 6378137.0;
13pub const WGS84_INV_F: f64 = 298.257223563;
14pub const WGS84_F: f64 = 1.0 / WGS84_INV_F;
15pub const WGS84_B: f64 = 6356752.314245179;
16pub const WGS84_E2: f64 = 6.694379990141317e-3;
17pub const WGS84_EP2: f64 = (WGS84_A * WGS84_A - WGS84_B * WGS84_B) / (WGS84_B * WGS84_B);
18pub const MEAN_EARTH_RADIUS_M: f64 = 6371008.8;
19
20/// Geodetic coordinates (latitude, longitude in degrees, ellipsoidal height in meters).
21#[derive(Debug, Clone, Copy, PartialEq)]
22pub struct GeodeticCoord {
23    pub lat_deg: f64,
24    pub lon_deg: f64,
25    pub alt_m: f64,
26}
27
28impl GeodeticCoord {
29    #[inline]
30    pub const fn new(lat_deg: f64, lon_deg: f64, alt_m: f64) -> Self {
31        Self { lat_deg, lon_deg, alt_m }
32    }
33}
34
35/// Earth-Centered, Earth-Fixed (ECEF) Cartesian coordinates in meters.
36#[derive(Debug, Clone, Copy, PartialEq)]
37pub struct EcefCoord {
38    pub x: f64,
39    pub y: f64,
40    pub z: f64,
41}
42
43impl EcefCoord {
44    #[inline]
45    pub const fn new(x: f64, y: f64, z: f64) -> Self {
46        Self { x, y, z }
47    }
48}
49
50/// Result of an inverse geodesic computation between two points on the ellipsoid.
51#[derive(Debug, Clone, Copy, PartialEq)]
52pub struct GeodesicInverseResult {
53    pub distance_m: f64,
54    pub initial_bearing_deg: f64,
55    pub final_bearing_deg: f64,
56    pub iterations: u32,
57}
58
59/// Result of a direct geodesic computation from a starting point, bearing, and distance.
60#[derive(Debug, Clone, Copy, PartialEq)]
61pub struct GeodesicDirectResult {
62    pub lat_deg: f64,
63    pub lon_deg: f64,
64    pub final_bearing_deg: f64,
65}
66
67#[inline]
68fn wrap_rad(rad: f64) -> f64 {
69    (rad + PI).rem_euclid(2.0 * PI) - PI
70}
71
72#[inline]
73fn normalize_deg(deg: f64) -> f64 {
74    deg.rem_euclid(360.0)
75}
76
77#[inline]
78fn vincenty_ab(cos2_a: f64) -> (f64, f64) {
79    let u2 = cos2_a * WGS84_EP2;
80    let a = 1.0 + (u2 / 16384.0) * (4096.0 + u2 * (-768.0 + u2 * (320.0 - 175.0 * u2)));
81    let b = (u2 / 1024.0) * (256.0 + u2 * (-128.0 + u2 * (74.0 - 47.0 * u2)));
82    (a, b)
83}
84
85#[inline]
86fn vincenty_delta_sigma(b: f64, sin_s: f64, cos_s: f64, cos2_sm: f64) -> f64 {
87    let c2 = cos2_sm * cos2_sm;
88    b * sin_s
89        * (cos2_sm
90            + (b / 4.0)
91                * (cos_s * (-1.0 + 2.0 * c2)
92                    - (b / 6.0) * cos2_sm * (-3.0 + 4.0 * sin_s * sin_s) * (-3.0 + 4.0 * c2)))
93}
94
95#[inline]
96fn vincenty_dlambda(cos2_a: f64, sin_a: f64, sigma: f64, sin_s: f64, cos_s: f64, cos2_sm: f64) -> f64 {
97    let c = (WGS84_F / 16.0) * cos2_a * (4.0 + WGS84_F * (4.0 - 3.0 * cos2_a));
98    (1.0 - c) * WGS84_F * sin_a * (sigma + c * sin_s * (cos2_sm + c * cos_s * (-1.0 + 2.0 * cos2_sm * cos2_sm)))
99}
100
101/// Converts geodetic latitude/longitude/altitude to ECEF coordinates.
102pub fn geodetic_to_ecef(c: GeodeticCoord) -> EcefCoord {
103    let (s_lat, c_lat) = (c.lat_deg.to_radians().sin(), c.lat_deg.to_radians().cos());
104    let (s_lon, c_lon) = (c.lon_deg.to_radians().sin(), c.lon_deg.to_radians().cos());
105    let n = WGS84_A / (1.0 - WGS84_E2 * s_lat * s_lat).sqrt();
106    EcefCoord {
107        x: (n + c.alt_m) * c_lat * c_lon,
108        y: (n + c.alt_m) * c_lat * s_lon,
109        z: (n * (1.0 - WGS84_E2) + c.alt_m) * s_lat,
110    }
111}
112
113/// Converts ECEF coordinates to geodetic coordinates using Bowring's multi-step refinement.
114pub fn ecef_to_geodetic_bowring(ecef: EcefCoord) -> GeodeticCoord {
115    let p = (ecef.x * ecef.x + ecef.y * ecef.y).sqrt();
116    if p < 1e-12 {
117        let lat_deg = if ecef.z >= 0.0 { 90.0 } else { -90.0 };
118        return GeodeticCoord::new(lat_deg, 0.0, ecef.z.abs() - WGS84_B);
119    }
120    let lon_rad = ecef.y.atan2(ecef.x);
121    let mut theta = (ecef.z * WGS84_A).atan2(p * WGS84_B);
122    let mut lat_rad = 0.0;
123    for _ in 0..3 {
124        let (st, ct) = (theta.sin(), theta.cos());
125        lat_rad = (ecef.z + WGS84_EP2 * WGS84_B * st.powi(3))
126            .atan2(p - WGS84_E2 * WGS84_A * ct.powi(3));
127        theta = (WGS84_B * lat_rad.sin()).atan2(WGS84_A * lat_rad.cos());
128    }
129    let (s_lat, c_lat) = (lat_rad.sin(), lat_rad.cos());
130    let w = (1.0 - WGS84_E2 * s_lat * s_lat).sqrt();
131    let alt_m = p * c_lat + ecef.z * s_lat - WGS84_A * w;
132    GeodeticCoord::new(lat_rad.to_degrees(), lon_rad.to_degrees(), alt_m)
133}
134
135/// Computes principal radii of curvature in meters: `(meridional_radius_m, prime_vertical_radius_n)`.
136pub fn principal_radii_of_curvature(lat_deg: f64) -> (f64, f64) {
137    let s = lat_deg.to_radians().sin();
138    let denom = 1.0 - WGS84_E2 * s * s;
139    let root = denom.sqrt();
140    (WGS84_A * (1.0 - WGS84_E2) / (denom * root), WGS84_A / root)
141}
142
143/// Orthonormal rotation matrix transforming ECEF delta vectors into local ENU `[East, North, Up]`.
144pub fn enu_basis(lat_deg: f64, lon_deg: f64) -> [[f64; 3]; 3] {
145    let (s_lat, c_lat) = (lat_deg.to_radians().sin(), lat_deg.to_radians().cos());
146    let (s_lon, c_lon) = (lon_deg.to_radians().sin(), lon_deg.to_radians().cos());
147    [
148        [-s_lon, c_lon, 0.0],
149        [-s_lat * c_lon, -s_lat * s_lon, c_lat],
150        [c_lat * c_lon, c_lat * s_lon, s_lat],
151    ]
152}
153
154/// Transforms an ECEF point to local ENU tangent plane relative to an anchor.
155pub fn ecef_to_enu(point: EcefCoord, anchor: GeodeticCoord) -> [f64; 3] {
156    let anc = geodetic_to_ecef(anchor);
157    let (dx, dy, dz) = (point.x - anc.x, point.y - anc.y, point.z - anc.z);
158    let r = enu_basis(anchor.lat_deg, anchor.lon_deg);
159    [
160        r[0][0] * dx + r[0][1] * dy + r[0][2] * dz,
161        r[1][0] * dx + r[1][1] * dy + r[1][2] * dz,
162        r[2][0] * dx + r[2][1] * dy + r[2][2] * dz,
163    ]
164}
165
166/// Transforms local ENU coordinates `[East, North, Up]` relative to an anchor back into ECEF.
167pub fn enu_to_ecef(enu: [f64; 3], anchor: GeodeticCoord) -> EcefCoord {
168    let anc = geodetic_to_ecef(anchor);
169    let r = enu_basis(anchor.lat_deg, anchor.lon_deg);
170    let dx = r[0][0] * enu[0] + r[1][0] * enu[1] + r[2][0] * enu[2];
171    let dy = r[0][1] * enu[0] + r[1][1] * enu[1] + r[2][1] * enu[2];
172    let dz = r[0][2] * enu[0] + r[1][2] * enu[1] + r[2][2] * enu[2];
173    EcefCoord::new(anc.x + dx, anc.y + dy, anc.z + dz)
174}
175
176/// Geodesic inverse problem on WGS84 with antipodal fallback.
177pub fn geodesic_inverse(lat1_deg: f64, lon1_deg: f64, lat2_deg: f64, lon2_deg: f64) -> GeodesicInverseResult {
178    let (phi1, phi2) = (lat1_deg.to_radians(), lat2_deg.to_radians());
179    let (l1, l2) = (lon1_deg.to_radians(), lon2_deg.to_radians());
180    let d_lon = wrap_rad(l2 - l1);
181
182    if (lat1_deg - lat2_deg).abs() < 1e-12 && d_lon.abs() < 1e-12 {
183        return GeodesicInverseResult { distance_m: 0.0, initial_bearing_deg: 0.0, final_bearing_deg: 0.0, iterations: 0 };
184    }
185
186    let (u1, u2) = (((1.0 - WGS84_F) * phi1.tan()).atan(), ((1.0 - WGS84_F) * phi2.tan()).atan());
187    let (sin_u1, cos_u1) = (u1.sin(), u1.cos());
188    let (sin_u2, cos_u2) = (u2.sin(), u2.cos());
189
190    let (mut lambda, mut iters, mut converged) = (d_lon, 0, false);
191    let (mut sin_s, mut cos_s, mut sigma, mut cos2_a, mut cos2_sm) = (0.0, 1.0, 0.0, 0.0, 0.0);
192
193    for i in 0..200 {
194        iters = i + 1;
195        let (sin_l, cos_l) = (lambda.sin(), lambda.cos());
196        let (t1, t2) = (cos_u2 * sin_l, cos_u1 * sin_u2 - sin_u1 * cos_u2 * cos_l);
197        sin_s = (t1 * t1 + t2 * t2).sqrt();
198        if sin_s.abs() < 1e-15 {
199            cos_s = 1.0; sigma = 0.0; converged = true; break;
200        }
201        cos_s = sin_u1 * sin_u2 + cos_u1 * cos_u2 * cos_l;
202        sigma = sin_s.atan2(cos_s);
203        let sin_a = cos_u1 * cos_u2 * sin_l / sin_s;
204        cos2_a = 1.0 - sin_a * sin_a;
205        cos2_sm = if cos2_a.abs() > 1e-12 { cos_s - 2.0 * sin_u1 * sin_u2 / cos2_a } else { 0.0 };
206
207        let lambda_next = d_lon + vincenty_dlambda(cos2_a, sin_a, sigma, sin_s, cos_s, cos2_sm);
208        if (lambda_next - lambda).abs() < 1e-12 {
209            lambda = lambda_next; converged = true; break;
210        }
211        lambda = lambda_next;
212    }
213
214    if !converged {
215        let h = ((phi2 - phi1) * 0.5).sin().powi(2) + phi1.cos() * phi2.cos() * (d_lon * 0.5).sin().powi(2);
216        let dist = MEAN_EARTH_RADIUS_M * 2.0 * h.clamp(0.0, 1.0).sqrt().asin();
217        let (y1, x1) = (d_lon.sin() * phi2.cos(), phi1.cos() * phi2.sin() - phi1.sin() * phi2.cos() * d_lon.cos());
218        let (y2, x2) = (d_lon.sin() * phi1.cos(), -phi2.cos() * phi1.sin() + phi2.sin() * phi1.cos() * d_lon.cos());
219        return GeodesicInverseResult {
220            distance_m: dist,
221            initial_bearing_deg: normalize_deg(y1.atan2(x1).to_degrees()),
222            final_bearing_deg: normalize_deg(y2.atan2(x2).to_degrees()),
223            iterations: iters,
224        };
225    }
226
227    let (a_param, b_param) = vincenty_ab(cos2_a);
228    let delta_s = vincenty_delta_sigma(b_param, sin_s, cos_s, cos2_sm);
229    let distance_m = WGS84_B * a_param * (sigma - delta_s);
230    let (sin_l, cos_l) = (lambda.sin(), lambda.cos());
231    let init_brg = (cos_u2 * sin_l).atan2(cos_u1 * sin_u2 - sin_u1 * cos_u2 * cos_l);
232    let fin_brg = (cos_u1 * sin_l).atan2(-sin_u1 * cos_u2 + cos_u1 * sin_u2 * cos_l);
233
234    GeodesicInverseResult {
235        distance_m,
236        initial_bearing_deg: normalize_deg(init_brg.to_degrees()),
237        final_bearing_deg: normalize_deg(fin_brg.to_degrees()),
238        iterations: iters,
239    }
240}
241
242/// Geodesic direct problem on WGS84.
243pub fn geodesic_direct(lat1_deg: f64, lon1_deg: f64, bearing_deg: f64, distance_m: f64) -> GeodesicDirectResult {
244    if distance_m.abs() < 1e-9 {
245        return GeodesicDirectResult { lat_deg: lat1_deg, lon_deg: lon1_deg, final_bearing_deg: normalize_deg(bearing_deg) };
246    }
247
248    let (phi1, alpha1) = (lat1_deg.to_radians(), bearing_deg.to_radians());
249    let u1 = ((1.0 - WGS84_F) * phi1.tan()).atan();
250    let (sin_u1, cos_u1) = (u1.sin(), u1.cos());
251    let sigma1 = u1.tan().atan2(alpha1.cos());
252    let sin_a = cos_u1 * alpha1.sin();
253    let cos2_a = 1.0 - sin_a * sin_a;
254    let (a_param, b_param) = vincenty_ab(cos2_a);
255
256    let mut sigma = distance_m / (WGS84_B * a_param);
257    let (mut sin_s, mut cos_s, mut cos2_sm) = (0.0, 1.0, 0.0);
258
259    for _ in 0..100 {
260        cos2_sm = (2.0 * sigma1 + sigma).cos();
261        sin_s = sigma.sin();
262        cos_s = sigma.cos();
263        let delta_s = vincenty_delta_sigma(b_param, sin_s, cos_s, cos2_sm);
264        let sigma_next = distance_m / (WGS84_B * a_param) + delta_s;
265        if (sigma_next - sigma).abs() < 1e-12 {
266            sigma = sigma_next; break;
267        }
268        sigma = sigma_next;
269    }
270
271    let x = sin_u1 * sin_s - cos_u1 * cos_s * alpha1.cos();
272    let phi2 = (sin_u1 * cos_s + cos_u1 * sin_s * alpha1.cos())
273        .atan2((1.0 - WGS84_F) * (sin_a * sin_a + x * x).sqrt());
274    let lambda = (sin_s * alpha1.sin()).atan2(cos_u1 * cos_s - sin_u1 * sin_s * alpha1.cos());
275    let l = lambda - vincenty_dlambda(cos2_a, sin_a, sigma, sin_s, cos_s, cos2_sm);
276    let lon2_deg = (lon1_deg + l.to_degrees() + 180.0).rem_euclid(360.0) - 180.0;
277    let alpha2 = sin_a.atan2(-x);
278
279    GeodesicDirectResult {
280        lat_deg: phi2.to_degrees(),
281        lon_deg: lon2_deg,
282        final_bearing_deg: normalize_deg(alpha2.to_degrees()),
283    }
284}
285
286/// Computes intermediate waypoint along the geodesic at parametric fraction `[0.0, 1.0]`.
287pub fn geodesic_intermediate(lat1_deg: f64, lon1_deg: f64, lat2_deg: f64, lon2_deg: f64, fraction: f64) -> (f64, f64) {
288    if fraction <= 0.0 {
289        return (lat1_deg, lon1_deg);
290    }
291    if fraction >= 1.0 {
292        return (lat2_deg, lon2_deg);
293    }
294    let inv = geodesic_inverse(lat1_deg, lon1_deg, lat2_deg, lon2_deg);
295    let dir = geodesic_direct(lat1_deg, lon1_deg, inv.initial_bearing_deg, inv.distance_m * fraction);
296    (dir.lat_deg, dir.lon_deg)
297}
298
299/// Zero-heap ellipsoidal polygon area in square meters using authalic latitude mapping.
300pub fn geodesic_polygon_area_m2(ring_lat_lon_deg: &[(f64, f64)]) -> f64 {
301    let len = ring_lat_lon_deg.len();
302    if len < 3 {
303        return 0.0;
304    }
305    let n = if len > 1 && ring_lat_lon_deg[0] == ring_lat_lon_deg[len - 1] { len - 1 } else { len };
306    if n < 3 {
307        return 0.0;
308    }
309
310    let e = WGS84_E2.sqrt();
311    let q_p = 1.0 + ((1.0 - WGS84_E2) / e) * e.atanh();
312    let r_auth2 = (WGS84_A * WGS84_A * q_p) * 0.5;
313
314    let authalic_sin = |lat_deg: f64| -> f64 {
315        let s = lat_deg.to_radians().sin();
316        let es = (e * s).clamp(-0.99999999999999, 0.99999999999999);
317        let q = (1.0 - WGS84_E2) * (s / (1.0 - WGS84_E2 * s * s) + (1.0 / e) * es.atanh());
318        (q / q_p).clamp(-1.0, 1.0)
319    };
320
321    let mut sum = 0.0;
322    for i in 0..n {
323        let j = (i + 1) % n;
324        let (lat1, lon1) = ring_lat_lon_deg[i];
325        let (lat2, lon2) = ring_lat_lon_deg[j];
326        let dlon = wrap_rad((lon2 - lon1).to_radians());
327        let s1 = authalic_sin(lat1);
328        let s2 = authalic_sin(lat2);
329        sum += dlon * (2.0 + s1 + s2);
330    }
331
332    (sum * 0.5 * r_auth2).abs()
333}
334
335/// Web Mercator ground resolution in meters per pixel at a given latitude and zoom level.
336pub fn ground_resolution_m_per_px(lat_deg: f64, zoom: u8, tile_px: u32) -> f64 {
337    let cos_lat = lat_deg.to_radians().cos().abs();
338    let total_px = (tile_px as f64) * (2.0_f64).powi(zoom as i32);
339    (2.0 * PI * WGS84_A * cos_lat) / total_px
340}
341
342#[cfg(test)]
343mod tests {
344    use super::*;
345
346    #[test]
347    fn test_sydney_kosciuszko_geodesic_roundtrip() {
348        let (syd_lat, syd_lon) = (-33.8688, 151.2093);
349        let (kos_lat, kos_lon) = (-36.4558, 148.2636);
350
351        let inv = geodesic_inverse(syd_lat, syd_lon, kos_lat, kos_lon);
352        assert!(inv.distance_m > 380_000.0 && inv.distance_m < 410_000.0);
353        assert!(inv.iterations > 0 && inv.iterations < 20);
354
355        let dir = geodesic_direct(syd_lat, syd_lon, inv.initial_bearing_deg, inv.distance_m);
356        let error_check = geodesic_inverse(dir.lat_deg, dir.lon_deg, kos_lat, kos_lon);
357        assert!(error_check.distance_m < 1e-3, "Endpoint error: {} m", error_check.distance_m);
358        assert!((dir.final_bearing_deg - inv.final_bearing_deg).abs() < 1e-4);
359    }
360
361    #[test]
362    fn test_bowring_roundtrip_altitudes() {
363        let test_points = [
364            GeodeticCoord::new(-33.8688, 151.2093, 0.0),       // Sydney sea level
365            GeodeticCoord::new(-36.4558, 148.2636, 2228.0),    // Mt Kosciuszko summit
366            GeodeticCoord::new(45.0, 10.0, 400_000.0),          // LEO satellite orbit
367        ];
368
369        for pt in test_points {
370            let ecef = geodetic_to_ecef(pt);
371            let recovered = ecef_to_geodetic_bowring(ecef);
372            assert!((pt.lat_deg - recovered.lat_deg).abs() < 1e-8, "Lat mismatch: {:?}", pt);
373            assert!((pt.lon_deg - recovered.lon_deg).abs() < 1e-8, "Lon mismatch: {:?}", pt);
374            assert!((pt.alt_m - recovered.alt_m).abs() < 1e-3, "Alt mismatch: {:?}", pt);
375        }
376    }
377
378    #[test]
379    fn test_enu_ecef_roundtrip() {
380        let anchor = GeodeticCoord::new(-36.4558, 148.2636, 2228.0);
381        let enu_original = [1250.5, -3420.25, 450.0];
382
383        let ecef = enu_to_ecef(enu_original, anchor);
384        let enu_recovered = ecef_to_enu(ecef, anchor);
385
386        assert!((enu_original[0] - enu_recovered[0]).abs() < 1e-6);
387        assert!((enu_original[1] - enu_recovered[1]).abs() < 1e-6);
388        assert!((enu_original[2] - enu_recovered[2]).abs() < 1e-6);
389    }
390
391    #[test]
392    fn test_geodesic_polygon_area_1km_square() {
393        let (p0_lat, p0_lon) = (-36.4558, 148.2636);
394
395        let p1 = geodesic_direct(p0_lat, p0_lon, 90.0, 1000.0);
396        let p2 = geodesic_direct(p1.lat_deg, p1.lon_deg, 0.0, 1000.0);
397        let p3 = geodesic_direct(p0_lat, p0_lon, 0.0, 1000.0);
398
399        let square = [
400            (p0_lat, p0_lon),
401            (p1.lat_deg, p1.lon_deg),
402            (p2.lat_deg, p2.lon_deg),
403            (p3.lat_deg, p3.lon_deg),
404            (p0_lat, p0_lon),
405        ];
406
407        let area = geodesic_polygon_area_m2(&square);
408        let diff = (area - 1_000_000.0).abs();
409        assert!(diff < 1000.0, "Expected ~1,000,000 m^2, got {} (diff: {})", area, diff);
410    }
411
412    #[test]
413    fn test_principal_radii_of_curvature() {
414        let (m_eq, n_eq) = principal_radii_of_curvature(0.0);
415        assert!((n_eq - WGS84_A).abs() < 1e-3);
416        assert!((m_eq - WGS84_A * (1.0 - WGS84_E2)).abs() < 1e-3);
417
418        let (m_pole, n_pole) = principal_radii_of_curvature(90.0);
419        assert!((m_pole - n_pole).abs() < 1e-3);
420        assert!((m_pole - (WGS84_A * WGS84_A / WGS84_B)).abs() < 1e-2);
421    }
422
423    #[test]
424    fn test_ground_resolution() {
425        let res_eq_z0 = ground_resolution_m_per_px(0.0, 0, 256);
426        assert!((res_eq_z0 - 156543.0339).abs() < 1.0);
427        let res_eq_z1 = ground_resolution_m_per_px(0.0, 1, 256);
428        assert!((res_eq_z1 - res_eq_z0 * 0.5).abs() < 1e-6);
429    }
430}