peat_protocol/discovery/
geo.rs

1//! Geographic primitives for Peat protocol
2//!
3//! Provides fundamental geographic types and operations for defining
4//! operational areas and spatial relationships between platforms.
5
6use serde::{Deserialize, Serialize};
7use std::fmt;
8
9/// Geographic coordinate (WGS84)
10#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
11pub struct GeoCoordinate {
12    /// Latitude in decimal degrees (-90 to 90)
13    pub lat: f64,
14    /// Longitude in decimal degrees (-180 to 180)
15    pub lon: f64,
16    /// Altitude in meters above sea level
17    pub alt: f64,
18}
19
20impl GeoCoordinate {
21    /// Create a new geographic coordinate
22    pub fn new(lat: f64, lon: f64, alt: f64) -> Result<Self, &'static str> {
23        if !(-90.0..=90.0).contains(&lat) {
24            return Err("Latitude must be between -90 and 90 degrees");
25        }
26        if !(-180.0..=180.0).contains(&lon) {
27            return Err("Longitude must be between -180 and 180 degrees");
28        }
29        Ok(Self { lat, lon, alt })
30    }
31
32    /// Calculate distance to another coordinate using Haversine formula (meters)
33    pub fn distance_to(&self, other: &GeoCoordinate) -> f64 {
34        const EARTH_RADIUS: f64 = 6371000.0; // meters
35
36        let lat1 = self.lat.to_radians();
37        let lat2 = other.lat.to_radians();
38        let delta_lat = (other.lat - self.lat).to_radians();
39        let delta_lon = (other.lon - self.lon).to_radians();
40
41        let a = (delta_lat / 2.0).sin().powi(2)
42            + lat1.cos() * lat2.cos() * (delta_lon / 2.0).sin().powi(2);
43        let c = 2.0 * a.sqrt().atan2((1.0 - a).sqrt());
44
45        EARTH_RADIUS * c
46    }
47
48    /// Calculate 3D distance including altitude difference
49    pub fn distance_3d(&self, other: &GeoCoordinate) -> f64 {
50        let horizontal = self.distance_to(other);
51        let vertical = (other.alt - self.alt).abs();
52        (horizontal.powi(2) + vertical.powi(2)).sqrt()
53    }
54
55    /// Calculate bearing to another coordinate (degrees, 0-360)
56    pub fn bearing_to(&self, other: &GeoCoordinate) -> f64 {
57        let lat1 = self.lat.to_radians();
58        let lat2 = other.lat.to_radians();
59        let delta_lon = (other.lon - self.lon).to_radians();
60
61        let y = delta_lon.sin() * lat2.cos();
62        let x = lat1.cos() * lat2.sin() - lat1.sin() * lat2.cos() * delta_lon.cos();
63        let bearing = y.atan2(x).to_degrees();
64
65        (bearing + 360.0) % 360.0
66    }
67}
68
69impl From<(f64, f64, f64)> for GeoCoordinate {
70    fn from(tuple: (f64, f64, f64)) -> Self {
71        Self {
72            lat: tuple.0,
73            lon: tuple.1,
74            alt: tuple.2,
75        }
76    }
77}
78
79impl From<GeoCoordinate> for (f64, f64, f64) {
80    fn from(coord: GeoCoordinate) -> Self {
81        (coord.lat, coord.lon, coord.alt)
82    }
83}
84
85impl fmt::Display for GeoCoordinate {
86    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
87        write!(
88            f,
89            "{:.6}°{}, {:.6}°{}, {:.1}m",
90            self.lat.abs(),
91            if self.lat >= 0.0 { "N" } else { "S" },
92            self.lon.abs(),
93            if self.lon >= 0.0 { "E" } else { "W" },
94            self.alt
95        )
96    }
97}
98
99/// Operational box defining geographic bounds for CAP operations
100///
101/// The operational box is a fundamental primitive provided by C2 that defines
102/// the geographic area where the autonomous fleet will operate.
103#[derive(Debug, Clone, Serialize, Deserialize)]
104pub struct OperationalBox {
105    /// Unique identifier for this operational box
106    pub id: String,
107
108    /// Southwest corner (minimum lat/lon)
109    pub southwest: GeoCoordinate,
110
111    /// Northeast corner (maximum lat/lon)
112    pub northeast: GeoCoordinate,
113
114    /// Minimum altitude (meters)
115    pub min_altitude: f64,
116
117    /// Maximum altitude (meters)
118    pub max_altitude: f64,
119
120    /// Optional name/description
121    pub name: Option<String>,
122}
123
124impl OperationalBox {
125    /// Create a new operational box from corner coordinates
126    pub fn new(
127        id: String,
128        southwest: GeoCoordinate,
129        northeast: GeoCoordinate,
130        min_altitude: f64,
131        max_altitude: f64,
132    ) -> Result<Self, &'static str> {
133        // Validate bounds
134        if southwest.lat >= northeast.lat {
135            return Err("Southwest latitude must be less than northeast latitude");
136        }
137        if southwest.lon >= northeast.lon {
138            return Err("Southwest longitude must be less than northeast longitude");
139        }
140        if min_altitude >= max_altitude {
141            return Err("Minimum altitude must be less than maximum altitude");
142        }
143
144        Ok(Self {
145            id,
146            southwest,
147            northeast,
148            min_altitude,
149            max_altitude,
150            name: None,
151        })
152    }
153
154    /// Create from center point and dimensions
155    pub fn from_center(
156        id: String,
157        center: GeoCoordinate,
158        width_meters: f64,
159        height_meters: f64,
160        altitude_range: (f64, f64),
161    ) -> Result<Self, &'static str> {
162        // Approximate degrees per meter at this latitude
163        let meters_per_degree_lat = 111320.0;
164        let meters_per_degree_lon = 111320.0 * center.lat.to_radians().cos();
165
166        let half_width_deg = (width_meters / 2.0) / meters_per_degree_lon;
167        let half_height_deg = (height_meters / 2.0) / meters_per_degree_lat;
168
169        let southwest = GeoCoordinate::new(
170            center.lat - half_height_deg,
171            center.lon - half_width_deg,
172            altitude_range.0,
173        )?;
174
175        let northeast = GeoCoordinate::new(
176            center.lat + half_height_deg,
177            center.lon + half_width_deg,
178            altitude_range.1,
179        )?;
180
181        Self::new(id, southwest, northeast, altitude_range.0, altitude_range.1)
182    }
183
184    /// Check if a coordinate is within the operational box
185    pub fn contains(&self, coord: &GeoCoordinate) -> bool {
186        coord.lat >= self.southwest.lat
187            && coord.lat <= self.northeast.lat
188            && coord.lon >= self.southwest.lon
189            && coord.lon <= self.northeast.lon
190            && coord.alt >= self.min_altitude
191            && coord.alt <= self.max_altitude
192    }
193
194    /// Get the center point of the box
195    pub fn center(&self) -> GeoCoordinate {
196        GeoCoordinate {
197            lat: (self.southwest.lat + self.northeast.lat) / 2.0,
198            lon: (self.southwest.lon + self.northeast.lon) / 2.0,
199            alt: (self.min_altitude + self.max_altitude) / 2.0,
200        }
201    }
202
203    /// Get the width of the box (meters)
204    pub fn width(&self) -> f64 {
205        let sw_ne = GeoCoordinate::new(self.southwest.lat, self.northeast.lon, 0.0).unwrap();
206        self.southwest.distance_to(&sw_ne)
207    }
208
209    /// Get the height of the box (meters)
210    pub fn height(&self) -> f64 {
211        let sw_ne = GeoCoordinate::new(self.northeast.lat, self.southwest.lon, 0.0).unwrap();
212        self.southwest.distance_to(&sw_ne)
213    }
214
215    /// Get the area of the box (square meters)
216    pub fn area(&self) -> f64 {
217        self.width() * self.height()
218    }
219
220    /// Get the volume of the box (cubic meters)
221    pub fn volume(&self) -> f64 {
222        self.area() * (self.max_altitude - self.min_altitude)
223    }
224
225    /// Divide the box into a grid of sub-boxes
226    pub fn subdivide(&self, rows: usize, cols: usize) -> Vec<OperationalBox> {
227        let lat_step = (self.northeast.lat - self.southwest.lat) / rows as f64;
228        let lon_step = (self.northeast.lon - self.southwest.lon) / cols as f64;
229
230        let mut boxes = Vec::new();
231
232        for row in 0..rows {
233            for col in 0..cols {
234                let sw_lat = self.southwest.lat + (row as f64 * lat_step);
235                let sw_lon = self.southwest.lon + (col as f64 * lon_step);
236                let ne_lat = sw_lat + lat_step;
237                let ne_lon = sw_lon + lon_step;
238
239                let sw = GeoCoordinate::new(sw_lat, sw_lon, self.min_altitude).unwrap();
240                let ne = GeoCoordinate::new(ne_lat, ne_lon, self.max_altitude).unwrap();
241
242                let sub_box = OperationalBox::new(
243                    format!("{}_{}_{}", self.id, row, col),
244                    sw,
245                    ne,
246                    self.min_altitude,
247                    self.max_altitude,
248                )
249                .unwrap();
250
251                boxes.push(sub_box);
252            }
253        }
254
255        boxes
256    }
257}
258
259impl fmt::Display for OperationalBox {
260    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
261        write!(
262            f,
263            "OperationalBox[{}]: {} to {}, alt {:.0}-{:.0}m ({:.1}km²)",
264            self.id,
265            self.southwest,
266            self.northeast,
267            self.min_altitude,
268            self.max_altitude,
269            self.area() / 1_000_000.0
270        )
271    }
272}
273
274#[cfg(test)]
275mod tests {
276    use super::*;
277
278    #[test]
279    fn test_geocoordinate_creation() {
280        let coord = GeoCoordinate::new(37.7749, -122.4194, 100.0).unwrap();
281        assert_eq!(coord.lat, 37.7749);
282        assert_eq!(coord.lon, -122.4194);
283        assert_eq!(coord.alt, 100.0);
284
285        // Invalid latitude
286        assert!(GeoCoordinate::new(91.0, 0.0, 0.0).is_err());
287        assert!(GeoCoordinate::new(-91.0, 0.0, 0.0).is_err());
288
289        // Invalid longitude
290        assert!(GeoCoordinate::new(0.0, 181.0, 0.0).is_err());
291        assert!(GeoCoordinate::new(0.0, -181.0, 0.0).is_err());
292    }
293
294    #[test]
295    fn test_distance_calculation() {
296        // San Francisco to Los Angeles (approximately 559 km)
297        let sf = GeoCoordinate::new(37.7749, -122.4194, 0.0).unwrap();
298        let la = GeoCoordinate::new(34.0522, -118.2437, 0.0).unwrap();
299
300        let distance = sf.distance_to(&la);
301        assert!((distance - 559_000.0).abs() < 5000.0); // Within 5km tolerance
302    }
303
304    #[test]
305    fn test_bearing_calculation() {
306        let coord1 = GeoCoordinate::new(0.0, 0.0, 0.0).unwrap();
307        let coord2 = GeoCoordinate::new(1.0, 0.0, 0.0).unwrap(); // North
308        let coord3 = GeoCoordinate::new(0.0, 1.0, 0.0).unwrap(); // East
309
310        let bearing_north = coord1.bearing_to(&coord2);
311        let bearing_east = coord1.bearing_to(&coord3);
312
313        assert!((bearing_north - 0.0).abs() < 1.0); // North is ~0 degrees
314        assert!((bearing_east - 90.0).abs() < 1.0); // East is ~90 degrees
315    }
316
317    #[test]
318    fn test_operational_box_creation() {
319        let sw = GeoCoordinate::new(37.0, -122.0, 0.0).unwrap();
320        let ne = GeoCoordinate::new(38.0, -121.0, 0.0).unwrap();
321
322        let op_box = OperationalBox::new("test_box".to_string(), sw, ne, 0.0, 1000.0).unwrap();
323
324        assert_eq!(op_box.id, "test_box");
325        assert_eq!(op_box.southwest.lat, 37.0);
326        assert_eq!(op_box.northeast.lat, 38.0);
327    }
328
329    #[test]
330    fn test_operational_box_contains() {
331        let sw = GeoCoordinate::new(37.0, -122.0, 0.0).unwrap();
332        let ne = GeoCoordinate::new(38.0, -121.0, 0.0).unwrap();
333        let op_box = OperationalBox::new("test".to_string(), sw, ne, 0.0, 1000.0).unwrap();
334
335        let inside = GeoCoordinate::new(37.5, -121.5, 500.0).unwrap();
336        let outside = GeoCoordinate::new(36.5, -121.5, 500.0).unwrap();
337
338        assert!(op_box.contains(&inside));
339        assert!(!op_box.contains(&outside));
340    }
341
342    #[test]
343    fn test_operational_box_center() {
344        let sw = GeoCoordinate::new(37.0, -122.0, 0.0).unwrap();
345        let ne = GeoCoordinate::new(38.0, -121.0, 0.0).unwrap();
346        let op_box = OperationalBox::new("test".to_string(), sw, ne, 0.0, 1000.0).unwrap();
347
348        let center = op_box.center();
349        assert_eq!(center.lat, 37.5);
350        assert_eq!(center.lon, -121.5);
351        assert_eq!(center.alt, 500.0);
352    }
353
354    #[test]
355    fn test_operational_box_from_center() {
356        let center = GeoCoordinate::new(37.5, -121.5, 500.0).unwrap();
357        let op_box = OperationalBox::from_center(
358            "test".to_string(),
359            center,
360            10000.0, // 10km wide
361            20000.0, // 20km tall
362            (0.0, 1000.0),
363        )
364        .unwrap();
365
366        let box_center = op_box.center();
367        assert!((box_center.lat - center.lat).abs() < 0.01);
368        assert!((box_center.lon - center.lon).abs() < 0.01);
369    }
370
371    #[test]
372    fn test_operational_box_subdivide() {
373        let sw = GeoCoordinate::new(37.0, -122.0, 0.0).unwrap();
374        let ne = GeoCoordinate::new(38.0, -121.0, 0.0).unwrap();
375        let op_box = OperationalBox::new("test".to_string(), sw, ne, 0.0, 1000.0).unwrap();
376
377        let sub_boxes = op_box.subdivide(2, 2);
378        assert_eq!(sub_boxes.len(), 4);
379
380        // Verify all sub-boxes are within original box
381        for sub_box in &sub_boxes {
382            assert!(op_box.contains(&sub_box.center()));
383        }
384    }
385}
peat_protocol/discovery/geo.rs

peat_protocol/discovery/
geo.rs
