qualia_core_db/domains/geospatial/
geodetic.rs

1use std::f64::consts::PI;
2
3pub const WGS84_A: f64 = 6378137.0;
4pub const WGS84_INV_F: f64 = 298.257223563;
5pub const WGS84_F: f64 = 1.0 / WGS84_INV_F;
6pub const WGS84_B: f64 = WGS84_A * (1.0 - WGS84_F);
7pub const WGS84_E2: f64 = 1.0 - (WGS84_B * WGS84_B) / (WGS84_A * WGS84_A);
8pub const WGS84_EP2: f64 = (WGS84_A * WGS84_A - WGS84_B * WGS84_B) / (WGS84_B * WGS84_B);
9
10/// Convert degrees to radians
11#[inline]
12pub fn deg2rad(deg: f64) -> f64 {
13    deg * PI / 180.0
14}
15
16/// Convert radians to degrees
17#[inline]
18pub fn rad2deg(rad: f64) -> f64 {
19    rad * 180.0 / PI
20}
21
22/// Converts Geodetic coordinates (latitude, longitude, altitude) to Earth-Centered, Earth-Fixed (ECEF) coordinates.
23/// Latitude and longitude are in degrees. Altitude is in meters.
24/// Returns (X, Y, Z) in meters.
25pub fn lat_lon_alt_to_ecef(lat_deg: f64, lon_deg: f64, alt_m: f64) -> (f64, f64, f64) {
26    let lat = deg2rad(lat_deg);
27    let lon = deg2rad(lon_deg);
28
29    let sin_lat = lat.sin();
30    let cos_lat = lat.cos();
31    let sin_lon = lon.sin();
32    let cos_lon = lon.cos();
33
34    let n = WGS84_A / (1.0 - WGS84_E2 * sin_lat * sin_lat).sqrt();
35
36    let x = (n + alt_m) * cos_lat * cos_lon;
37    let y = (n + alt_m) * cos_lat * sin_lon;
38    let z = (n * (1.0 - WGS84_E2) + alt_m) * sin_lat;
39
40    (x, y, z)
41}
42
43/// Converts Earth-Centered, Earth-Fixed (ECEF) coordinates to Geodetic (latitude, longitude, altitude).
44/// Returns (latitude in degrees, longitude in degrees, altitude in meters).
45pub fn ecef_to_lat_lon_alt(x: f64, y: f64, z: f64) -> (f64, f64, f64) {
46    let p = (x * x + y * y).sqrt();
47    let lon = y.atan2(x);
48
49    let theta = (z * WGS84_A).atan2(p * WGS84_B);
50    let sin_theta = theta.sin();
51    let cos_theta = theta.cos();
52
53    let lat = (z + WGS84_EP2 * WGS84_B * sin_theta * sin_theta * sin_theta)
54        .atan2(p - WGS84_E2 * WGS84_A * cos_theta * cos_theta * cos_theta);
55
56    let sin_lat = lat.sin();
57    let n = WGS84_A / (1.0 - WGS84_E2 * sin_lat * sin_lat).sqrt();
58    let alt = p / lat.cos() - n;
59
60    (rad2deg(lat), rad2deg(lon), alt)
61}
62
63/// Computes the rotation matrix to convert from ECEF to Local Tangent Plane (ENU) at a given reference point.
64/// The reference point is given in Geodetic coordinates (degrees).
65pub fn ecef_to_enu_matrix(ref_lat_deg: f64, ref_lon_deg: f64) -> [[f64; 3]; 3] {
66    let lat = deg2rad(ref_lat_deg);
67    let lon = deg2rad(ref_lon_deg);
68
69    let sin_lat = lat.sin();
70    let cos_lat = lat.cos();
71    let sin_lon = lon.sin();
72    let cos_lon = lon.cos();
73
74    [
75        [-sin_lon, cos_lon, 0.0],
76        [-sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat],
77        [cos_lat * cos_lon, cos_lat * sin_lon, sin_lat],
78    ]
79}
80
81/// Converts a position in ECEF to Local Tangent Plane (ENU) relative to a reference point.
82pub fn ecef_to_enu(
83    x: f64,
84    y: f64,
85    z: f64,
86    ref_lat_deg: f64,
87    ref_lon_deg: f64,
88    ref_alt_m: f64,
89) -> (f64, f64, f64) {
90    let (ref_x, ref_y, ref_z) = lat_lon_alt_to_ecef(ref_lat_deg, ref_lon_deg, ref_alt_m);
91
92    let dx = x - ref_x;
93    let dy = y - ref_y;
94    let dz = z - ref_z;
95
96    let r = ecef_to_enu_matrix(ref_lat_deg, ref_lon_deg);
97
98    let e = r[0][0] * dx + r[0][1] * dy + r[0][2] * dz;
99    let n = r[1][0] * dx + r[1][1] * dy + r[1][2] * dz;
100    let u = r[2][0] * dx + r[2][1] * dy + r[2][2] * dz;
101
102    (e, n, u)
103}
104
105#[cfg(test)]
106mod tests {
107    use super::*;
108
109    #[test]
110    fn test_lla_ecef_roundtrip() {
111        let lat = -33.8688; // Sydney
112        let lon = 151.2093;
113        let alt = 50.0;
114
115        let (x, y, z) = lat_lon_alt_to_ecef(lat, lon, alt);
116        let (r_lat, r_lon, r_alt) = ecef_to_lat_lon_alt(x, y, z);
117
118        assert!((lat - r_lat).abs() < 1e-8);
119        assert!((lon - r_lon).abs() < 1e-8);
120        assert!((alt - r_alt).abs() < 1e-4);
121    }
122}
qualia_core_db/domains/geospatial/geodetic.rs

qualia_core_db/domains/geospatial/
geodetic.rs
