geo_to_decart/
lib.rs

1use std::f64::consts::PI;
2
3// WGS-84 constants
4const A: f64 = 6_378_137.0; // semi-major axis, meters
5const E2: f64 = 6.694_379_9901413165e-3; // first eccentricity squared
6
7fn deg_to_rad(deg: f64) -> f64 {
8    deg * PI / 180.0
9}
10
11/// Convert geodetic (lat, lon, h) to ECEF (X, Y, Z)
12/// lat, lon in degrees; h in meters
13pub fn geodetic_to_ecef(lat_deg: f64, lon_deg: f64, h: f64) -> (f64, f64, f64) {
14    let lat = deg_to_rad(lat_deg);
15    let lon = deg_to_rad(lon_deg);
16
17    let sin_lat = lat.sin();
18    let cos_lat = lat.cos();
19    let cos_lon = lon.cos();
20    let sin_lon = lon.sin();
21
22    let n = A / (1.0 - E2 * sin_lat * sin_lat).sqrt();
23
24    let x = (n + h) * cos_lat * cos_lon;
25    let y = (n + h) * cos_lat * sin_lon;
26    let z = (n * (1.0 - E2) + h) * sin_lat;
27
28    (x, y, z)
29}
30
31/// Convert ECEF -> ENU relative to reference point (lat0, lon0, h0)
32/// lat0, lon0 in degrees; h0 in meters
33/// Returns (east, north, up) in meters
34pub fn ecef_to_enu(x: f64, y: f64, z: f64, lat0_deg: f64, lon0_deg: f64, h0: f64) -> (f64, f64, f64) {
35    // ECEF of the reference point
36    let (x0, y0, z0) = geodetic_to_ecef(lat0_deg, lon0_deg, h0);
37
38    // Deltas
39    let dx = x - x0;
40    let dy = y - y0;
41    let dz = z - z0;
42
43    // Angles of the reference point (radians)
44    let lat0 = deg_to_rad(lat0_deg);
45    let lon0 = deg_to_rad(lon0_deg);
46
47    let sin_lat0 = lat0.sin();
48    let cos_lat0 = lat0.cos();
49    let sin_lon0 = lon0.sin();
50    let cos_lon0 = lon0.cos();
51
52    // Rotation matrix for ENU
53    let east = -sin_lon0 * dx + cos_lon0 * dy;
54    let north = -sin_lat0 * cos_lon0 * dx - sin_lat0 * sin_lon0 * dy + cos_lat0 * dz;
55    let up = cos_lat0 * cos_lon0 * dx + cos_lat0 * sin_lon0 * dy + sin_lat0 * dz;
56
57    (east, north, up)
58}
59
60/// Convenience wrapper: (lat, lon, h) -> (e, n, u) relative to reference (lat0, lon0, h0)
61pub fn llh_to_enu(lat_deg: f64, lon_deg: f64, h: f64, lat0_deg: f64, lon0_deg: f64, h0: f64) -> (f64, f64, f64) {
62    let (x, y, z) = geodetic_to_ecef(lat_deg, lon_deg, h);
63    ecef_to_enu(x, y, z, lat0_deg, lon0_deg, h0)
64}
65
66/// Compute centroid latitude/longitude for a list of points (lat, lon) in degrees.
67/// Simple average; suitable for relatively small areas.
68pub fn centroid_lat_lon(points: &[(f64, f64)]) -> (f64, f64) {
69    let n = points.len() as f64;
70    let mut sum_lat = 0.0;
71    let mut sum_lon = 0.0;
72    for &(lat, lon) in points {
73        sum_lat += lat;
74        sum_lon += lon;
75    }
76    (sum_lat / n, sum_lon / n)
77}
78
79#[cfg(test)]
80mod tests {
81    use super::*;
82
83    #[test]
84    fn ecef_enu_roundtrip_center_zero() {
85        let lat = 50.0;
86        let lon = 10.0;
87        let h = 200.0;
88        let (x, y, z) = geodetic_to_ecef(lat, lon, h);
89        let (e, n, u) = ecef_to_enu(x, y, z, lat, lon, h);
90        assert!(e.abs() < 1e-9);
91        assert!(n.abs() < 1e-9);
92        assert!(u.abs() < 1e-9);
93    }
94
95    #[test]
96    fn small_offset_check() {
97        let lat0 = 42.680067;
98        let lon0 = 3.034061;
99        let h0 = 0.0;
100
101        let lat1 = 42.680499;
102        let lon1 = 3.035775;
103        let h1 = 1.0;
104
105        let (e, n, _u) = llh_to_enu(lat1, lon1, h1, lat0, lon0, h0);
106        let approx_n = (super::deg_to_rad(lat1 - lat0)) * A;
107        println!("e= {:.3}, n={:.3} approx={:.3}",e, n, approx_n);
108        assert!((n - approx_n).abs() < 5.0);
109        assert!(e.abs() > 0.0);
110    }
111}
geo_to_decart/lib.rs

geo_to_decart/
lib.rs
