extern crate approx;
extern crate hifitime;
extern crate serde;
use super::hyperdual::linalg::norm;
use super::hyperdual::{Float, Hyperdual};
use super::na::{Vector3, U7};
use super::{Frame, Orbit};
use crate::md::StateParameter;
use crate::time::Epoch;
use crate::{NyxError, TimeTagged};
use std::f64::consts::PI;
use std::f64::EPSILON;
use std::fmt;
pub const ECC_EPSILON: f64 = 1e-11;
#[derive(Copy, Clone, Debug)]
pub struct OrbitDual {
pub x: Hyperdual<f64, U7>,
pub y: Hyperdual<f64, U7>,
pub z: Hyperdual<f64, U7>,
pub vx: Hyperdual<f64, U7>,
pub vy: Hyperdual<f64, U7>,
pub vz: Hyperdual<f64, U7>,
pub dt: Epoch,
pub frame: Frame,
}
impl From<Orbit> for OrbitDual {
fn from(orbit: Orbit) -> Self {
Self {
x: Hyperdual::from_slice(&[orbit.x, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
y: Hyperdual::from_slice(&[orbit.y, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0]),
z: Hyperdual::from_slice(&[orbit.z, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]),
vx: Hyperdual::from_slice(&[orbit.vx, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]),
vy: Hyperdual::from_slice(&[orbit.vy, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0]),
vz: Hyperdual::from_slice(&[orbit.vz, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]),
dt: orbit.dt,
frame: orbit.frame,
}
}
}
#[derive(Copy, Clone, Debug)]
pub struct OrbitPartial {
pub param: StateParameter,
pub dual: Hyperdual<f64, U7>,
}
impl OrbitPartial {
pub fn real(&self) -> f64 {
self.dual[0]
}
pub fn wtr_x(&self) -> f64 {
self.dual[1]
}
pub fn wtr_y(&self) -> f64 {
self.dual[2]
}
pub fn wtr_z(&self) -> f64 {
self.dual[3]
}
pub fn wtr_vx(&self) -> f64 {
self.dual[4]
}
pub fn wtr_vy(&self) -> f64 {
self.dual[5]
}
pub fn wtr_vz(&self) -> f64 {
self.dual[6]
}
}
impl fmt::Display for OrbitPartial {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
write!(f, "{:?} {}", self.param, self.dual)
}
}
impl OrbitDual {
pub fn partial_for(&self, param: &StateParameter) -> Result<OrbitPartial, NyxError> {
match param {
StateParameter::X => Ok(OrbitPartial {
dual: self.x,
param: StateParameter::X,
}),
StateParameter::Y => Ok(OrbitPartial {
dual: self.y,
param: StateParameter::Y,
}),
StateParameter::Z => Ok(OrbitPartial {
dual: self.z,
param: StateParameter::Z,
}),
StateParameter::VX => Ok(OrbitPartial {
dual: self.vx,
param: StateParameter::VX,
}),
StateParameter::VY => Ok(OrbitPartial {
dual: self.vy,
param: StateParameter::VY,
}),
StateParameter::VZ => Ok(OrbitPartial {
dual: self.vz,
param: StateParameter::VZ,
}),
StateParameter::Rmag => Ok(self.rmag()),
StateParameter::Vmag => Ok(self.vmag()),
StateParameter::HX => Ok(self.hx()),
StateParameter::HY => Ok(self.hy()),
StateParameter::HZ => Ok(self.hz()),
StateParameter::Hmag => Ok(self.hmag()),
StateParameter::Energy => Ok(self.energy()),
StateParameter::SMA => Ok(self.sma()),
StateParameter::Eccentricity => Ok(self.ecc()),
StateParameter::Inclination => Ok(self.inc()),
StateParameter::AoP => Ok(self.aop()),
StateParameter::AoL => Ok(self.aol()),
StateParameter::RAAN => Ok(self.raan()),
StateParameter::Periapsis => Ok(self.periapsis()),
StateParameter::Apoapsis => Ok(self.apoapsis()),
StateParameter::TrueLongitude => Ok(self.tlong()),
StateParameter::FlightPathAngle => Ok(self.fpa()),
StateParameter::MeanAnomaly => Ok(self.ma()),
StateParameter::EccentricAnomaly => Ok(self.ea()),
StateParameter::GeodeticHeight => Ok(self.geodetic_height()),
StateParameter::GeodeticLatitude => Ok(self.geodetic_latitude()),
StateParameter::GeodeticLongitude => Ok(self.geodetic_longitude()),
StateParameter::C3 => Ok(self.c3()),
StateParameter::RightAscension => Ok(self.right_ascension()),
StateParameter::Declination => Ok(self.declination()),
StateParameter::HyperbolicAnomaly => self.hyperbolic_anomaly(),
StateParameter::SemiParameter => Ok(self.semi_parameter()),
StateParameter::SemiMinorAxis => Ok(self.semi_minor_axis()),
_ => Err(NyxError::PartialsUndefined),
}
}
pub fn rmag(&self) -> OrbitPartial {
OrbitPartial {
param: StateParameter::Rmag,
dual: (self.x.powi(2) + self.y.powi(2) + self.z.powi(2)).sqrt(),
}
}
pub fn vmag(&self) -> OrbitPartial {
OrbitPartial {
param: StateParameter::Vmag,
dual: (self.vx.powi(2) + self.vy.powi(2) + self.vz.powi(2)).sqrt(),
}
}
pub(crate) fn radius(&self) -> Vector3<Hyperdual<f64, U7>> {
Vector3::new(self.x, self.y, self.z)
}
pub(crate) fn velocity(&self) -> Vector3<Hyperdual<f64, U7>> {
Vector3::new(self.vx, self.vy, self.vz)
}
pub(crate) fn hvec(&self) -> Vector3<Hyperdual<f64, U7>> {
self.radius().cross(&self.velocity())
}
pub fn hx(&self) -> OrbitPartial {
OrbitPartial {
dual: self.hvec()[0],
param: StateParameter::HX,
}
}
pub fn hy(&self) -> OrbitPartial {
OrbitPartial {
dual: self.hvec()[1],
param: StateParameter::HY,
}
}
pub fn hz(&self) -> OrbitPartial {
OrbitPartial {
dual: self.hvec()[2],
param: StateParameter::HZ,
}
}
pub fn hmag(&self) -> OrbitPartial {
OrbitPartial {
dual: norm(&self.hvec()),
param: StateParameter::Hmag,
}
}
pub fn energy(&self) -> OrbitPartial {
match self.frame {
Frame::Geoid { gm, .. } | Frame::Celestial { gm, .. } => OrbitPartial {
dual: self.vmag().dual.powi(2) / Hyperdual::from(2.0)
- Hyperdual::from(gm) / self.rmag().dual,
param: StateParameter::Energy,
},
_ => panic!("orbital energy not defined in this frame"),
}
}
pub fn sma(&self) -> OrbitPartial {
match self.frame {
Frame::Geoid { gm, .. } | Frame::Celestial { gm, .. } => OrbitPartial {
dual: -Hyperdual::from(gm) / (Hyperdual::from(2.0) * self.energy().dual),
param: StateParameter::SMA,
},
_ => panic!("sma not defined in this frame"),
}
}
pub(crate) fn evec(&self) -> Vector3<Hyperdual<f64, U7>> {
match self.frame {
Frame::Geoid { gm, .. } | Frame::Celestial { gm, .. } => {
let r = self.radius();
let v = self.velocity();
let hgm = Hyperdual::from(gm);
Vector3::new(
((norm(&v).powi(2) - hgm / norm(&r)) * r[0] - (r.dot(&v)) * v[0]) / hgm,
((norm(&v).powi(2) - hgm / norm(&r)) * r[1] - (r.dot(&v)) * v[1]) / hgm,
((norm(&v).powi(2) - hgm / norm(&r)) * r[2] - (r.dot(&v)) * v[2]) / hgm,
)
}
_ => panic!("eccentricity not defined in this frame"),
}
}
pub fn ecc(&self) -> OrbitPartial {
OrbitPartial {
dual: norm(&self.evec()),
param: StateParameter::Eccentricity,
}
}
pub fn inc(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => OrbitPartial {
dual: (self.hvec()[(2, 0)] / self.hmag().dual).acos().to_degrees(),
param: StateParameter::Inclination,
},
_ => panic!("inclination not defined in this frame"),
}
}
pub fn aop(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => {
let n = Vector3::new(
Hyperdual::from(0.0),
Hyperdual::from(0.0),
Hyperdual::from(1.0),
)
.cross(&self.hvec());
let aop = (n.dot(&self.evec()) / (norm(&n) * self.ecc().dual)).acos();
if aop.is_nan() {
error!("AoP is NaN");
OrbitPartial {
dual: Hyperdual::from(0.0),
param: StateParameter::AoP,
}
} else if self.evec()[2].real() < 0.0 {
OrbitPartial {
dual: (Hyperdual::from(2.0 * PI) - aop).to_degrees(),
param: StateParameter::AoP,
}
} else {
OrbitPartial {
dual: aop.to_degrees(),
param: StateParameter::AoP,
}
}
}
_ => panic!("aop not defined in this frame"),
}
}
pub fn raan(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => {
let n = Vector3::new(
Hyperdual::from(0.0),
Hyperdual::from(0.0),
Hyperdual::from(1.0),
)
.cross(&self.hvec());
let raan = (n[(0, 0)] / norm(&n)).acos();
if raan.is_nan() {
warn!("RAAN is NaN");
OrbitPartial {
dual: Hyperdual::from(0.0),
param: StateParameter::RAAN,
}
} else if n[(1, 0)] < 0.0 {
OrbitPartial {
dual: (Hyperdual::from(2.0 * PI) - raan).to_degrees(),
param: StateParameter::RAAN,
}
} else {
OrbitPartial {
dual: raan.to_degrees(),
param: StateParameter::RAAN,
}
}
}
_ => panic!("RAAN not defined in this frame"),
}
}
pub fn ta(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => {
if self.ecc().real() < ECC_EPSILON {
warn!(
"true anomaly ill-defined for circular orbit (e = {})",
self.ecc()
);
}
let cos_nu = self.evec().dot(&self.radius()) / (self.ecc().dual * self.rmag().dual);
if (cos_nu.real().abs() - 1.0).abs() < EPSILON {
if cos_nu > 1.0 {
OrbitPartial {
dual: Hyperdual::from(180.0),
param: StateParameter::TrueAnomaly,
}
} else {
OrbitPartial {
dual: Hyperdual::from(0.0),
param: StateParameter::TrueAnomaly,
}
}
} else {
let ta = cos_nu.acos();
if ta.is_nan() {
warn!("TA is NaN");
OrbitPartial {
dual: Hyperdual::from(0.0),
param: StateParameter::TrueAnomaly,
}
} else if self.radius().dot(&self.velocity()) < 0.0 {
OrbitPartial {
dual: (Hyperdual::from(2.0 * PI) - ta).to_degrees(),
param: StateParameter::TrueAnomaly,
}
} else {
OrbitPartial {
dual: ta.to_degrees(),
param: StateParameter::TrueAnomaly,
}
}
}
}
_ => panic!("true anomaly not defined in this frame"),
}
}
pub fn tlong(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => {
OrbitPartial {
dual: self.aop().dual + self.raan().dual + self.ta().dual,
param: StateParameter::TrueLongitude,
}
}
_ => panic!("true longitude not defined in this frame"),
}
}
pub fn aol(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => {
if self.ecc().real() < ECC_EPSILON {
OrbitPartial {
dual: self.tlong().dual - self.raan().dual,
param: StateParameter::AoL,
}
} else {
OrbitPartial {
dual: self.aop().dual + self.ta().dual,
param: StateParameter::AoL,
}
}
}
_ => panic!("argument of latitude not defined in this frame"),
}
}
pub fn periapsis(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => OrbitPartial {
dual: self.sma().dual * (Hyperdual::from(1.0) - self.ecc().dual),
param: StateParameter::Periapsis,
},
_ => panic!("periapsis not defined in this frame"),
}
}
pub fn apoapsis(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => OrbitPartial {
dual: self.sma().dual * (Hyperdual::from(1.0) + self.ecc().dual),
param: StateParameter::Apoapsis,
},
_ => panic!("apoapsis not defined in this frame"),
}
}
pub fn ea(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => {
let (sin_ta, cos_ta) = self.ta().dual.to_radians().sin_cos();
let ecc_cos_ta = self.ecc().dual * cos_ta;
let sin_ea = ((Hyperdual::from(1.0) - self.ecc().dual.powi(2)).sqrt() * sin_ta)
/ (Hyperdual::from(1.0) + ecc_cos_ta);
let cos_ea = (self.ecc().dual + cos_ta) / (Hyperdual::from(1.0) + ecc_cos_ta);
OrbitPartial {
dual: sin_ea.atan2(cos_ea).to_degrees(),
param: StateParameter::EccentricAnomaly,
}
}
_ => panic!("eccentric anomaly is not defined in this frame"),
}
}
pub fn fpa(&self) -> OrbitPartial {
let nu = self.ta().dual.to_radians();
let ecc = self.ecc().dual;
let denom =
(Hyperdual::from(1.0) + Hyperdual::from(2.0) * ecc * nu.cos() + ecc.powi(2)).sqrt();
let sin_fpa = ecc * nu.sin() / denom;
let cos_fpa = Hyperdual::from(1.0) + ecc * nu.cos() / denom;
OrbitPartial {
dual: sin_fpa.atan2(cos_fpa).to_degrees(),
param: StateParameter::FlightPathAngle,
}
}
pub fn ma(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => {
if self.ecc().real() < 1.0 {
OrbitPartial {
dual: (self.ea().dual.to_radians()
- self.ecc().dual * self.ea().dual.to_radians().sin())
.to_degrees(),
param: StateParameter::MeanAnomaly,
}
} else if self.ecc().real() > 1.0 {
info!("computing the hyperbolic anomaly");
OrbitPartial {
dual: ((self.ta().dual.to_radians().sin()
* (self.ecc().dual.powi(2) - Hyperdual::from(1.0)))
.sqrt()
/ (Hyperdual::from(1.0)
+ self.ecc().dual * self.ta().dual.to_radians().cos()))
.asinh()
.to_degrees(),
param: StateParameter::MeanAnomaly,
}
} else {
error!("parabolic orbit: setting mean anomaly to 0.0");
OrbitPartial {
dual: Hyperdual::from(0.0),
param: StateParameter::MeanAnomaly,
}
}
}
_ => panic!("mean anomaly is not defined in this frame"),
}
}
pub fn semi_parameter(&self) -> OrbitPartial {
match self.frame {
Frame::Celestial { .. } | Frame::Geoid { .. } => OrbitPartial {
dual: self.sma().dual * (Hyperdual::from(1.0) - self.ecc().dual.powi(2)),
param: StateParameter::SemiParameter,
},
_ => panic!("semi parameter is not defined in this frame"),
}
}
pub fn geodetic_longitude(&self) -> OrbitPartial {
match self.frame {
Frame::Geoid { .. } => OrbitPartial {
dual: self.y.atan2(self.x).to_degrees(),
param: StateParameter::GeodeticLongitude,
},
_ => panic!("geodetic elements only defined in a Geoid frame"),
}
}
pub fn geodetic_latitude(&self) -> OrbitPartial {
match self.frame {
Frame::Geoid {
flattening,
semi_major_radius,
..
} => {
let eps = 1e-12;
let max_attempts = 20;
let mut attempt_no = 0;
let r_delta = (self.x.powi(2) + self.y.powi(2)).sqrt();
let mut latitude = (self.z / self.rmag().dual).asin();
let e2 = Hyperdual::from(flattening * (2.0 - flattening));
loop {
attempt_no += 1;
let c_earth = Hyperdual::from(semi_major_radius)
/ ((Hyperdual::from(1.0) - e2 * (latitude).sin().powi(2)).sqrt());
let new_latitude = (self.z + c_earth * e2 * (latitude).sin()).atan2(r_delta);
if (latitude - new_latitude).abs() < eps {
return OrbitPartial {
dual: new_latitude.to_degrees(),
param: StateParameter::GeodeticLatitude,
};
} else if attempt_no >= max_attempts {
error!(
"geodetic latitude failed to converge -- error = {}",
(latitude - new_latitude).abs()
);
return OrbitPartial {
dual: new_latitude.to_degrees(),
param: StateParameter::GeodeticLatitude,
};
}
latitude = new_latitude;
}
}
_ => panic!("geodetic elements only defined in a Geoid frame"),
}
}
pub fn geodetic_height(&self) -> OrbitPartial {
match self.frame {
Frame::Geoid {
flattening,
semi_major_radius,
..
} => {
let e2 = Hyperdual::from(flattening * (2.0 - flattening));
let latitude = self.geodetic_latitude().dual.to_radians();
let sin_lat = latitude.sin();
if (latitude - Hyperdual::from(1.0)).abs() < 0.1 {
let s_earth0: f64 = semi_major_radius * (1.0 - flattening).powi(2);
let s_earth = Hyperdual::from(s_earth0)
/ ((Hyperdual::from(1.0) - e2 * sin_lat.powi(2)).sqrt());
OrbitPartial {
dual: self.z / latitude.sin() - s_earth,
param: StateParameter::GeodeticHeight,
}
} else {
let c_earth = Hyperdual::from(semi_major_radius)
/ ((Hyperdual::from(1.0) - e2 * sin_lat.powi(2)).sqrt());
let r_delta = (self.x.powi(2) + self.y.powi(2)).sqrt();
OrbitPartial {
dual: r_delta / latitude.cos() - c_earth,
param: StateParameter::GeodeticHeight,
}
}
}
_ => panic!("geodetic elements only defined in a Geoid frame"),
}
}
#[allow(clippy::eq_op)]
pub fn right_ascension(&self) -> OrbitPartial {
OrbitPartial {
dual: (self.y.atan2(self.x)).to_degrees(),
param: StateParameter::RightAscension,
}
}
#[allow(clippy::eq_op)]
pub fn declination(&self) -> OrbitPartial {
OrbitPartial {
dual: (self.z / self.rmag().dual).asin().to_degrees(),
param: StateParameter::Declination,
}
}
pub fn semi_minor_axis(&self) -> OrbitPartial {
if self.ecc().real() <= 1.0 {
OrbitPartial {
dual: ((self.sma().dual * self.ecc().dual).powi(2) - self.sma().dual.powi(2))
.sqrt(),
param: StateParameter::SemiMinorAxis,
}
} else {
OrbitPartial {
dual: self.hmag().dual.powi(2)
/ (Hyperdual::from(self.frame.gm())
* (self.ecc().dual.powi(2) - Hyperdual::from(1.0)).sqrt()),
param: StateParameter::SemiMinorAxis,
}
}
}
pub fn velocity_declination(&self) -> OrbitPartial {
OrbitPartial {
dual: (self.vz / self.vmag().dual).asin().to_degrees(),
param: StateParameter::VelocityDeclination,
}
}
pub fn c3(&self) -> OrbitPartial {
OrbitPartial {
dual: -Hyperdual::from(self.frame.gm()) / self.sma().dual,
param: StateParameter::C3,
}
}
pub fn hyperbolic_anomaly(&self) -> Result<OrbitPartial, NyxError> {
if self.ecc().real() <= 1.0 {
Err(NyxError::NotHyperbolic(
"Orbit is not hyperbolic so there is no hyperbolic anomaly.".to_string(),
))
} else {
let (sin_ta, cos_ta) = self.ta().dual.to_radians().sin_cos();
let sinh_h = (sin_ta * (self.ecc().dual.powi(2) - Hyperdual::from(1.0)).sqrt())
/ (Hyperdual::from(1.0) + self.ecc().dual * cos_ta);
Ok(OrbitPartial {
dual: sinh_h.asinh().to_degrees(),
param: StateParameter::HyperbolicAnomaly,
})
}
}
}
impl TimeTagged for OrbitDual {
fn epoch(&self) -> Epoch {
self.dt
}
fn set_epoch(&mut self, epoch: Epoch) {
self.dt = epoch
}
}