1const WGS84_A: f64 = 6_378_137.0; const WGS84_F: f64 = 1.0 / 298.257223563; const WGS84_B: f64 = WGS84_A * (1.0 - WGS84_F); const WGS84_E2: f64 = WGS84_F * (2.0 - WGS84_F); #[derive(Debug, Clone, Copy, PartialEq)]
13pub struct Geodetic {
14 pub lat_deg: f64,
16 pub lon_deg: f64,
18 pub alt_m: f64,
20}
21
22#[derive(Debug, Clone, Copy, PartialEq)]
24pub struct Ecef {
25 pub x: f64,
26 pub y: f64,
27 pub z: f64,
28}
29
30#[derive(Debug, Clone, Copy, PartialEq)]
32pub struct Enu {
33 pub east: f64,
34 pub north: f64,
35 pub up: f64,
36}
37
38pub fn geodetic_to_ecef(g: Geodetic) -> Ecef {
40 let lat = g.lat_deg.to_radians();
41 let lon = g.lon_deg.to_radians();
42 let sin_lat = lat.sin();
43 let cos_lat = lat.cos();
44 let sin_lon = lon.sin();
45 let cos_lon = lon.cos();
46
47 let n = WGS84_A / (1.0 - WGS84_E2 * sin_lat * sin_lat).sqrt();
49
50 Ecef {
51 x: (n + g.alt_m) * cos_lat * cos_lon,
52 y: (n + g.alt_m) * cos_lat * sin_lon,
53 z: (n * (1.0 - WGS84_E2) + g.alt_m) * sin_lat,
54 }
55}
56
57pub fn ecef_to_geodetic(e: Ecef) -> Geodetic {
59 let x = e.x;
60 let y = e.y;
61 let z = e.z;
62
63 let lon = y.atan2(x);
64
65 let p = (x * x + y * y).sqrt();
66 if p < 1e-12 {
67 return Geodetic {
69 lat_deg: if z > 0.0 { 90.0 } else { -90.0 },
70 lon_deg: 0.0,
71 alt_m: z.abs() - WGS84_B,
72 };
73 }
74
75 let e_prime_sq = (WGS84_A * WGS84_A - WGS84_B * WGS84_B) / (WGS84_B * WGS84_B);
78
79 let theta = (z * WGS84_A).atan2(p * WGS84_B);
81 let sin_theta = theta.sin();
82 let cos_theta = theta.cos();
83
84 let lat = (z + e_prime_sq * WGS84_B * sin_theta * sin_theta * sin_theta)
86 .atan2(p - WGS84_E2 * WGS84_A * cos_theta * cos_theta * cos_theta);
87
88 let sin_lat = lat.sin();
89 let n = WGS84_A / (1.0 - WGS84_E2 * sin_lat * sin_lat).sqrt();
90 let alt = p / lat.cos() - n;
91
92 Geodetic {
93 lat_deg: lat.to_degrees(),
94 lon_deg: lon.to_degrees(),
95 alt_m: alt,
96 }
97}
98
99pub fn ecef_to_enu(e: Ecef, ref_geo: Geodetic) -> Enu {
101 let ref_ecef = geodetic_to_ecef(ref_geo);
102 let dx = e.x - ref_ecef.x;
103 let dy = e.y - ref_ecef.y;
104 let dz = e.z - ref_ecef.z;
105
106 let lat = ref_geo.lat_deg.to_radians();
107 let lon = ref_geo.lon_deg.to_radians();
108 let sin_lat = lat.sin();
109 let cos_lat = lat.cos();
110 let sin_lon = lon.sin();
111 let cos_lon = lon.cos();
112
113 Enu {
115 east: -sin_lon * dx + cos_lon * dy,
116 north: -sin_lat * cos_lon * dx - sin_lat * sin_lon * dy + cos_lat * dz,
117 up: cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz,
118 }
119}
120
121pub fn enu_to_ecef(enu: Enu, ref_geo: Geodetic) -> Ecef {
123 let ref_ecef = geodetic_to_ecef(ref_geo);
124 let lat = ref_geo.lat_deg.to_radians();
125 let lon = ref_geo.lon_deg.to_radians();
126 let sin_lat = lat.sin();
127 let cos_lat = lat.cos();
128 let sin_lon = lon.sin();
129 let cos_lon = lon.cos();
130
131 Ecef {
133 x: ref_ecef.x - sin_lon * enu.east - sin_lat * cos_lon * enu.north
134 + cos_lat * cos_lon * enu.up,
135 y: ref_ecef.y + cos_lon * enu.east - sin_lat * sin_lon * enu.north
136 + cos_lat * sin_lon * enu.up,
137 z: ref_ecef.z + cos_lat * enu.north + sin_lat * enu.up,
138 }
139}
140
141pub fn geodetic_distance(a: Geodetic, b: Geodetic) -> f64 {
144 let lat1 = a.lat_deg.to_radians();
145 let lat2 = b.lat_deg.to_radians();
146 let dlat = lat2 - lat1;
147 let dlon = (b.lon_deg - a.lon_deg).to_radians();
148
149 let sin_dlat = (dlat / 2.0).sin();
150 let sin_dlon = (dlon / 2.0).sin();
151 let h = sin_dlat * sin_dlat + lat1.cos() * lat2.cos() * sin_dlon * sin_dlon;
152 let r = 6_371_008.8;
154 2.0 * r * h.sqrt().asin()
155}
156
157pub fn ecef_to_value(e: Ecef) -> crate::value::Value {
159 let mut rec = std::collections::BTreeMap::new();
160 rec.insert("x".into(), crate::value::Value::F64(e.x));
161 rec.insert("y".into(), crate::value::Value::F64(e.y));
162 rec.insert("z".into(), crate::value::Value::F64(e.z));
163 crate::value::Value::Record(rec)
164}
165
166pub fn geodetic_to_value(g: Geodetic) -> crate::value::Value {
168 let mut rec = std::collections::BTreeMap::new();
169 rec.insert("lat".into(), crate::value::Value::F64(g.lat_deg));
170 rec.insert("lon".into(), crate::value::Value::F64(g.lon_deg));
171 rec.insert("alt".into(), crate::value::Value::F64(g.alt_m));
172 crate::value::Value::Record(rec)
173}
174
175#[derive(Debug, Clone, Copy, PartialEq)]
179pub struct FourVector {
180 pub ct: f64,
181 pub x: f64,
182 pub y: f64,
183 pub z: f64,
184}
185
186impl FourVector {
187 pub fn new(ct: f64, x: f64, y: f64, z: f64) -> Self {
189 Self { ct, x, y, z }
190 }
191
192 pub fn spacetime_interval_sq(&self) -> f64 {
194 -self.ct * self.ct + self.x * self.x + self.y * self.y + self.z * self.z
195 }
196}
197
198#[derive(Debug, Clone, Copy, PartialEq)]
200pub struct LorentzBoost {
201 pub beta_x: f64,
202 pub beta_y: f64,
203 pub beta_z: f64,
204}
205
206impl LorentzBoost {
207 pub fn new(beta_x: f64, beta_y: f64, beta_z: f64) -> Result<Self, &'static str> {
209 let beta_sq = beta_x * beta_x + beta_y * beta_y + beta_z * beta_z;
210 if beta_sq >= 1.0 {
211 return Err("Superluminal boost rejected: |beta| >= 1.0");
212 }
213 Ok(Self {
214 beta_x,
215 beta_y,
216 beta_z,
217 })
218 }
219
220 pub fn gamma(&self) -> f64 {
222 let beta_sq =
223 self.beta_x * self.beta_x + self.beta_y * self.beta_y + self.beta_z * self.beta_z;
224 1.0 / (1.0 - beta_sq).sqrt()
225 }
226
227 pub fn matrix(&self) -> [[f64; 4]; 4] {
229 let g = self.gamma();
230 let beta_sq =
231 self.beta_x * self.beta_x + self.beta_y * self.beta_y + self.beta_z * self.beta_z;
232 if beta_sq < 1e-15 {
233 return [
234 [1.0, 0.0, 0.0, 0.0],
235 [0.0, 1.0, 0.0, 0.0],
236 [0.0, 0.0, 1.0, 0.0],
237 [0.0, 0.0, 0.0, 1.0],
238 ];
239 }
240 let factor = (g - 1.0) / beta_sq;
241 let bx = self.beta_x;
242 let by = self.beta_y;
243 let bz = self.beta_z;
244
245 [
246 [g, -g * bx, -g * by, -g * bz],
247 [
248 -g * bx,
249 1.0 + factor * bx * bx,
250 factor * bx * by,
251 factor * bx * bz,
252 ],
253 [
254 -g * by,
255 factor * by * bx,
256 1.0 + factor * by * by,
257 factor * by * bz,
258 ],
259 [
260 -g * bz,
261 factor * bz * bx,
262 factor * bz * by,
263 1.0 + factor * bz * bz,
264 ],
265 ]
266 }
267
268 pub fn transform(&self, v: FourVector) -> FourVector {
270 let m = self.matrix();
271 let in_vec = [v.ct, v.x, v.y, v.z];
272 let mut out = [0.0; 4];
273 for i in 0..4 {
274 for j in 0..4 {
275 out[i] += m[i][j] * in_vec[j];
276 }
277 }
278 FourVector {
279 ct: out[0],
280 x: out[1],
281 y: out[2],
282 z: out[3],
283 }
284 }
285}
286
287#[cfg(test)]
288mod tests {
289 use super::*;
290
291 #[test]
292 fn geodetic_to_ecef_origin() {
293 let g = Geodetic {
295 lat_deg: 0.0,
296 lon_deg: 0.0,
297 alt_m: 0.0,
298 };
299 let e = geodetic_to_ecef(g);
300 assert!((e.x - WGS84_A).abs() < 1e-6, "x should be WGS84_A");
301 assert!(e.y.abs() < 1e-6);
302 assert!(e.z.abs() < 1e-6);
303 }
304
305 #[test]
306 fn geodetic_to_ecef_north_pole() {
307 let g = Geodetic {
308 lat_deg: 90.0,
309 lon_deg: 0.0,
310 alt_m: 0.0,
311 };
312 let e = geodetic_to_ecef(g);
313 assert!(e.x.abs() < 1e-6);
314 assert!(e.y.abs() < 1e-6);
315 assert!((e.z - WGS84_B).abs() < 1e-6, "z should be WGS84_B");
316 }
317
318 #[test]
319 fn ecef_to_geodetic_round_trip() {
320 let original = Geodetic {
321 lat_deg: 37.8080,
322 lon_deg: -122.4177,
323 alt_m: 10.0,
324 };
325 let ecef = geodetic_to_ecef(original);
326 let recovered = ecef_to_geodetic(ecef);
327 assert!(
328 (recovered.lat_deg - original.lat_deg).abs() < 1e-6,
329 "lat mismatch"
330 );
331 assert!(
332 (recovered.lon_deg - original.lon_deg).abs() < 1e-6,
333 "lon mismatch"
334 );
335 assert!(
336 (recovered.alt_m - original.alt_m).abs() < 1e-3,
337 "alt mismatch"
338 );
339 }
340
341 #[test]
342 fn ecef_to_geodetic_round_trip_high_lat() {
343 let original = Geodetic {
344 lat_deg: 78.5,
345 lon_deg: 15.0,
346 alt_m: 500.0,
347 };
348 let ecef = geodetic_to_ecef(original);
349 let recovered = ecef_to_geodetic(ecef);
350 assert!((recovered.lat_deg - original.lat_deg).abs() < 1e-6);
351 assert!((recovered.lon_deg - original.lon_deg).abs() < 1e-6);
352 assert!((recovered.alt_m - original.alt_m).abs() < 1e-3);
353 }
354
355 #[test]
356 fn ecef_enu_round_trip() {
357 let ref_geo = Geodetic {
358 lat_deg: 37.8080,
359 lon_deg: -122.4177,
360 alt_m: 10.0,
361 };
362 let original = Enu {
363 east: 100.0,
364 north: 200.0,
365 up: 50.0,
366 };
367 let ecef = enu_to_ecef(original, ref_geo);
368 let recovered = ecef_to_enu(ecef, ref_geo);
369 assert!(
370 (recovered.east - original.east).abs() < 1e-6,
371 "east mismatch"
372 );
373 assert!(
374 (recovered.north - original.north).abs() < 1e-6,
375 "north mismatch"
376 );
377 assert!((recovered.up - original.up).abs() < 1e-6, "up mismatch");
378 }
379
380 #[test]
381 fn geodetic_distance_sf_to_la() {
382 let sf = Geodetic {
384 lat_deg: 37.7749,
385 lon_deg: -122.4194,
386 alt_m: 0.0,
387 };
388 let la = Geodetic {
389 lat_deg: 34.0522,
390 lon_deg: -118.2437,
391 alt_m: 0.0,
392 };
393 let d = geodetic_distance(sf, la);
394 assert!((d - 559_000.0).abs() < 5_000.0, "got {} expected ~559km", d);
395 }
396
397 #[test]
398 fn geodetic_distance_same_point() {
399 let g = Geodetic {
400 lat_deg: 45.0,
401 lon_deg: 90.0,
402 alt_m: 0.0,
403 };
404 assert!(geodetic_distance(g, g).abs() < 1e-6);
405 }
406
407 #[test]
408 fn geodetic_distance_antipode() {
409 let a = Geodetic {
411 lat_deg: 0.0,
412 lon_deg: 0.0,
413 alt_m: 0.0,
414 };
415 let b = Geodetic {
416 lat_deg: 0.0,
417 lon_deg: 180.0,
418 alt_m: 0.0,
419 };
420 let d = geodetic_distance(a, b);
421 assert!(
422 (d - 20_015_000.0).abs() < 50_000.0,
423 "got {} expected ~20015km",
424 d
425 );
426 }
427
428 #[test]
429 fn ecef_to_geodetic_at_pole() {
430 let e = Ecef {
431 x: 0.0,
432 y: 0.0,
433 z: WGS84_B,
434 };
435 let g = ecef_to_geodetic(e);
436 assert!((g.lat_deg - 90.0).abs() < 1e-6);
437 }
438
439 #[test]
440 fn test_lorentz_boost_spacetime_interval_invariance() {
441 let v = FourVector::new(10.0, 3.0, 4.0, 5.0);
442 let s2_before = v.spacetime_interval_sq();
443
444 let boost = LorentzBoost::new(0.6, 0.0, 0.0).unwrap();
445 assert!((boost.gamma() - 1.25).abs() < 1e-6);
446
447 let v_prime = boost.transform(v);
448 let s2_after = v_prime.spacetime_interval_sq();
449
450 assert!(
451 (s2_before - s2_after).abs() < 1e-10,
452 "Minkowski interval must be invariant: before={}, after={}",
453 s2_before,
454 s2_after
455 );
456 }
457
458 #[test]
459 fn test_lorentz_boost_3d_velocity() {
460 let v = FourVector::new(50.0, 10.0, 20.0, 30.0);
461 let s2_before = v.spacetime_interval_sq();
462
463 let boost = LorentzBoost::new(0.3, 0.4, 0.5).unwrap();
464 let v_prime = boost.transform(v);
465 let s2_after = v_prime.spacetime_interval_sq();
466
467 assert!((s2_before - s2_after).abs() < 1e-9);
468 }
469
470 #[test]
471 fn test_lorentz_boost_superluminal_rejected() {
472 let res = LorentzBoost::new(0.8, 0.8, 0.0);
473 assert!(res.is_err());
474 }
475}