leodos_protocols/network/isl/
geo.rs

1//! Geographic coordinate conversions for AOI mapping.
2//!
3//! Converts between geographic coordinates (latitude/longitude) and
4//! Earth-Centered Earth-Fixed (ECEF) Cartesian coordinates.
5//!
6//! # Equations (from SpaceCoMP paper, Equations 4-6)
7//!
8//! ```text
9//! x = R × cos(φ) × cos(Λ)
10//! y = R × cos(φ) × sin(Λ)
11//! z = R × sin(φ)
12//! ```
13//!
14//! Where R is Earth's radius, φ is latitude, Λ is longitude.
15
16use core::f32::consts::PI;
17
18const EARTH_RADIUS_M: f32 = 6_371_000.0;
19
20/// A geographic coordinate in degrees of latitude and longitude.
21#[derive(Debug, Clone, Copy, PartialEq)]
22pub struct LatLon {
23    /// Latitude in degrees (-90 to 90).
24    pub lat_deg: f32,
25    /// Longitude in degrees (-180 to 180).
26    pub lon_deg: f32,
27}
28
29impl LatLon {
30    /// Creates a new coordinate from latitude and longitude in degrees.
31    pub fn new(lat_deg: f32, lon_deg: f32) -> Self {
32        Self { lat_deg, lon_deg }
33    }
34
35    /// Returns the latitude converted to radians.
36    pub fn lat_rad(&self) -> f32 {
37        self.lat_deg * PI / 180.0
38    }
39
40    /// Returns the longitude converted to radians.
41    pub fn lon_rad(&self) -> f32 {
42        self.lon_deg * PI / 180.0
43    }
44}
45
46/// Earth-Centered Earth-Fixed (ECEF) Cartesian coordinate in meters.
47#[derive(Debug, Clone, Copy, PartialEq)]
48pub struct Ecef {
49    /// X coordinate in meters.
50    pub x: f32,
51    /// Y coordinate in meters.
52    pub y: f32,
53    /// Z coordinate in meters.
54    pub z: f32,
55}
56
57impl Ecef {
58    /// Converts a geographic coordinate to ECEF Cartesian coordinates.
59    pub fn from_latlon(coord: LatLon) -> Self {
60        let phi = coord.lat_rad();
61        let lambda = coord.lon_rad();
62        let cos_phi = libm::cosf(phi);
63
64        Self {
65            x: EARTH_RADIUS_M * cos_phi * libm::cosf(lambda),
66            y: EARTH_RADIUS_M * cos_phi * libm::sinf(lambda),
67            z: EARTH_RADIUS_M * libm::sinf(phi),
68        }
69    }
70
71    /// Converts back to a geographic coordinate.
72    pub fn to_latlon(&self) -> LatLon {
73        let r = libm::sqrtf(self.x * self.x + self.y * self.y + self.z * self.z);
74        let lat_rad = libm::asinf(self.z / r);
75        let lon_rad = libm::atan2f(self.y, self.x);
76
77        LatLon {
78            lat_deg: lat_rad * 180.0 / PI,
79            lon_deg: lon_rad * 180.0 / PI,
80        }
81    }
82
83    /// Returns the Euclidean distance to another ECEF point in meters.
84    pub fn distance(&self, other: &Ecef) -> f32 {
85        let dx = self.x - other.x;
86        let dy = self.y - other.y;
87        let dz = self.z - other.z;
88        libm::sqrtf(dx * dx + dy * dy + dz * dz)
89    }
90
91    /// Returns the elevation angle (in degrees) from a ground
92    /// station at `station` to this ECEF point (a satellite).
93    ///
94    /// Positive means above the local horizon, negative means
95    /// below. A satellite is visible when elevation exceeds a
96    /// minimum (typically 5-10°).
97    pub fn elevation_from(&self, station: LatLon) -> f32 {
98        let gs = Ecef::from_latlon(station);
99        let dx = self.x - gs.x;
100        let dy = self.y - gs.y;
101        let dz = self.z - gs.z;
102        let dist = libm::sqrtf(dx * dx + dy * dy + dz * dz);
103        if dist < 1.0 {
104            return 90.0;
105        }
106
107        // Local "up" unit vector at the ground station
108        let gs_mag = libm::sqrtf(
109            gs.x * gs.x + gs.y * gs.y + gs.z * gs.z,
110        );
111        let ux = gs.x / gs_mag;
112        let uy = gs.y / gs_mag;
113        let uz = gs.z / gs_mag;
114
115        // sin(elevation) = dot(d_hat, up_hat)
116        let sin_el = (dx * ux + dy * uy + dz * uz) / dist;
117        libm::asinf(sin_el) * 180.0 / PI
118    }
119}
120
121/// A geographic area of interest defined by its bounding box corners.
122#[derive(Debug, Clone, Copy)]
123pub struct GeoAoi {
124    /// Upper-left (northwest) corner of the bounding box.
125    pub upper_left: LatLon,
126    /// Lower-right (southeast) corner of the bounding box.
127    pub lower_right: LatLon,
128}
129
130impl GeoAoi {
131    /// Creates a new geographic AOI from its bounding box corners.
132    pub fn new(upper_left: LatLon, lower_right: LatLon) -> Self {
133        Self {
134            upper_left,
135            lower_right,
136        }
137    }
138
139    /// Returns `true` if the given point lies within this AOI.
140    pub fn contains(&self, point: LatLon) -> bool {
141        let lat_ok =
142            point.lat_deg <= self.upper_left.lat_deg && point.lat_deg >= self.lower_right.lat_deg;
143        let lon_ok =
144            point.lon_deg >= self.upper_left.lon_deg && point.lon_deg <= self.lower_right.lon_deg;
145        lat_ok && lon_ok
146    }
147
148    /// Returns the center point of this AOI.
149    pub fn center(&self) -> LatLon {
150        LatLon {
151            lat_deg: (self.upper_left.lat_deg + self.lower_right.lat_deg) / 2.0,
152            lon_deg: (self.upper_left.lon_deg + self.lower_right.lon_deg) / 2.0,
153        }
154    }
155
156    /// Returns the longitudinal width in degrees.
157    pub fn width_deg(&self) -> f32 {
158        self.lower_right.lon_deg - self.upper_left.lon_deg
159    }
160
161    /// Returns the latitudinal height in degrees.
162    pub fn height_deg(&self) -> f32 {
163        self.upper_left.lat_deg - self.lower_right.lat_deg
164    }
165}
166
167#[cfg(test)]
168mod tests {
169    use super::*;
170
171    fn approx_eq(a: f32, b: f32, epsilon: f32) -> bool {
172        (a - b).abs() < epsilon
173    }
174
175    #[test]
176    fn test_ecef_equator_prime_meridian() {
177        let coord = LatLon::new(0.0, 0.0);
178        let ecef = Ecef::from_latlon(coord);
179
180        assert!(approx_eq(ecef.x, EARTH_RADIUS_M, 1.0));
181        assert!(approx_eq(ecef.y, 0.0, 1.0));
182        assert!(approx_eq(ecef.z, 0.0, 1.0));
183    }
184
185    #[test]
186    fn test_ecef_north_pole() {
187        let coord = LatLon::new(90.0, 0.0);
188        let ecef = Ecef::from_latlon(coord);
189
190        assert!(approx_eq(ecef.x, 0.0, 1.0));
191        assert!(approx_eq(ecef.y, 0.0, 1.0));
192        assert!(approx_eq(ecef.z, EARTH_RADIUS_M, 1.0));
193    }
194
195    #[test]
196    fn test_roundtrip() {
197        let original = LatLon::new(45.0, -122.0);
198        let ecef = Ecef::from_latlon(original);
199        let back = ecef.to_latlon();
200
201        assert!(approx_eq(original.lat_deg, back.lat_deg, 0.001));
202        assert!(approx_eq(original.lon_deg, back.lon_deg, 0.001));
203    }
204
205    #[test]
206    fn test_geo_aoi_contains() {
207        let aoi = GeoAoi::new(LatLon::new(50.0, -10.0), LatLon::new(40.0, 10.0));
208
209        assert!(aoi.contains(LatLon::new(45.0, 0.0)));
210        assert!(!aoi.contains(LatLon::new(60.0, 0.0)));
211        assert!(!aoi.contains(LatLon::new(45.0, 20.0)));
212    }
213
214    #[test]
215    fn test_geo_aoi_center() {
216        let aoi = GeoAoi::new(LatLon::new(50.0, -10.0), LatLon::new(40.0, 10.0));
217        let center = aoi.center();
218
219        assert!(approx_eq(center.lat_deg, 45.0, 0.001));
220        assert!(approx_eq(center.lon_deg, 0.0, 0.001));
221    }
222
223    #[test]
224    fn test_elevation_directly_above() {
225        // Satellite directly above (0,0) at 550 km
226        let sat = Ecef {
227            x: EARTH_RADIUS_M + 550_000.0,
228            y: 0.0,
229            z: 0.0,
230        };
231        let station = LatLon::new(0.0, 0.0);
232        let elev = sat.elevation_from(station);
233        assert!(
234            approx_eq(elev, 90.0, 0.1),
235            "directly above: elev={}",
236            elev
237        );
238    }
239
240    #[test]
241    fn test_elevation_below_horizon() {
242        // Satellite on the opposite side of the Earth
243        let sat = Ecef {
244            x: -(EARTH_RADIUS_M + 550_000.0),
245            y: 0.0,
246            z: 0.0,
247        };
248        let station = LatLon::new(0.0, 0.0);
249        let elev = sat.elevation_from(station);
250        assert!(elev < 0.0, "opposite side: elev={}", elev);
251    }
252}
leodos_protocols/network/isl/geo.rs

leodos_protocols/network/isl/
geo.rs
