use crate::kepler::UniversalKeplerParams;
#[cfg(feature = "serde")]
use serde::Deserialize;
#[cfg(feature = "serde")]
use serde::Serialize;
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ParabolicPrelimMethod {
Cardano,
Newton,
}
pub fn prelim_parabolic(params: &UniversalKeplerParams) -> f64 {
match params.solver_type.params.parabolic_solving_method {
ParabolicPrelimMethod::Cardano => prelim_parabolic_cardano(params),
ParabolicPrelimMethod::Newton => prelim_parabolic_newton(
params,
params.solver_type.params.convergency,
params.solver_type.params.max_iter_prelim_kepuni,
),
}
}
#[inline]
fn cubic_residual_and_derivative(
universal_anomaly: f64,
radial_distance: f64,
radial_velocity_proxy: f64,
scaled_time_of_flight: f64,
) -> (f64, f64) {
let residual = universal_anomaly.powi(3) / 6.0
+ radial_velocity_proxy / 2.0 * universal_anomaly.powi(2)
+ radial_distance * universal_anomaly
- scaled_time_of_flight;
let derivative = universal_anomaly.powi(2) / 2.0
+ radial_velocity_proxy * universal_anomaly
+ radial_distance;
(residual, derivative)
}
#[inline]
fn is_cubic_non_monotonic(radial_distance: f64, radial_velocity_proxy: f64) -> bool {
radial_velocity_proxy * radial_velocity_proxy > 2.0 * radial_distance
}
fn prelim_parabolic_newton(params: &UniversalKeplerParams, contr: f64, max_iter: usize) -> f64 {
let radial_distance = params.r0;
let radial_velocity_proxy = params.sig0;
let time_of_flight = params.dt;
let scaled_time_of_flight = params.mu.sqrt() * time_of_flight;
if time_of_flight == 0.0 {
return 0.0;
}
if is_cubic_non_monotonic(radial_distance, radial_velocity_proxy) {
return prelim_parabolic_cardano(params);
}
let mut universal_anomaly = scaled_time_of_flight / radial_distance;
for _ in 0..max_iter {
let (residual, derivative) = cubic_residual_and_derivative(
universal_anomaly,
radial_distance,
radial_velocity_proxy,
scaled_time_of_flight,
);
if !derivative.is_finite() || derivative.abs() < 10.0 * f64::EPSILON {
universal_anomaly *= 0.5;
continue;
}
let raw_step = -residual / derivative;
let max_step_magnitude = 2.0 * (1.0 + universal_anomaly.abs());
let newton_step = raw_step.clamp(-max_step_magnitude, max_step_magnitude);
universal_anomaly += newton_step;
if newton_step.abs() < contr {
break;
}
}
universal_anomaly
}
fn prelim_parabolic_cardano(params: &UniversalKeplerParams) -> f64 {
let radial_distance = params.r0;
let radial_velocity_proxy = params.sig0;
let time_of_flight = params.dt;
let scaled_time_of_flight = params.mu.sqrt() * time_of_flight;
if time_of_flight == 0.0 {
return 0.0;
}
let leading_coefficient = 1.0 / 6.0;
let quadratic_coefficient = (radial_velocity_proxy / 2.0) / leading_coefficient;
let linear_coefficient = radial_distance / leading_coefficient;
let constant_coefficient = -scaled_time_of_flight / leading_coefficient;
let depression_shift = quadratic_coefficient / 3.0;
let depressed_linear_term = linear_coefficient - quadratic_coefficient * depression_shift;
let depressed_constant_term = 2.0 * depression_shift.powi(3)
- linear_coefficient * depression_shift
+ constant_coefficient;
let half_constant_term = depressed_constant_term / 2.0;
let discriminant =
half_constant_term * half_constant_term + (depressed_linear_term / 3.0).powi(3);
let candidate_roots: Vec<f64> = if discriminant > 0.0 {
let sqrt_discriminant = discriminant.sqrt();
let depressed_root = (-half_constant_term + sqrt_discriminant).cbrt()
+ (-half_constant_term - sqrt_discriminant).cbrt();
vec![depressed_root - depression_shift]
} else {
three_real_roots_trigonometric(depressed_linear_term, depressed_constant_term)
.into_iter()
.map(|depressed_root| depressed_root - depression_shift)
.collect()
};
let selected_root = select_physical_root(
&candidate_roots,
radial_distance,
radial_velocity_proxy,
scaled_time_of_flight,
);
polish_root_by_newton(
selected_root,
radial_distance,
radial_velocity_proxy,
scaled_time_of_flight,
)
}
const POLISHING_ITERATIONS: usize = 2;
fn polish_root_by_newton(
initial_estimate: f64,
radial_distance: f64,
radial_velocity_proxy: f64,
scaled_time_of_flight: f64,
) -> f64 {
let mut universal_anomaly = initial_estimate;
for _ in 0..POLISHING_ITERATIONS {
let (residual, derivative) = cubic_residual_and_derivative(
universal_anomaly,
radial_distance,
radial_velocity_proxy,
scaled_time_of_flight,
);
if derivative == 0.0 || !derivative.is_finite() {
break;
}
universal_anomaly -= residual / derivative;
}
universal_anomaly
}
fn three_real_roots_trigonometric(linear_term: f64, constant_term: f64) -> [f64; 3] {
use std::f64::consts::PI;
let acos_argument = ((3.0 * constant_term) / (2.0 * linear_term) * (-3.0 / linear_term).sqrt())
.clamp(-1.0, 1.0);
let base_angle = acos_argument.acos() / 3.0;
let amplitude = 2.0 * (-linear_term / 3.0).sqrt();
[
amplitude * base_angle.cos(),
amplitude * (base_angle - 2.0 * PI / 3.0).cos(),
amplitude * (base_angle - 4.0 * PI / 3.0).cos(),
]
}
fn select_physical_root(
candidate_roots: &[f64],
radial_distance: f64,
radial_velocity_proxy: f64,
scaled_time_of_flight: f64,
) -> f64 {
let linear_estimate = scaled_time_of_flight / radial_distance;
let closest_to_linear_estimate = |roots: &[f64]| -> f64 {
*roots
.iter()
.min_by(|first_root, second_root| {
(**first_root - linear_estimate)
.abs()
.partial_cmp(&(**second_root - linear_estimate).abs())
.unwrap()
})
.expect("candidate_roots must be non-empty")
};
let monotonic_branch_roots: Vec<f64> = candidate_roots
.iter()
.copied()
.filter(|&universal_anomaly| {
let (_, derivative) = cubic_residual_and_derivative(
universal_anomaly,
radial_distance,
radial_velocity_proxy,
scaled_time_of_flight,
);
derivative >= 0.0
})
.collect();
if monotonic_branch_roots.is_empty() {
closest_to_linear_estimate(candidate_roots)
} else {
closest_to_linear_estimate(&monotonic_branch_roots)
}
}
#[cfg(test)]
mod prelim_parabolic_tests {
use super::*;
use crate::kepler::params::SolverType;
const GRAVITATIONAL_PARAMETER: f64 = 2.959_122_082_855_911_5E-4;
fn make_params(
time_of_flight: f64,
radial_distance: f64,
radial_velocity_proxy: f64,
) -> UniversalKeplerParams {
UniversalKeplerParams {
dt: time_of_flight,
r0: radial_distance,
sig0: radial_velocity_proxy,
mu: GRAVITATIONAL_PARAMETER,
alpha: 0.0,
e0: 1.0,
solver_type: SolverType::default(),
}
}
fn residual_at(params: &UniversalKeplerParams, universal_anomaly: f64) -> f64 {
cubic_residual_and_derivative(
universal_anomaly,
params.r0,
params.sig0,
params.mu.sqrt() * params.dt,
)
.0
}
#[test]
fn zero_time_of_flight_gives_zero_anomaly() {
let params = make_params(0.0, 1.3, 0.05);
for method in [
ParabolicPrelimMethod::Cardano,
ParabolicPrelimMethod::Newton,
] {
let universal_anomaly = prelim_parabolic(¶ms);
assert!(
universal_anomaly.abs() < 1e-9,
"{method:?} gave {universal_anomaly} instead of ~0"
);
}
}
#[test]
fn cardano_and_newton_agree_on_monotonic_case() {
let mut params = make_params(5.0, 1.3, 0.02);
let psi_cardano = prelim_parabolic(¶ms);
params.solver_type.params.parabolic_solving_method = ParabolicPrelimMethod::Newton;
let psi_newton = prelim_parabolic(¶ms);
assert!((psi_cardano - psi_newton).abs() < 1e-6);
assert!(residual_at(¶ms, psi_cardano).abs() < 1e-6);
}
#[test]
fn newton_falls_back_to_cardano_when_non_monotonic() {
let mut params = make_params(
35.929_395_402_202_93,
1.242_343_356_943_632_2,
-5.995_054_062_733_072,
);
assert!(is_cubic_non_monotonic(params.r0, params.sig0));
let psi_cardano = prelim_parabolic(¶ms);
params.solver_type.params.parabolic_solving_method = ParabolicPrelimMethod::Newton;
let psi_newton = prelim_parabolic(¶ms);
assert!((psi_cardano - psi_newton).abs() < 1e-6);
assert!(residual_at(¶ms, psi_newton).abs() < 1e-3);
}
#[test]
fn negative_time_of_flight_gives_negative_anomaly() {
let mut params = make_params(-4.0, 1.1, 0.02);
let psi_cardano = prelim_parabolic(¶ms);
params.solver_type.params.parabolic_solving_method = ParabolicPrelimMethod::Newton;
let psi_newton = prelim_parabolic(¶ms);
assert!(psi_cardano < 0.0);
assert!(psi_newton < 0.0);
assert!((psi_cardano - psi_newton).abs() < 1e-6);
}
#[test]
fn residual_is_small_across_a_parameter_grid() {
let radial_distances = [0.2, 1.0, 3.0];
let radial_velocities = [-6.0, -0.5, 0.0, 0.5, 6.0];
let times_of_flight = [-50.0, -1.0, 0.0, 1.0, 50.0];
for &r0 in &radial_distances {
for &sig0 in &radial_velocities {
for &dt in ×_of_flight {
let mut params = make_params(dt, r0, sig0);
for method in [
ParabolicPrelimMethod::Cardano,
ParabolicPrelimMethod::Newton,
] {
params.solver_type.params.parabolic_solving_method = method;
let universal_anomaly = prelim_parabolic(¶ms);
let residual = residual_at(¶ms, universal_anomaly);
assert!(
residual.abs() < 1e-3,
"{method:?} residual too large for r0={r0}, sig0={sig0}, dt={dt}: {residual}"
);
}
}
}
}
}
mod proptests {
use super::*;
use crate::kepler::params::SolverType;
use proptest::prelude::*;
const GRAVITATIONAL_PARAMETER: f64 = 2.959_122_082_855_911_5E-4;
fn make_params(
time_of_flight: f64,
radial_distance: f64,
radial_velocity_proxy: f64,
) -> UniversalKeplerParams {
UniversalKeplerParams {
dt: time_of_flight,
r0: radial_distance,
sig0: radial_velocity_proxy,
mu: GRAVITATIONAL_PARAMETER,
alpha: 0.0,
e0: 1.0,
solver_type: SolverType::default(),
}
}
fn radial_distance_strategy() -> impl Strategy<Value = f64> {
0.05..10.0f64
}
fn radial_velocity_strategy() -> impl Strategy<Value = f64> {
-10.0..10.0f64
}
fn nonzero_time_of_flight_strategy() -> impl Strategy<Value = f64> {
prop_oneof![(-500.0..-1e-6f64), (1e-6..500.0f64)]
}
proptest! {
#[test]
fn prelim_parabolic_never_returns_non_finite(
radial_distance in radial_distance_strategy(),
radial_velocity_proxy in radial_velocity_strategy(),
time_of_flight in nonzero_time_of_flight_strategy(),
) {
let mut params = make_params(time_of_flight, radial_distance, radial_velocity_proxy);
for method in [ParabolicPrelimMethod::Cardano, ParabolicPrelimMethod::Newton] {
params.solver_type.params.parabolic_solving_method = method;
let universal_anomaly = prelim_parabolic(¶ms);
prop_assert!(
universal_anomaly.is_finite(),
"{method:?} returned a non-finite value for r0={radial_distance}, \
sig0={radial_velocity_proxy}, dt={time_of_flight}"
);
}
}
#[test]
fn cardano_root_satisfies_cubic_residual(
radial_distance in radial_distance_strategy(),
radial_velocity_proxy in radial_velocity_strategy(),
time_of_flight in nonzero_time_of_flight_strategy(),
) {
let mut params = make_params(time_of_flight, radial_distance, radial_velocity_proxy);
params.solver_type.params.max_iter_prelim_kepuni = 100;
let universal_anomaly = prelim_parabolic(¶ms);
let scaled_time_of_flight = GRAVITATIONAL_PARAMETER.sqrt() * time_of_flight;
let (residual, _) = cubic_residual_and_derivative(
universal_anomaly,
radial_distance,
radial_velocity_proxy,
scaled_time_of_flight,
);
let term_magnitude_scale = (universal_anomaly.powi(3) / 6.0).abs()
+ (radial_velocity_proxy / 2.0 * universal_anomaly.powi(2)).abs()
+ (radial_distance * universal_anomaly).abs()
+ scaled_time_of_flight.abs();
let tolerance = 1e-9 * term_magnitude_scale.max(1.0);
prop_assert!(
residual.abs() < tolerance,
"residual {residual} exceeds tolerance {tolerance} (term scale \
{term_magnitude_scale}) for r0={radial_distance}, \
sig0={radial_velocity_proxy}, dt={time_of_flight}"
);
}
#[test]
fn newton_agrees_with_cardano(
radial_distance in radial_distance_strategy(),
radial_velocity_proxy in radial_velocity_strategy(),
time_of_flight in nonzero_time_of_flight_strategy(),
) {
let mut params = make_params(time_of_flight, radial_distance, radial_velocity_proxy);
params.solver_type.params.max_iter_prelim_kepuni = 100;
let psi_cardano = prelim_parabolic(¶ms);
params.solver_type.params.parabolic_solving_method = ParabolicPrelimMethod::Newton;
let psi_newton = prelim_parabolic(¶ms);
let tolerance = 1e-4 * (1.0 + psi_cardano.abs());
prop_assert!(
(psi_cardano - psi_newton).abs() < tolerance,
"Cardano={psi_cardano} vs Newton={psi_newton} disagree beyond {tolerance} \
for r0={radial_distance}, sig0={radial_velocity_proxy}, dt={time_of_flight}"
);
}
#[test]
fn zero_time_of_flight_is_always_the_zero_root(
radial_distance in radial_distance_strategy(),
radial_velocity_proxy in radial_velocity_strategy(),
) {
let mut params = make_params(0.0, radial_distance, radial_velocity_proxy);
params.solver_type.params.max_iter_prelim_kepuni = 100;
for method in [ParabolicPrelimMethod::Cardano, ParabolicPrelimMethod::Newton] {
params.solver_type.params.parabolic_solving_method = method;
let universal_anomaly = prelim_parabolic(¶ms);
prop_assert!(
universal_anomaly.abs() < 1e-8,
"{method:?} gave {universal_anomaly} instead of 0 at dt=0 \
for r0={radial_distance}, sig0={radial_velocity_proxy}"
);
}
}
#[test]
fn zero_radial_velocity_gives_odd_symmetry_in_time_of_flight(
radial_distance in radial_distance_strategy(),
time_of_flight in 1e-6..500.0f64,
) {
let mut params_forward = make_params(time_of_flight, radial_distance, 0.0);
params_forward.solver_type.params.max_iter_prelim_kepuni = 100;
let mut params_backward = make_params(-time_of_flight, radial_distance, 0.0);
params_backward.solver_type.params.max_iter_prelim_kepuni = 100;
for method in [ParabolicPrelimMethod::Cardano, ParabolicPrelimMethod::Newton] {
params_forward.solver_type.params.parabolic_solving_method = method;
let psi_forward = prelim_parabolic(¶ms_forward);
params_backward.solver_type.params.parabolic_solving_method = method;
let psi_backward = prelim_parabolic(¶ms_backward);
prop_assert!(
(psi_forward + psi_backward).abs() < 1e-6 * (1.0 + psi_forward.abs()),
"{method:?}: psi(dt)={psi_forward} and psi(-dt)={psi_backward} \
are not opposite for r0={radial_distance}, dt={time_of_flight}"
);
}
}
#[test]
fn sign_matches_time_of_flight_in_monotonic_regime(
radial_distance in radial_distance_strategy(),
time_of_flight in nonzero_time_of_flight_strategy(),
) {
let radial_velocity_proxy = 0.0;
let mut params = make_params(time_of_flight, radial_distance, radial_velocity_proxy);
params.solver_type.params.max_iter_prelim_kepuni = 100;
prop_assert!(!is_cubic_non_monotonic(
radial_distance,
radial_velocity_proxy,
));
for method in [ParabolicPrelimMethod::Cardano, ParabolicPrelimMethod::Newton] {
params.solver_type.params.parabolic_solving_method = method;
let universal_anomaly = prelim_parabolic(¶ms);
prop_assert!(universal_anomaly.signum() == time_of_flight.signum());
}
}
}
}
}