1use 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#[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#[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#[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#[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
101pub 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
113pub 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
135pub 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
143pub 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
154pub 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
166pub 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
176pub 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
242pub 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
286pub 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
299pub 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
335pub 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), GeodeticCoord::new(-36.4558, 148.2636, 2228.0), GeodeticCoord::new(45.0, 10.0, 400_000.0), ];
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}