use sciforge::hub::domain::common::constants::EARTH_RADIUS;
pub const EARTHFLATTENING: f64 = 1.0 / 298.257223563;
pub const EARTHSEMIMAJORM: f64 = 6378137.0;
pub const EARTHSEMIMINORM: f64 = 6356752.314245;
#[derive(Debug, Clone, Copy)]
pub struct LatLon {
pub latdeg: f64,
pub londeg: f64,
pub altm: f64,
}
#[derive(Debug, Clone, Copy)]
pub struct Ecef {
pub x: f64,
pub y: f64,
pub z: f64,
}
impl LatLon {
pub fn new(latdeg: f64, londeg: f64, altm: f64) -> Self {
Self {
latdeg,
londeg,
altm,
}
}
pub fn toecef(&self) -> Ecef {
let lat = self.latdeg.to_radians();
let lon = self.londeg.to_radians();
let e2 = 2.0 * EARTHFLATTENING - EARTHFLATTENING * EARTHFLATTENING;
let sinlat = lat.sin();
let coslat = lat.cos();
let n = EARTHSEMIMAJORM / (1.0 - e2 * sinlat * sinlat).sqrt();
Ecef {
x: (n + self.altm) * coslat * lon.cos(),
y: (n + self.altm) * coslat * lon.sin(),
z: (n * (1.0 - e2) + self.altm) * sinlat,
}
}
pub fn tocartesiansimple(&self) -> [f64; 3] {
let lat = self.latdeg.to_radians();
let lon = self.londeg.to_radians();
let r = EARTH_RADIUS + self.altm;
[
r * lat.cos() * lon.cos(),
r * lat.cos() * lon.sin(),
r * lat.sin(),
]
}
pub fn distanceto(&self, other: &LatLon) -> f64 {
let lat1 = self.latdeg.to_radians();
let lat2 = other.latdeg.to_radians();
let dlat = (other.latdeg - self.latdeg).to_radians();
let dlon = (other.londeg - self.londeg).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 tolatlon(&self) -> LatLon {
let e2 = 2.0 * EARTHFLATTENING - EARTHFLATTENING * EARTHFLATTENING;
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 step in 0..10 {
let sinlat = lat.sin();
let n = EARTHSEMIMAJORM / (1.0 - e2 * sinlat * sinlat).sqrt();
lat = (self.z + e2 * n * sinlat).atan2(p);
if step == 9 {
debug_assert!(lat.is_finite());
}
}
let sinlat = lat.sin();
let n = EARTHSEMIMAJORM / (1.0 - e2 * sinlat * sinlat).sqrt();
let alt = p / lat.cos() - n;
LatLon {
latdeg: lat.to_degrees(),
londeg: lon.to_degrees(),
altm: alt,
}
}
}