Skip to main content

vibe/cosmic/
transforms.rs

1//! Coordinate transforms — WGS84 ↔ ECEF ↔ ENU, geodetic distance (OCS §4).
2//!
3//! Reference: OCS Specification v2.2.0 §4.
4
5/// WGS84 ellipsoid constants.
6const WGS84_A: f64 = 6_378_137.0; // Semi-major axis (m)
7const WGS84_F: f64 = 1.0 / 298.257223563; // Flattening
8const WGS84_B: f64 = WGS84_A * (1.0 - WGS84_F); // Semi-minor axis
9const WGS84_E2: f64 = WGS84_F * (2.0 - WGS84_F); // First eccentricity squared
10
11/// Geodetic coordinates: latitude, longitude, altitude (OCS §4.1).
12#[derive(Debug, Clone, Copy, PartialEq)]
13pub struct Geodetic {
14    /// Latitude in degrees.
15    pub lat_deg: f64,
16    /// Longitude in degrees.
17    pub lon_deg: f64,
18    /// Ellipsoidal height in meters.
19    pub alt_m: f64,
20}
21
22/// ECEF Cartesian coordinates: X, Y, Z (OCS §4.1).
23#[derive(Debug, Clone, Copy, PartialEq)]
24pub struct Ecef {
25    pub x: f64,
26    pub y: f64,
27    pub z: f64,
28}
29
30/// ENU local tangent plane: East, North, Up (OCS §4.1).
31#[derive(Debug, Clone, Copy, PartialEq)]
32pub struct Enu {
33    pub east: f64,
34    pub north: f64,
35    pub up: f64,
36}
37
38/// Convert geodetic coordinates to ECEF (OCS §4.1).
39pub 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    // Prime vertical radius of curvature
48    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
57/// Convert ECEF to geodetic coordinates using Bowring's closed-form method (OCS §4.1).
58pub 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        // At pole
68        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    // Bowring's closed-form formula (1985)
76    // Second eccentricity squared: e'² = (a² - b²) / b²
77    let e_prime_sq = (WGS84_A * WGS84_A - WGS84_B * WGS84_B) / (WGS84_B * WGS84_B);
78
79    // Auxiliary angle: θ = atan2(z·a, p·b)
80    let theta = (z * WGS84_A).atan2(p * WGS84_B);
81    let sin_theta = theta.sin();
82    let cos_theta = theta.cos();
83
84    // Geodetic latitude
85    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
99/// Convert ECEF to ENU relative to a reference geodetic point (OCS §4.1).
100pub 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    // Rotation matrix: ECEF → ENU
114    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
121/// Convert ENU to ECEF relative to a reference geodetic point (OCS §4.1).
122pub 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    // Inverse rotation: ENU → ECEF
132    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
141/// Great-circle distance between two geodetic points using the Haversine formula.
142/// Returns distance in meters.
143pub 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    // Earth's mean radius
153    let r = 6_371_008.8;
154    2.0 * r * h.sqrt().asin()
155}
156
157/// Convert Ecef to a Value::Record.
158pub 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
166/// Convert Geodetic to a Value::Record.
167pub 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// ── Relativistic Lorentz Boost (SO+(1,3)) ──────────────────────────────────────
176
177/// A relativistic 4-vector in Minkowski spacetime: (c*t, x, y, z).
178#[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    /// Creates a new `FourVector`.
188    pub fn new(ct: f64, x: f64, y: f64, z: f64) -> Self {
189        Self { ct, x, y, z }
190    }
191
192    /// Minkowski spacetime interval s^2 = -(c*t)^2 + x^2 + y^2 + z^2.
193    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/// Relativistic Lorentz Boost SO+(1,3) parameterised by dimensionless velocity vector beta = v / c.
199#[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    /// Creates a new `LorentzBoost`.
208    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    /// Lorentz gamma factor: gamma = 1 / sqrt(1 - beta^2).
221    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    /// Exact 4x4 matrix representation Lambda^mu_nu in SO+(1,3).
228    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    /// Transform a 4-vector under this Lorentz boost.
269    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        // At (0, 0, 0) — equator/prime meridian on the ellipsoid
294        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        // San Francisco to Los Angeles — ~559 km
383        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        // Antipodal points — ~20015 km (half circumference)
410        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}