fn normalize_near(angle: f64, reference: f64) -> f64 {
let two_pi = std::f64::consts::TAU;
let mut normalized = angle;
while normalized - reference > std::f64::consts::PI {
normalized -= two_pi;
}
while normalized - reference < -std::f64::consts::PI {
normalized += two_pi;
}
normalized
}
pub fn farthest_feasible_index(coords: &[(f64, f64)], start: usize, tolerance: f64) -> usize {
let n = coords.len();
if start + 1 >= n {
return start;
}
let (ax, ay) = coords[start];
let mut wedge: Option<(f64, f64)> = None;
let mut farthest = start;
for (k, &(px, py)) in coords.iter().enumerate().take(n).skip(start + 1) {
let dx = px - ax;
let dy = py - ay;
let distance = (dx * dx + dy * dy).sqrt();
if distance > tolerance {
let mut theta = dy.atan2(dx);
let alpha = (tolerance / distance).clamp(-1.0, 1.0).asin();
let (lo, hi) = match wedge {
None => (theta - alpha, theta + alpha),
Some((lo, hi)) => {
let center = (lo + hi) / 2.0;
theta = normalize_near(theta, center);
(lo.max(theta - alpha), hi.min(theta + alpha))
}
};
if lo > hi {
return farthest;
}
wedge = Some((lo, hi));
}
farthest = k;
}
farthest
}