use core::f64::consts::PI;
use qtty::angular::Radians;
use crate::eccentricity::Eccentricity;
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct MeanAnomaly(Radians);
impl MeanAnomaly {
#[must_use]
pub const fn new(r: Radians) -> Self {
Self(r)
}
#[must_use]
pub fn from_value(v: f64) -> Self {
Self(Radians::new(v))
}
#[must_use]
pub const fn radians(self) -> Radians {
self.0
}
#[must_use]
pub fn value(self) -> f64 {
self.0.value()
}
}
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct TrueAnomaly(Radians);
impl TrueAnomaly {
#[must_use]
pub const fn new(r: Radians) -> Self {
Self(r)
}
#[must_use]
pub fn from_value(v: f64) -> Self {
Self(Radians::new(v))
}
#[must_use]
pub const fn radians(self) -> Radians {
self.0
}
#[must_use]
pub fn value(self) -> f64 {
self.0.value()
}
}
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct EccentricAnomaly(Radians);
impl EccentricAnomaly {
#[must_use]
pub const fn new(r: Radians) -> Self {
Self(r)
}
#[must_use]
pub fn from_value(v: f64) -> Self {
Self(Radians::new(v))
}
#[must_use]
pub const fn radians(self) -> Radians {
self.0
}
#[must_use]
pub fn value(self) -> f64 {
self.0.value()
}
}
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct HyperbolicAnomaly(f64);
impl HyperbolicAnomaly {
#[must_use]
pub const fn new(v: f64) -> Self {
Self(v)
}
#[must_use]
pub const fn value(self) -> f64 {
self.0
}
}
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct ParabolicAnomaly(f64);
impl ParabolicAnomaly {
#[must_use]
pub const fn new(v: f64) -> Self {
Self(v)
}
#[must_use]
pub const fn value(self) -> f64 {
self.0
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct AnomalyOptions {
max_iter: u32,
tol: f64,
}
impl AnomalyOptions {
pub fn try_new(max_iter: u32, tol: f64) -> Result<Self, AnomalyError> {
if max_iter == 0 {
return Err(AnomalyError::InvalidOptions(
"max_iter must be greater than zero",
));
}
if !tol.is_finite() || tol <= 0.0 {
return Err(AnomalyError::InvalidOptions(
"tol must be a strictly positive finite value",
));
}
Ok(Self { max_iter, tol })
}
#[must_use]
pub const fn max_iter(self) -> u32 {
self.max_iter
}
#[must_use]
pub const fn tol(self) -> f64 {
self.tol
}
}
impl Default for AnomalyOptions {
fn default() -> Self {
Self {
max_iter: 64,
tol: 1.0e-12,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, thiserror::Error)]
pub enum AnomalyError {
#[error(
"Kepler solver did not converge after {iterations} iterations (residual {residual:e})"
)]
NotConverged {
iterations: u32,
residual: f64,
},
#[error("invalid eccentricity {0}")]
InvalidEccentricity(f64),
#[error("invalid mean anomaly {0}")]
InvalidMeanAnomaly(f64),
#[error("invalid AnomalyOptions: {0}")]
InvalidOptions(&'static str),
}
pub fn kepler_elliptic(
mean_anomaly: MeanAnomaly,
ecc: Eccentricity,
opts: AnomalyOptions,
) -> Result<EccentricAnomaly, AnomalyError> {
let ecc_value = ecc.value();
let mean_value = mean_anomaly.value();
if !(0.0..1.0).contains(&ecc_value) || !ecc_value.is_finite() {
return Err(AnomalyError::InvalidEccentricity(ecc_value));
}
if !mean_value.is_finite() {
return Err(AnomalyError::InvalidMeanAnomaly(mean_value));
}
if ecc_value == 0.0 {
return Ok(EccentricAnomaly::new(Radians::new(mean_value)));
}
let mut e_anom = mean_value + ecc_value * mean_value.sin();
for i in 0..opts.max_iter {
let mut residual = elliptic_residual(e_anom, ecc_value, mean_value);
if residual.abs() <= opts.tol {
return Ok(EccentricAnomaly::new(Radians::new(e_anom)));
}
let fp = 1.0 - ecc_value * e_anom.cos();
e_anom -= residual / fp;
if !e_anom.is_finite() {
break;
}
if i + 1 == opts.max_iter {
residual = elliptic_residual(e_anom, ecc_value, mean_value);
if residual.abs() <= opts.tol {
return Ok(EccentricAnomaly::new(Radians::new(e_anom)));
}
}
}
elliptic_bisection(mean_value, ecc_value, opts).map(|v| EccentricAnomaly::new(Radians::new(v)))
}
pub fn kepler_hyperbolic(
mean_anomaly: MeanAnomaly,
ecc: Eccentricity,
opts: AnomalyOptions,
) -> Result<HyperbolicAnomaly, AnomalyError> {
let ecc_value = ecc.value();
let mean_value = mean_anomaly.value();
if ecc_value <= 1.0 || !ecc_value.is_finite() {
return Err(AnomalyError::InvalidEccentricity(ecc_value));
}
if !mean_value.is_finite() {
return Err(AnomalyError::InvalidMeanAnomaly(mean_value));
}
if mean_value == 0.0 {
return Ok(HyperbolicAnomaly::new(0.0));
}
let abs_mean = mean_value.abs();
let mut f_anom = if abs_mean > 50.0 * ecc_value {
mean_value.signum() * (2.0 * abs_mean / ecc_value).ln()
} else {
(mean_value / ecc_value).asinh()
};
for _ in 0..opts.max_iter {
let residual = hyperbolic_residual(f_anom, ecc_value, mean_value);
if residual.abs() <= opts.tol {
return Ok(HyperbolicAnomaly::new(f_anom));
}
let fp = ecc_value * f_anom.cosh() - 1.0;
f_anom -= residual / fp;
if !f_anom.is_finite() {
break;
}
}
hyperbolic_bisection(mean_value, ecc_value, opts).map(HyperbolicAnomaly::new)
}
#[must_use]
pub fn parabolic_from_mean(m_d: ParabolicAnomaly) -> ParabolicAnomaly {
let a = 1.5 * m_d.value();
ParabolicAnomaly::new((a + (a * a + 1.0).sqrt()).cbrt() - ((a * a + 1.0).sqrt() - a).cbrt())
}
#[must_use]
pub fn eccentric_from_true(nu: TrueAnomaly, ecc: Eccentricity) -> EccentricAnomaly {
let nu_value = nu.value();
let ecc_value = ecc.value();
let e = 2.0
* (((1.0 - ecc_value).sqrt() * (0.5 * nu_value).sin())
.atan2((1.0 + ecc_value).sqrt() * (0.5 * nu_value).cos()));
EccentricAnomaly::new(Radians::new(wrap_two_pi_raw(e)))
}
#[must_use]
pub fn true_from_eccentric(ea: EccentricAnomaly, ecc: Eccentricity) -> TrueAnomaly {
let ea_value = ea.value();
let ecc_value = ecc.value();
let s = ((1.0 + ecc_value).sqrt() * (0.5 * ea_value).sin())
.atan2((1.0 - ecc_value).sqrt() * (0.5 * ea_value).cos());
TrueAnomaly::new(Radians::new(wrap_two_pi_raw(2.0 * s)))
}
#[must_use]
pub fn mean_from_eccentric(ea: EccentricAnomaly, ecc: Eccentricity) -> MeanAnomaly {
let ea_value = ea.value();
MeanAnomaly::new(Radians::new(ea_value - ecc.value() * ea_value.sin()))
}
pub fn eccentric_from_mean(
m: MeanAnomaly,
ecc: Eccentricity,
opts: AnomalyOptions,
) -> Result<EccentricAnomaly, AnomalyError> {
kepler_elliptic(m, ecc, opts)
}
pub fn true_from_mean(
m: MeanAnomaly,
ecc: Eccentricity,
opts: AnomalyOptions,
) -> Result<TrueAnomaly, AnomalyError> {
Ok(true_from_eccentric(eccentric_from_mean(m, ecc, opts)?, ecc))
}
#[must_use]
pub fn mean_from_true(nu: TrueAnomaly, ecc: Eccentricity) -> MeanAnomaly {
mean_from_eccentric(eccentric_from_true(nu, ecc), ecc)
}
#[must_use]
pub fn hyperbolic_from_true(nu: TrueAnomaly, ecc: Eccentricity) -> HyperbolicAnomaly {
let ecc_value = ecc.value();
let t = (0.5 * nu.value()).tan() * ((ecc_value - 1.0) / (ecc_value + 1.0)).sqrt();
HyperbolicAnomaly::new(2.0 * t.atanh())
}
#[must_use]
pub fn true_from_hyperbolic(fa: HyperbolicAnomaly, ecc: Eccentricity) -> TrueAnomaly {
let ecc_value = ecc.value();
TrueAnomaly::new(Radians::new(
2.0 * (((ecc_value + 1.0).sqrt() * (0.5 * fa.value()).sinh())
.atan2((ecc_value - 1.0).sqrt() * (0.5 * fa.value()).cosh())),
))
}
#[must_use]
pub fn mean_from_hyperbolic(fa: HyperbolicAnomaly, ecc: Eccentricity) -> MeanAnomaly {
MeanAnomaly::new(Radians::new(ecc.value() * fa.value().sinh() - fa.value()))
}
pub fn hyperbolic_from_mean(
m: MeanAnomaly,
ecc: Eccentricity,
opts: AnomalyOptions,
) -> Result<HyperbolicAnomaly, AnomalyError> {
kepler_hyperbolic(m, ecc, opts)
}
#[must_use]
pub fn true_from_parabolic(dp: ParabolicAnomaly) -> TrueAnomaly {
TrueAnomaly::new(Radians::new(2.0 * dp.value().atan()))
}
#[must_use]
pub fn parabolic_from_true(nu: TrueAnomaly) -> ParabolicAnomaly {
ParabolicAnomaly::new((nu.value() / 2.0).tan())
}
#[must_use]
pub(crate) fn wrap_two_pi_raw(x: f64) -> f64 {
x.rem_euclid(2.0 * PI)
}
#[inline]
fn elliptic_residual(ea: f64, ecc: f64, mean_anomaly: f64) -> f64 {
ea - ecc * ea.sin() - mean_anomaly
}
fn elliptic_bisection(
mean_anomaly: f64,
ecc: f64,
opts: AnomalyOptions,
) -> Result<f64, AnomalyError> {
let mut lower = mean_anomaly - PI;
let mut upper = mean_anomaly + PI;
let mut residual = elliptic_residual(0.5 * (lower + upper), ecc, mean_anomaly);
for _ in 0..opts.max_iter {
let mid = 0.5 * (lower + upper);
residual = elliptic_residual(mid, ecc, mean_anomaly);
if residual.abs() <= opts.tol || (upper - lower).abs() <= opts.tol {
return Ok(mid);
}
if residual.is_sign_positive() {
upper = mid;
} else {
lower = mid;
}
}
Err(AnomalyError::NotConverged {
iterations: opts.max_iter,
residual: residual.abs(),
})
}
#[inline]
fn hyperbolic_residual(fa: f64, ecc: f64, mean_anomaly: f64) -> f64 {
ecc * fa.sinh() - fa - mean_anomaly
}
fn hyperbolic_bisection(
mean_anomaly: f64,
ecc: f64,
opts: AnomalyOptions,
) -> Result<f64, AnomalyError> {
let sign = mean_anomaly.signum();
let positive_mean_anomaly = mean_anomaly.abs();
let mut lower = 0.0;
let mut upper = (positive_mean_anomaly / ecc).asinh().max(1.0);
let mut upper_residual = hyperbolic_residual(upper, ecc, positive_mean_anomaly);
for _ in 0..opts.max_iter {
if upper_residual.is_sign_positive() || upper_residual == 0.0 {
break;
}
upper *= 2.0;
upper_residual = hyperbolic_residual(upper, ecc, positive_mean_anomaly);
}
if upper_residual.is_sign_negative() {
return Err(AnomalyError::NotConverged {
iterations: opts.max_iter,
residual: upper_residual.abs(),
});
}
let mut residual = upper_residual;
for _ in 0..opts.max_iter {
let mid = 0.5 * (lower + upper);
residual = hyperbolic_residual(mid, ecc, positive_mean_anomaly);
if residual.abs() <= opts.tol || (upper - lower).abs() <= opts.tol {
return Ok(sign * mid);
}
if residual.is_sign_positive() {
upper = mid;
} else {
lower = mid;
}
}
Err(AnomalyError::NotConverged {
iterations: opts.max_iter,
residual: residual.abs(),
})
}
#[cfg(test)]
mod tests {
use super::*;
use core::f64::consts::TAU;
#[test]
fn solvers_converge() {
let zero = Eccentricity::new(0.0).unwrap();
assert_eq!(
kepler_elliptic(
MeanAnomaly::from_value(0.4),
zero,
AnomalyOptions::default()
)
.unwrap()
.value(),
0.4
);
let elliptic_ecc = Eccentricity::new(0.4).unwrap();
let e = kepler_elliptic(
MeanAnomaly::from_value(1.0),
elliptic_ecc,
AnomalyOptions::default(),
)
.unwrap();
assert!((mean_from_eccentric(e, elliptic_ecc).value() - 1.0).abs() < 1e-12);
let hyperbolic_ecc = Eccentricity::new(1.4).unwrap();
let f = kepler_hyperbolic(
MeanAnomaly::from_value(1.0),
hyperbolic_ecc,
AnomalyOptions::default(),
)
.unwrap();
assert!((mean_from_hyperbolic(f, hyperbolic_ecc).value() - 1.0).abs() < 1e-12);
}
#[test]
fn round_trip_elliptic_anomalies() {
let nu = TrueAnomaly::from_value(1.2);
let ecc = Eccentricity::new(0.3).unwrap();
let ea = eccentric_from_true(nu, ecc);
let m = mean_from_eccentric(ea, ecc);
let nu2 = true_from_mean(m, ecc, AnomalyOptions::default()).unwrap();
let diff = (nu2.value() - nu.value() + core::f64::consts::PI).rem_euclid(TAU)
- core::f64::consts::PI;
assert!(diff.abs() < 1e-12);
}
#[test]
fn round_trip_hyperbolic_anomalies() {
let nu = TrueAnomaly::from_value(0.8);
let ecc = Eccentricity::new(1.7).unwrap();
let f = hyperbolic_from_true(nu, ecc);
let m = mean_from_hyperbolic(f, ecc);
let f2 = hyperbolic_from_mean(m, ecc, AnomalyOptions::default()).unwrap();
assert!((f2.value() - f.value()).abs() < 1e-12);
}
#[test]
fn invalid_eccentricity() {
assert!(matches!(
kepler_elliptic(
MeanAnomaly::from_value(0.0),
Eccentricity::new_unchecked(1.0),
AnomalyOptions::default()
),
Err(AnomalyError::InvalidEccentricity(_))
));
assert!(matches!(
kepler_hyperbolic(
MeanAnomaly::from_value(0.0),
Eccentricity::new_unchecked(0.9),
AnomalyOptions::default()
),
Err(AnomalyError::InvalidEccentricity(_))
));
}
#[test]
fn try_new_rejects_invalid_options() {
assert!(AnomalyOptions::try_new(0, 1e-12).is_err());
assert!(AnomalyOptions::try_new(10, 0.0).is_err());
assert!(AnomalyOptions::try_new(10, -1.0).is_err());
assert!(AnomalyOptions::try_new(10, f64::NAN).is_err());
}
#[test]
fn detects_non_convergence() {
let opts = AnomalyOptions::try_new(1, 1e-20).unwrap();
let err = kepler_elliptic(
MeanAnomaly::from_value(1.0),
Eccentricity::new(0.9).unwrap(),
opts,
)
.unwrap_err();
assert!(matches!(err, AnomalyError::NotConverged { .. }));
}
#[test]
fn near_parabolic_hyperbolic_solver_converges() {
let ecc = Eccentricity::new(1.0000001).unwrap();
let mean = MeanAnomaly::from_value(0.001_f64.to_radians() + 0.01720209895 * 0.5);
let f = kepler_hyperbolic(mean, ecc, AnomalyOptions::try_new(100, 1e-14).unwrap()).unwrap();
assert!(f.value().is_finite());
assert!(hyperbolic_residual(f.value(), ecc.value(), mean.value()).abs() < 1e-13);
}
#[test]
fn elliptic_circular_orbit_returns_mean_anomaly() {
let zero = Eccentricity::new(0.0).unwrap();
let m = MeanAnomaly::from_value(1.23);
let e = eccentric_from_mean(m, zero, AnomalyOptions::default()).unwrap();
assert!((e.value() - m.value()).abs() < 1e-14);
}
#[test]
fn hyperbolic_mean_anomaly_zero_returns_zero() {
let ecc = Eccentricity::new_unchecked(1.5);
let f = hyperbolic_from_mean(MeanAnomaly::from_value(0.0), ecc, AnomalyOptions::default())
.unwrap();
assert!(f.value().abs() < 1e-14);
}
#[test]
fn hyperbolic_mean_anomaly_large_branch() {
let ecc = Eccentricity::new_unchecked(1.2);
let m = MeanAnomaly::from_value(100.0);
assert!(hyperbolic_from_mean(m, ecc, AnomalyOptions::default()).is_ok());
}
#[test]
fn parabolic_round_trips() {
let m = 0.5_f64;
let d = parabolic_from_mean(ParabolicAnomaly::new(m));
let m_back = d.value() + d.value().powi(3) / 3.0;
assert!((m_back - m).abs() < 1e-12);
}
#[test]
fn hyperbolic_anomaly_round_trips_all_conversions() {
let ecc = Eccentricity::new_unchecked(2.0);
let nu = TrueAnomaly::from_value(0.8);
let f = hyperbolic_from_true(nu, ecc);
let nu2 = true_from_hyperbolic(f, ecc);
assert!((nu2.value() - nu.value()).abs() < 1e-12);
let m = mean_from_hyperbolic(f, ecc);
let f2 = hyperbolic_from_mean(m, ecc, AnomalyOptions::default()).unwrap();
assert!((f2.value() - f.value()).abs() < 1e-12);
}
#[test]
fn hyperbolic_kepler_rejects_nan_mean_anomaly() {
let ecc = Eccentricity::new_unchecked(1.5);
let err = hyperbolic_from_mean(
MeanAnomaly::from_value(f64::NAN),
ecc,
AnomalyOptions::default(),
);
assert!(err.is_err());
}
#[test]
fn elliptic_kepler_rejects_nan_mean_anomaly() {
let ecc = Eccentricity::new_unchecked(0.5);
let err = eccentric_from_mean(
MeanAnomaly::from_value(f64::NAN),
ecc,
AnomalyOptions::default(),
);
assert!(err.is_err());
}
#[test]
fn elliptic_bisection_fallback_path_is_exercised() {
let ecc = Eccentricity::new_unchecked(0.5);
let opts = AnomalyOptions::try_new(2, 1e-300).unwrap();
let _ = kepler_elliptic(MeanAnomaly::from_value(1.0), ecc, opts);
}
#[test]
fn hyperbolic_bisection_fallback_path_is_exercised() {
let ecc = Eccentricity::new_unchecked(1.5);
let opts = AnomalyOptions::try_new(2, 1e-300).unwrap();
let _ = kepler_hyperbolic(MeanAnomaly::from_value(1.0), ecc, opts);
}
#[test]
fn hyperbolic_bisection_large_mean_anomaly_path_is_exercised() {
let ecc = Eccentricity::new_unchecked(1.1);
let opts = AnomalyOptions::try_new(2, 1e-300).unwrap();
let _ = kepler_hyperbolic(MeanAnomaly::from_value(50.0), ecc, opts);
}
#[test]
fn anomaly_newtypes_round_trip_values() {
let ea = EccentricAnomaly::from_value(1.2);
assert_eq!(ea.value(), 1.2);
assert_eq!(ea.radians().value(), 1.2);
let ta = TrueAnomaly::from_value(0.5);
assert_eq!(ta.value(), 0.5);
let ma = MeanAnomaly::from_value(0.8);
assert_eq!(ma.value(), 0.8);
let pa = ParabolicAnomaly::new(0.3);
assert_eq!(pa.value(), 0.3);
}
}