use spacelab_autodiff::Scalar;
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Station {
pub position: [f64; 3],
pub velocity: [f64; 3],
}
impl Station {
#[must_use]
pub const fn fixed(position: [f64; 3]) -> Self {
Self {
position,
velocity: [0.0, 0.0, 0.0],
}
}
}
#[inline]
fn relative<S: Scalar>(sat: &[S; 6], station: &Station) -> ([S; 3], [S; 3]) {
let dr = [
sat[0] - S::from_f64(station.position[0]),
sat[1] - S::from_f64(station.position[1]),
sat[2] - S::from_f64(station.position[2]),
];
let dv = [
sat[3] - S::from_f64(station.velocity[0]),
sat[4] - S::from_f64(station.velocity[1]),
sat[5] - S::from_f64(station.velocity[2]),
];
(dr, dv)
}
#[must_use]
pub fn range<S: Scalar>(sat: &[S; 6], station: &Station) -> S {
let (dr, _) = relative(sat, station);
(dr[0] * dr[0] + dr[1] * dr[1] + dr[2] * dr[2]).sqrt()
}
#[must_use]
pub fn range_rate<S: Scalar>(sat: &[S; 6], station: &Station) -> S {
let (dr, dv) = relative(sat, station);
let rho2 = dr[0] * dr[0] + dr[1] * dr[1] + dr[2] * dr[2];
let dot = dr[0] * dv[0] + dr[1] * dv[1] + dr[2] * dv[2];
dot * rho2.sqrt().recip()
}
#[must_use]
pub fn right_ascension<S: Scalar>(sat: &[S; 6], station: &Station) -> S {
let (dr, _) = relative(sat, station);
dr[1].atan2(dr[0])
}
#[must_use]
pub fn declination<S: Scalar>(sat: &[S; 6], station: &Station) -> S {
let (dr, _) = relative(sat, station);
let rho_xy = (dr[0] * dr[0] + dr[1] * dr[1]).sqrt();
dr[2].atan2(rho_xy)
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Topocentric {
pub position: [f64; 3],
pub east: [f64; 3],
pub north: [f64; 3],
pub up: [f64; 3],
}
impl Topocentric {
#[must_use]
pub fn from_geodetic(position: [f64; 3], latitude: f64, longitude: f64) -> Self {
let (sin_lat, cos_lat) = (libm::sin(latitude), libm::cos(latitude));
let (sin_lon, cos_lon) = (libm::sin(longitude), libm::cos(longitude));
Self {
position,
east: [-sin_lon, cos_lon, 0.0],
north: [-sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat],
up: [cos_lat * cos_lon, cos_lat * sin_lon, sin_lat],
}
}
}
#[inline]
fn horizon<S: Scalar>(sat: &[S; 6], topo: &Topocentric) -> (S, S, S) {
let dr = [
sat[0] - S::from_f64(topo.position[0]),
sat[1] - S::from_f64(topo.position[1]),
sat[2] - S::from_f64(topo.position[2]),
];
let project = |axis: &[f64; 3]| {
dr[0] * S::from_f64(axis[0]) + dr[1] * S::from_f64(axis[1]) + dr[2] * S::from_f64(axis[2])
};
(project(&topo.east), project(&topo.north), project(&topo.up))
}
#[must_use]
pub fn elevation<S: Scalar>(sat: &[S; 6], topo: &Topocentric) -> S {
let (e, n, u) = horizon(sat, topo);
let horizontal = (e * e + n * n).sqrt();
u.atan2(horizontal)
}
#[must_use]
pub fn azimuth<S: Scalar>(sat: &[S; 6], topo: &Topocentric) -> S {
let (e, n, _) = horizon(sat, topo);
e.atan2(n)
}