use sciforge::hub::domain::common::constants::EARTH_RADIUS;
pub const EARTH_FLATTENING: f64 = 1.0 / 298.257223563;
pub const EARTH_SEMI_MAJOR_M: f64 = 6_378_137.0;
pub const EARTH_SEMI_MINOR_M: f64 = 6_356_752.314245;
#[derive(Debug, Clone, Copy)]
pub struct LatLon {
pub lat_deg: f64,
pub lon_deg: f64,
pub alt_m: f64,
}
#[derive(Debug, Clone, Copy)]
pub struct Ecef {
pub x: f64,
pub y: f64,
pub z: f64,
}
impl LatLon {
pub fn new(lat_deg: f64, lon_deg: f64, alt_m: f64) -> Self {
Self {
lat_deg,
lon_deg,
alt_m,
}
}
pub fn to_ecef(&self) -> Ecef {
let lat = self.lat_deg.to_radians();
let lon = self.lon_deg.to_radians();
let e2 = 2.0 * EARTH_FLATTENING - EARTH_FLATTENING * EARTH_FLATTENING;
let sin_lat = lat.sin();
let cos_lat = lat.cos();
let n = EARTH_SEMI_MAJOR_M / (1.0 - e2 * sin_lat * sin_lat).sqrt();
Ecef {
x: (n + self.alt_m) * cos_lat * lon.cos(),
y: (n + self.alt_m) * cos_lat * lon.sin(),
z: (n * (1.0 - e2) + self.alt_m) * sin_lat,
}
}
pub fn to_cartesian_simple(&self) -> [f64; 3] {
let lat = self.lat_deg.to_radians();
let lon = self.lon_deg.to_radians();
let r = EARTH_RADIUS + self.alt_m;
[
r * lat.cos() * lon.cos(),
r * lat.cos() * lon.sin(),
r * lat.sin(),
]
}
pub fn distance_to(&self, other: &LatLon) -> f64 {
let lat1 = self.lat_deg.to_radians();
let lat2 = other.lat_deg.to_radians();
let dlat = (other.lat_deg - self.lat_deg).to_radians();
let dlon = (other.lon_deg - self.lon_deg).to_radians();
let a = (dlat / 2.0).sin().powi(2) + lat1.cos() * lat2.cos() * (dlon / 2.0).sin().powi(2);
let c = 2.0 * a.sqrt().asin();
EARTH_RADIUS * c
}
}
impl Ecef {
pub fn to_latlon(&self) -> LatLon {
let e2 = 2.0 * EARTH_FLATTENING - EARTH_FLATTENING * EARTH_FLATTENING;
let p = (self.x * self.x + self.y * self.y).sqrt();
let lon = self.y.atan2(self.x);
let mut lat = (self.z / p).atan();
for _ in 0..10 {
let sin_lat = lat.sin();
let n = EARTH_SEMI_MAJOR_M / (1.0 - e2 * sin_lat * sin_lat).sqrt();
lat = (self.z + e2 * n * sin_lat).atan2(p);
}
let sin_lat = lat.sin();
let n = EARTH_SEMI_MAJOR_M / (1.0 - e2 * sin_lat * sin_lat).sqrt();
let alt = p / lat.cos() - n;
LatLon {
lat_deg: lat.to_degrees(),
lon_deg: lon.to_degrees(),
alt_m: alt,
}
}
}