#[derive(Clone, Copy, Debug, PartialEq)]
pub struct CloseApproach {
pub tca: f64,
pub distance: f64,
pub relative_speed: f64,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum ScreeningError {
Step(f64),
TooManySamples {
step: f64,
requested: f64,
},
Tolerance(f64),
Span {
start: f64,
end: f64,
},
Coverage(f64),
}
impl core::fmt::Display for ScreeningError {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self {
Self::Step(step) => {
write!(f, "sample interval {step} is not positive and finite")
}
Self::TooManySamples { step, requested } => {
write!(
f,
"a {step} s step over this span is {requested} samples, past the {MAX_SAMPLES} a scan will take"
)
}
Self::Tolerance(tolerance) => {
write!(
f,
"refinement tolerance {tolerance} is not positive and finite"
)
}
Self::Span { start, end } => {
write!(
f,
"scan span [{start}, {end}] is empty, inverted or not finite"
)
}
Self::Coverage(epoch) => {
write!(f, "the relative state is not finite at epoch {epoch}")
}
}
}
}
impl core::error::Error for ScreeningError {}
fn dot(a: &[f64; 3], b: &[f64; 3]) -> f64 {
a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}
fn norm(a: &[f64; 3]) -> f64 {
libm::sqrt(dot(a, a))
}
pub fn closest_approaches<F>(
relative: F,
start: f64,
end: f64,
step: f64,
tolerance: f64,
emit: &mut dyn FnMut(CloseApproach),
) -> Result<(), ScreeningError>
where
F: Fn(f64) -> ([f64; 3], [f64; 3]),
{
if !step.is_finite() || step <= 0.0 {
return Err(ScreeningError::Step(step));
}
if !tolerance.is_finite() || tolerance <= 0.0 {
return Err(ScreeningError::Tolerance(tolerance));
}
let span = end - start;
if !span.is_finite() || span <= 0.0 {
return Err(ScreeningError::Span { start, end });
}
let requested = libm::ceil(span / step);
#[expect(
clippy::cast_possible_truncation,
clippy::cast_sign_loss,
reason = "clamped to [1, MAX_SAMPLES] on the line above, so neither can happen"
)]
let count = requested.clamp(1.0, MAX_SAMPLES) as usize;
if requested > MAX_SAMPLES {
return Err(ScreeningError::TooManySamples { step, requested });
}
let rate = |t: f64| {
let (dr, dv) = relative(t);
dot(&dr, &dv)
};
let mut previous_t = start;
let mut previous = rate(start);
if !previous.is_finite() {
return Err(ScreeningError::Coverage(start));
}
for i in 1..=count {
#[expect(
clippy::cast_precision_loss,
reason = "i <= MAX_SAMPLES = 1e6, exactly representable in f64"
)]
let t = (start + i as f64 * step).min(end);
let current = rate(t);
if !current.is_finite() {
return Err(ScreeningError::Coverage(t));
}
if previous < 0.0 && current >= 0.0 {
emit(refine(&relative, previous_t, t, tolerance));
}
previous_t = t;
previous = current;
}
Ok(())
}
pub const MAX_SAMPLES: f64 = 1.0e6;
const MAX_BISECTIONS: usize = 2_100;
fn refine<F>(relative: &F, mut a: f64, mut b: f64, tolerance: f64) -> CloseApproach
where
F: Fn(f64) -> ([f64; 3], [f64; 3]),
{
for _ in 0..MAX_BISECTIONS {
if b - a <= tolerance {
break;
}
let mid = f64::midpoint(a, b);
if mid <= a || mid >= b {
break;
}
let (dr, dv) = relative(mid);
if dot(&dr, &dv) < 0.0 {
a = mid;
} else {
b = mid;
}
}
let tca = f64::midpoint(a, b);
let (dr, dv) = relative(tca);
CloseApproach {
tca,
distance: norm(&dr),
relative_speed: norm(&dv),
}
}
#[must_use]
pub fn radially_separated(
perigee_a: f64,
apogee_a: f64,
perigee_b: f64,
apogee_b: f64,
threshold: f64,
) -> bool {
if !perigee_a.is_finite()
|| !apogee_a.is_finite()
|| !perigee_b.is_finite()
|| !apogee_b.is_finite()
|| !threshold.is_finite()
{
return false;
}
perigee_a - apogee_b > threshold || perigee_b - apogee_a > threshold
}
#[cfg(test)]
mod tests {
#![allow(clippy::expect_used)]
#![allow(clippy::cast_precision_loss)]
use super::*;
fn separation(d: &[f64; 3]) -> f64 {
libm::sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2])
}
fn rectilinear(closest: f64, speed: f64, offset: f64) -> impl Fn(f64) -> ([f64; 3], [f64; 3]) {
move |t: f64| ([speed * t - offset, closest, 0.0], [speed, 0.0, 0.0])
}
fn collect<F>(
relative: F,
start: f64,
end: f64,
step: f64,
tolerance: f64,
) -> Result<Vec<CloseApproach>, ScreeningError>
where
F: Fn(f64) -> ([f64; 3], [f64; 3]),
{
let mut out = Vec::new();
closest_approaches(relative, start, end, step, tolerance, &mut |c| out.push(c))?;
Ok(out)
}
#[test]
fn a_rectilinear_pass_is_found_where_the_closed_form_puts_it() {
let (closest, speed, offset) = (1_500.0, 12_000.0, 600_000.0);
let found = collect(rectilinear(closest, speed, offset), 0.0, 120.0, 5.0, 1e-9)
.expect("well-formed arguments");
assert_eq!(found.len(), 1, "{found:?}");
let expected = offset / speed;
assert!(
libm::fabs(found[0].tca - expected) < 1e-9,
"{} s against the closed-form {expected} s",
found[0].tca
);
assert!(
libm::fabs(found[0].distance - closest) < 1e-3,
"{} m against the {closest} m the geometry has",
found[0].distance
);
assert!(libm::fabs(found[0].relative_speed - speed) < 1e-9);
}
const MU: f64 = 3.986_004_418e14;
fn mean_motion(r: f64) -> f64 {
libm::sqrt(MU / (r * r * r))
}
fn co_planar(r1: f64, r2: f64, phase: f64) -> impl Fn(f64) -> ([f64; 3], [f64; 3]) {
let (n1, n2) = (mean_motion(r1), mean_motion(r2));
move |t: f64| {
let (s1, c1) = (libm::sin(n1 * t), libm::cos(n1 * t));
let (s2, c2) = (libm::sin(n2 * t + phase), libm::cos(n2 * t + phase));
(
[r2 * c2 - r1 * c1, r2 * s2 - r1 * s1, 0.0],
[
-r2 * n2 * s2 + r1 * n1 * s1,
r2 * n2 * c2 - r1 * n1 * c1,
0.0,
],
)
}
}
fn inclined(r1: f64, r2: f64, inc: f64, phase: f64) -> impl Fn(f64) -> ([f64; 3], [f64; 3]) {
let (n1, n2) = (mean_motion(r1), mean_motion(r2));
let (si, ci) = (libm::sin(inc), libm::cos(inc));
move |t: f64| {
let (s1, c1) = (libm::sin(n1 * t), libm::cos(n1 * t));
let (s2, c2) = (libm::sin(n2 * t + phase), libm::cos(n2 * t + phase));
(
[r2 * c2 - r1 * c1, r2 * s2 * ci - r1 * s1, r2 * s2 * si],
[
-r2 * n2 * s2 + r1 * n1 * s1,
r2 * n2 * c2 * ci - r1 * n1 * c1,
r2 * n2 * c2 * si,
],
)
}
}
fn beat(r1: f64, r2: f64) -> f64 {
core::f64::consts::TAU / libm::fabs(mean_motion(r2) - mean_motion(r1))
}
fn dense_minima<F>(motion: &F, span: f64, samples: usize) -> Vec<f64>
where
F: Fn(f64) -> ([f64; 3], [f64; 3]),
{
let at = |i: usize| separation(&motion(span * i as f64 / samples as f64).0);
let mut minima = Vec::new();
for i in 1..samples {
let (before, here, after) = (at(i - 1), at(i), at(i + 1));
if here < before && here <= after {
minima.push(span * i as f64 / samples as f64);
}
}
minima
}
const DENSE_SAMPLES: usize = 200_000;
#[test]
fn every_minimum_in_the_span_is_found_and_agrees_with_a_dense_scan() {
let (r1, r2) = (7_000_000.0_f64, 7_500_000.0_f64);
let motion = co_planar(r1, r2, 1.0);
let span = 2.0 * beat(r1, r2);
let found = collect(&motion, 0.0, span, 30.0, 1e-6).expect("well-formed arguments");
assert!(found.len() >= 2, "only {} minima found", found.len());
let minima = dense_minima(&motion, span, DENSE_SAMPLES);
assert_eq!(
found.len(),
minima.len(),
"search found {} minima, the dense scan {}",
found.len(),
minima.len()
);
let resolution = span / DENSE_SAMPLES as f64;
for (c, &t) in found.iter().zip(&minima) {
assert!(
libm::fabs(c.tca - t) <= resolution,
"{} s against the dense scan's {t} s, past its own {resolution} s resolution",
c.tca
);
}
for pair in found.windows(2) {
assert!(pair[0].tca < pair[1].tca, "{found:?} is not in time order");
}
}
#[test]
fn an_inclined_pair_exercises_the_out_of_plane_component() {
let (r1, r2) = (7_000_000.0_f64, 7_500_000.0_f64);
let motion = inclined(r1, r2, 63.4_f64.to_radians(), 1.0);
let span = 4.0 * core::f64::consts::TAU / mean_motion(r1);
let (dr, dv) = motion(0.3 * span);
assert!(
libm::fabs(dr[2]) > 1.0e5 && libm::fabs(dv[2]) > 1.0e2,
"the out-of-plane components are {} m and {} m/s, too small to exercise anything",
dr[2],
dv[2]
);
let found = collect(&motion, 0.0, span, 15.0, 1e-6).expect("well-formed arguments");
let minima = dense_minima(&motion, span, DENSE_SAMPLES);
assert_eq!(
found.len(),
minima.len(),
"search found {} minima, the dense scan {}",
found.len(),
minima.len()
);
let resolution = span / DENSE_SAMPLES as f64;
for (c, &t) in found.iter().zip(&minima) {
assert!(
libm::fabs(c.tca - t) <= resolution,
"{} s against the dense scan's {t} s, past its own {resolution} s resolution",
c.tca
);
let (dr, dv) = motion(c.tca);
assert!(
libm::fabs(c.distance - separation(&dr)) < 1e-6,
"reported {} m against {} m at the same epoch",
c.distance,
separation(&dr)
);
assert!(
libm::fabs(c.relative_speed - separation(&dv)) < 1e-6,
"reported {} m/s against {} m/s at the same epoch",
c.relative_speed,
separation(&dv)
);
}
}
#[test]
fn a_maximum_of_the_separation_is_not_reported_as_an_approach() {
let (closest, speed, offset) = (1_500.0, 12_000.0, 600_000.0);
let found = collect(rectilinear(closest, speed, offset), 60.0, 120.0, 5.0, 1e-9)
.expect("well-formed arguments");
assert!(found.is_empty(), "{found:?}");
let found = collect(rectilinear(closest, speed, offset), 0.0, 40.0, 5.0, 1e-9)
.expect("well-formed arguments");
assert!(found.is_empty(), "{found:?}");
}
#[test]
fn a_step_too_coarse_to_resolve_two_minima_reports_one() {
let (r1, r2) = (7_000_000.0_f64, 7_500_000.0_f64);
let motion = co_planar(r1, r2, 1.0);
let span = 2.0 * beat(r1, r2);
let fine = collect(&motion, 0.0, span, 30.0, 1e-6).expect("well-formed arguments");
let coarse = collect(&motion, 0.0, span, 0.75 * span, 1e-6).expect("well-formed arguments");
assert!(
coarse.len() < fine.len(),
"a one-sample scan found {} of the {} minima, so this test no longer demonstrates the \
limitation it documents",
coarse.len(),
fine.len()
);
}
#[test]
fn every_malformed_argument_is_reported_as_itself() {
let good = rectilinear(1_500.0, 12_000.0, 600_000.0);
let covers_40_s = |t: f64| {
if t > 40.0 {
([f64::NAN; 3], [f64::NAN; 3])
} else {
good(t)
}
};
let cases = [
(0.0, 120.0, 0.0, 1e-9, ScreeningError::Step(0.0)),
(0.0, 120.0, -5.0, 1e-9, ScreeningError::Step(-5.0)),
(0.0, 120.0, f64::NAN, 1e-9, ScreeningError::Step(f64::NAN)),
(0.0, 120.0, 5.0, 0.0, ScreeningError::Tolerance(0.0)),
(0.0, 120.0, 5.0, -1e-9, ScreeningError::Tolerance(-1e-9)),
(
0.0,
120.0,
5.0,
f64::NAN,
ScreeningError::Tolerance(f64::NAN),
),
(
0.0,
0.0,
5.0,
1e-9,
ScreeningError::Span {
start: 0.0,
end: 0.0,
},
),
(
120.0,
0.0,
5.0,
1e-9,
ScreeningError::Span {
start: 120.0,
end: 0.0,
},
),
(
0.0,
f64::NAN,
5.0,
1e-9,
ScreeningError::Span {
start: 0.0,
end: f64::NAN,
},
),
(
f64::NAN,
120.0,
5.0,
1e-9,
ScreeningError::Span {
start: f64::NAN,
end: 120.0,
},
),
(
0.0,
f64::INFINITY,
5.0,
1e-9,
ScreeningError::Span {
start: 0.0,
end: f64::INFINITY,
},
),
(
f64::NEG_INFINITY,
120.0,
5.0,
1e-9,
ScreeningError::Span {
start: f64::NEG_INFINITY,
end: 120.0,
},
),
];
for (start, end, step, tolerance, expected) in cases {
let outcome = collect(covers_40_s, start, end, step, tolerance);
assert_eq!(
format!("{outcome:?}"),
format!("{:?}", Err::<Vec<CloseApproach>, _>(expected)),
"({start}, {end}, {step}, {tolerance})"
);
}
}
#[test]
fn a_step_below_the_epochs_resolution_still_advances_because_the_scan_is_indexed() {
let start = 1.0e16_f64;
assert_eq!(
(start + 1.0).to_bits(),
start.to_bits(),
"the epoch is not past f64's reach, so this test demonstrates nothing"
);
let motion = move |t: f64| {
let dt = t - start;
(
[12_000.0 * dt - 360_000.0, 1_500.0, 0.0],
[12_000.0, 0.0, 0.0],
)
};
let found = collect(motion, start, start + 60.0, 1.0, 1e-9).expect("well-formed arguments");
assert_eq!(found.len(), 1, "{found:?}");
assert!(
libm::fabs(found[0].tca - (start + 30.0)) <= 2.0,
"{} s against {} s, past the 2 s ulp that is all the type has left",
found[0].tca,
start + 30.0
);
}
#[test]
fn a_step_that_asks_for_more_samples_than_a_scan_will_take_is_refused() {
let outcome = collect(
rectilinear(1_500.0, 12_000.0, 600_000.0),
0.0,
60.0,
1e-9,
1e-12,
);
assert_eq!(
outcome,
Err(ScreeningError::TooManySamples {
step: 1e-9,
requested: 6.0e10
}),
"{outcome:?}"
);
}
#[test]
fn the_sample_ceiling_admits_exactly_its_own_count_and_no_more() {
let evaluations = core::cell::Cell::new(0_usize);
let end = MAX_SAMPLES;
let at_the_ceiling = collect(counted_exact_pass(&evaluations), 0.0, end, 1.0, 1.0);
assert!(at_the_ceiling.is_ok(), "{at_the_ceiling:?}");
#[expect(
clippy::cast_precision_loss,
reason = "the count is bounded by MAX_SAMPLES plus a bisection, well under 2^53"
)]
let taken = evaluations.get() as f64;
assert!(
taken > MAX_SAMPLES,
"{taken} evaluations, so the scan did not reach the ceiling"
);
let one_past = collect(
rectilinear(1_500.0, 12_000.0, 0.0),
0.0,
end + 1.0,
1.0,
1.0,
);
assert_eq!(
one_past,
Err(ScreeningError::TooManySamples {
step: 1.0,
requested: MAX_SAMPLES + 1.0
}),
"{one_past:?}"
);
}
#[test]
fn a_non_finite_sample_ends_the_scan_rather_than_inventing_an_approach() {
let good = rectilinear(1_500.0, 12_000.0, 600_000.0);
let stops_at_40 = |t: f64| {
if t > 40.0 {
([f64::NAN; 3], [f64::NAN; 3])
} else {
good(t)
}
};
assert_eq!(
collect(stops_at_40, 0.0, 120.0, 5.0, 1e-9),
Err(ScreeningError::Coverage(45.0))
);
let bad_from_the_start = |_: f64| ([f64::NAN; 3], [f64::NAN; 3]);
assert_eq!(
collect(bad_from_the_start, 0.0, 120.0, 5.0, 1e-9),
Err(ScreeningError::Coverage(0.0))
);
}
#[test]
fn every_error_names_its_own_argument() {
let messages = [
(
ScreeningError::Step(-5.0),
"sample interval -5 is not positive and finite",
),
(
ScreeningError::TooManySamples {
step: 1e-9,
requested: 6.0e10,
},
"a 0.000000001 s step over this span is 60000000000 samples, past the 1000000 a scan will take",
),
(
ScreeningError::Tolerance(0.0),
"refinement tolerance 0 is not positive and finite",
),
(
ScreeningError::Span {
start: 120.0,
end: 0.0,
},
"scan span [120, 0] is empty, inverted or not finite",
),
(
ScreeningError::Coverage(45.0),
"the relative state is not finite at epoch 45",
),
];
for (error, expected) in messages {
assert_eq!(format!("{error}"), expected);
let dynamic: &dyn core::error::Error = &error;
assert_eq!(format!("{dynamic}"), expected);
}
}
fn counted_exact_pass(
evaluations: &core::cell::Cell<usize>,
) -> impl Fn(f64) -> ([f64; 3], [f64; 3]) {
move |t: f64| {
evaluations.set(evaluations.get() + 1);
([1024.0 * t - 3072.0, 512.0, 0.0], [1024.0, 0.0, 0.0])
}
}
#[test]
fn the_refinement_costs_what_the_cost_model_says() {
let evaluations = core::cell::Cell::new(0_usize);
let found = collect(counted_exact_pass(&evaluations), 0.0, 8.0, 8.0, 1.0)
.expect("well-formed arguments");
assert_eq!(found.len(), 1, "{found:?}");
assert_eq!(
found[0].tca.to_bits(),
2.5_f64.to_bits(),
"{} s, where three exact halvings of [0, 8] put the bracket centre at 2.5",
found[0].tca
);
assert_eq!(evaluations.get(), 6);
}
#[test]
fn a_tolerance_finer_than_the_type_stops_at_the_types_own_resolution() {
let evaluations = core::cell::Cell::new(0_usize);
let found = collect(counted_exact_pass(&evaluations), 0.0, 8.0, 8.0, 1e-30)
.expect("well-formed arguments");
assert_eq!(found.len(), 1, "{found:?}");
assert!(
libm::fabs(found[0].tca - 3.0) <= 4.5e-16,
"{} s against the exact 3 s, further than one ulp",
found[0].tca
);
assert!(
evaluations.get() < 64,
"{} evaluations to exhaust the bracket",
evaluations.get()
);
}
#[test]
fn a_sample_exactly_at_the_minimum_still_opens_a_bracket() {
let (closest, speed, offset) = (1_500.0, 12_000.0, 600_000.0);
let exact = offset / speed;
let found = collect(
rectilinear(closest, speed, offset),
0.0,
2.0 * exact,
exact,
1e-9,
)
.expect("well-formed arguments");
assert_eq!(found.len(), 1, "{found:?}");
assert!(libm::fabs(found[0].tca - exact) < 1e-9);
}
#[test]
fn the_radial_filter_never_drops_a_pair_that_could_conjunct() {
let threshold = 5_000.0;
let r1 = 7_000_000.0_f64;
for gap in [1.0_f64, 4_000.0, 4_999.0, 5_000.0, 5_001.0, 10_000.0, 5.0e5] {
let r2 = r1 + gap;
let separated = radially_separated(r1, r1, r2, r2, threshold);
assert_eq!(
separated,
gap > threshold,
"at a {gap} m gap the filter said {separated}, and the exact closest approach of \
two concentric circles is the gap itself"
);
}
let r2 = r1 + 5.0e5;
let closest = collect(co_planar(r1, r2, 1.0), 0.0, 2.0 * beat(r1, r2), 30.0, 1e-6)
.expect("well-formed arguments")
.into_iter()
.map(|c| c.distance)
.fold(f64::INFINITY, f64::min);
assert!(
libm::fabs(closest - 5.0e5) < 1.0,
"the search's closest approach is {closest} m against the exact 500000 m"
);
}
#[test]
fn the_radial_filter_is_symmetric_and_refuses_non_finite_input() {
let (lo_p, lo_a, hi_p, hi_a) = (7.0e6, 7.1e6, 7.2e6, 7.3e6);
assert!(radially_separated(lo_p, lo_a, hi_p, hi_a, 50_000.0));
assert!(radially_separated(hi_p, hi_a, lo_p, lo_a, 50_000.0));
assert!(!radially_separated(7.0e6, 7.3e6, 7.1e6, 7.2e6, 0.0));
assert!(!radially_separated(hi_p, hi_a, lo_p, lo_a, hi_p - lo_a));
assert!(radially_separated(
hi_p,
hi_a,
lo_p,
lo_a,
hi_p - lo_a - 1.0
));
for bad in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] {
assert!(!radially_separated(bad, lo_a, hi_p, hi_a, 1.0));
assert!(!radially_separated(lo_p, bad, hi_p, hi_a, 1.0));
assert!(!radially_separated(lo_p, lo_a, bad, hi_a, 1.0));
assert!(!radially_separated(lo_p, lo_a, hi_p, bad, 1.0));
assert!(!radially_separated(lo_p, lo_a, hi_p, hi_a, bad));
}
}
}