323 lines
11 KiB
Rust
323 lines
11 KiB
Rust
//! 2D intersection math and distance queries over (x, y) pairs and degree-sweep arcs.
|
|
|
|
use num_complex::Complex64;
|
|
|
|
/// closeness factor for common-endpoint and small-interval rejection.
|
|
const EPS_FRAC: f64 = 1.0e-8;
|
|
|
|
/// recovers the center and radius of the circle through an arc's endpoints with the given sweep.
|
|
pub fn circle_from_arc(p0: (f64, f64), p1: (f64, f64), sweep_deg: f64) -> (f64, f64, f64) {
|
|
let a0 = Complex64::new(p0.0, p0.1);
|
|
let a1 = Complex64::new(p1.0, p1.1);
|
|
let chord = (a1 - a0).norm();
|
|
let t = (a1 - a0) / chord;
|
|
let tta = sweep_deg.to_radians();
|
|
let r = chord / (2.0 * (tta / 2.0).sin());
|
|
let c = a0 + (Complex64::new(chord / 2.0, (r * r - chord * chord / 4.0).sqrt())) * t;
|
|
(c.re, c.im, r.abs())
|
|
}
|
|
|
|
/// shortest distance from a point to a segment.
|
|
pub fn shortest_distance_from_segment(p: (f64, f64), a: (f64, f64), b: (f64, f64)) -> f64 {
|
|
let dx = b.0 - a.0;
|
|
let dy = b.1 - a.1;
|
|
let len2 = dx * dx + dy * dy;
|
|
if len2 < 1e-18 {
|
|
return ((p.0 - a.0).powi(2) + (p.1 - a.1).powi(2)).sqrt();
|
|
}
|
|
let t = (((p.0 - a.0) * dx + (p.1 - a.1) * dy) / len2).clamp(0.0, 1.0);
|
|
let cx = a.0 + t * dx;
|
|
let cy = a.1 + t * dy;
|
|
((p.0 - cx).powi(2) + (p.1 - cy).powi(2)).sqrt()
|
|
}
|
|
|
|
/// shortest distance from a point to a sweep-defined arc.
|
|
pub fn shortest_distance_from_arc(
|
|
p: (f64, f64),
|
|
a0: (f64, f64),
|
|
a1: (f64, f64),
|
|
sweep_deg: f64,
|
|
) -> f64 {
|
|
let (cx, cy, r) = circle_from_arc(a0, a1, sweep_deg);
|
|
let pc = Complex64::new(p.0 - cx, p.1 - cy);
|
|
let d = pc.norm();
|
|
if d == 0.0 {
|
|
return r;
|
|
}
|
|
let t = pc / d;
|
|
let foot = pc - Complex64::new(r * t.re, r * t.im);
|
|
let l = foot.norm();
|
|
let a0_dir = Complex64::new(a0.0 - cx, a0.1 - cy);
|
|
let z = ((t / a0_dir).arg() * 180.0 / std::f64::consts::PI + 360.0) % 360.0;
|
|
let sweep_abs = sweep_deg.abs();
|
|
if z > 0.0 && z < sweep_abs {
|
|
return l;
|
|
}
|
|
let e0 = ((p.0 - a0.0).powi(2) + (p.1 - a0.1).powi(2)).sqrt();
|
|
let e1 = ((p.0 - a1.0).powi(2) + (p.1 - a1.1).powi(2)).sqrt();
|
|
e0.min(e1)
|
|
}
|
|
|
|
/// intersection of the open segments p0->p1 and q0->q1, or None when none exists.
|
|
pub fn line_line_intersection(
|
|
p0: (f64, f64),
|
|
p1: (f64, f64),
|
|
q0: (f64, f64),
|
|
q1: (f64, f64),
|
|
) -> Option<(f64, f64)> {
|
|
let p0c = Complex64::new(p0.0, p0.1);
|
|
let p1c = Complex64::new(p1.0, p1.1);
|
|
let q0c = Complex64::new(q0.0, q0.1);
|
|
let q1c = Complex64::new(q1.0, q1.1);
|
|
|
|
// shared endpoint -> no other intersection
|
|
if (p0c - q0c).norm() < f64::EPSILON
|
|
|| (p0c - q1c).norm() < f64::EPSILON
|
|
|| (p1c - q0c).norm() < f64::EPSILON
|
|
|| (p1c - q1c).norm() < f64::EPSILON
|
|
{
|
|
return None;
|
|
}
|
|
|
|
let ee = ((p1c - p0c).norm()).min((q1c - q0c).norm()) * EPS_FRAC;
|
|
|
|
let denom = p1c - p0c;
|
|
if denom.norm() < f64::EPSILON {
|
|
return None;
|
|
}
|
|
let r0 = (q0c - p0c) / denom;
|
|
let r1 = (q1c - p0c) / denom;
|
|
|
|
if r0.re <= 0.0 && r1.re <= 0.0 { return None; }
|
|
if r0.re >= 1.0 && r1.re >= 1.0 { return None; }
|
|
if r0.im <= 0.0 && r1.im <= 0.0 { return None; }
|
|
if r0.im >= 0.0 && r1.im >= 0.0 { return None; }
|
|
|
|
let denom_im = r0.im - r1.im;
|
|
if denom_im.abs() < f64::EPSILON { return None; }
|
|
let z = r0.im / denom_im;
|
|
let x = ((1.0 - z) * r0 + z * r1).re;
|
|
if x < ee || x > 1.0 - ee {
|
|
return None;
|
|
}
|
|
|
|
let hit = Complex64::new(p0.0, p0.1) * (1.0 - z) + Complex64::new(q0.0, q0.1) * 0.0;
|
|
let _ = hit;
|
|
let result = Complex64::new(q0.0, q0.1) * (1.0 - z) + Complex64::new(q1.0, q1.1) * z;
|
|
Some((result.re, result.im))
|
|
}
|
|
|
|
/// intersection points of an open line segment with an open arc, returning 0, 1, or 2 hits.
|
|
pub fn line_arc_intersection(
|
|
p0: (f64, f64),
|
|
p1: (f64, f64),
|
|
a0: (f64, f64),
|
|
a1: (f64, f64),
|
|
sweep_deg: f64,
|
|
) -> Vec<(f64, f64)> {
|
|
let p0c = Complex64::new(p0.0, p0.1);
|
|
let p1c = Complex64::new(p1.0, p1.1);
|
|
let a0c = Complex64::new(a0.0, a0.1);
|
|
let (cx, cy, r) = circle_from_arc(a0, a1, sweep_deg);
|
|
let c = Complex64::new(cx, cy);
|
|
|
|
let d = (p1c - p0c).norm();
|
|
if d < f64::EPSILON {
|
|
return Vec::new();
|
|
}
|
|
let t = (p1c - p0c) / d;
|
|
let v = (c - p0c) / t;
|
|
|
|
if v.im.abs() > r {
|
|
return Vec::new();
|
|
}
|
|
let l = (r * r - v.im * v.im).sqrt();
|
|
let tta = sweep_deg.to_radians().abs();
|
|
|
|
let mut out = Vec::with_capacity(2);
|
|
|
|
if (l / r) < 1.0e-5 {
|
|
let hit = p0c + Complex64::new(v.re, 0.0) * t;
|
|
let r_param = ((hit - p0c) / t).re;
|
|
let z = ((hit - c) / (a0c - c)).arg();
|
|
if r_param > 0.0 && r_param < d && z > 0.0 && z < tta {
|
|
out.push((hit.re, hit.im));
|
|
}
|
|
return out;
|
|
}
|
|
|
|
for sign in [1.0, -1.0] {
|
|
let hit = p0c + Complex64::new(v.re + sign * l, 0.0) * t;
|
|
let r_param = ((hit - p0c) / t).re;
|
|
let z = ((hit - c) / (a0c - c)).arg();
|
|
if r_param > 0.0 && r_param < d && z > 0.0 && z < tta {
|
|
out.push((hit.re, hit.im));
|
|
}
|
|
}
|
|
out
|
|
}
|
|
|
|
/// intersection points of two open arcs, returning 0, 1, or 2 hits.
|
|
pub fn arc_arc_intersection(
|
|
a0_p0: (f64, f64),
|
|
a0_p1: (f64, f64),
|
|
a0_sweep_deg: f64,
|
|
a1_p0: (f64, f64),
|
|
a1_p1: (f64, f64),
|
|
a1_sweep_deg: f64,
|
|
) -> Vec<(f64, f64)> {
|
|
let (c0x, c0y, r0) = circle_from_arc(a0_p0, a0_p1, a0_sweep_deg);
|
|
let (c1x, c1y, r1) = circle_from_arc(a1_p0, a1_p1, a1_sweep_deg);
|
|
let c0 = Complex64::new(c0x, c0y);
|
|
let c1 = Complex64::new(c1x, c1y);
|
|
let d = (c1 - c0).norm();
|
|
|
|
if d > r0 + r1 || d < 1.0e-8 {
|
|
return Vec::new();
|
|
}
|
|
|
|
let l = ((r0 + r1 - d) * (d + r0 - r1) * (d - r0 + r1) * (d + r0 + r1)).sqrt() / (2.0 * d);
|
|
let c = 1.0 + (r0 / d) * (r0 / d) - (r1 / d) * (r1 / d);
|
|
let t = (c1 - c0) / d;
|
|
let tta0 = a0_sweep_deg.to_radians().abs();
|
|
let tta1 = a1_sweep_deg.to_radians().abs();
|
|
let a0c = Complex64::new(a0_p0.0, a0_p0.1);
|
|
let a1c = Complex64::new(a1_p0.0, a1_p0.1);
|
|
|
|
let mut out = Vec::with_capacity(2);
|
|
|
|
let first = c0 + Complex64::new(c * d / 2.0, l) * t;
|
|
let z0 = ((first - c0) / (a0c - c0)).arg();
|
|
let z1 = ((first - c1) / (a1c - c1)).arg();
|
|
if z0 > 0.0 && z0 < tta0 && z1 > 0.0 && z1 < tta1 {
|
|
out.push((first.re, first.im));
|
|
}
|
|
|
|
// tangent-touch case: only one intersection
|
|
if (d - r0 + r1).abs() / (r0 + r1) < 1.0e-5 {
|
|
return out;
|
|
}
|
|
|
|
let second = c0 + Complex64::new(c * d / 2.0, -l) * t;
|
|
let z0 = ((second - c0) / (a0c - c0)).arg();
|
|
let z1 = ((second - c1) / (a1c - c1)).arg();
|
|
if z0 > 0.0 && z0 < tta0 && z1 > 0.0 && z1 < tta1 {
|
|
out.push((second.re, second.im));
|
|
}
|
|
|
|
out
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
fn close(a: (f64, f64), b: (f64, f64), tol: f64) -> bool {
|
|
((a.0 - b.0).powi(2) + (a.1 - b.1).powi(2)).sqrt() < tol
|
|
}
|
|
|
|
#[test]
|
|
fn line_line_crosses_at_origin() {
|
|
// (-1,-1)->(1,1) crosses (-1,1)->(1,-1) at origin.
|
|
let hit = line_line_intersection((-1.0, -1.0), (1.0, 1.0), (-1.0, 1.0), (1.0, -1.0));
|
|
assert!(hit.is_some());
|
|
assert!(close(hit.unwrap(), (0.0, 0.0), 1e-9));
|
|
}
|
|
|
|
#[test]
|
|
fn line_line_parallel_no_intersection() {
|
|
let hit = line_line_intersection((0.0, 0.0), (1.0, 0.0), (0.0, 1.0), (1.0, 1.0));
|
|
assert!(hit.is_none());
|
|
}
|
|
|
|
#[test]
|
|
fn line_line_t_junction_at_endpoint_rejects() {
|
|
// the second segment's q0 sits ON the first segment's interior
|
|
// but the prospective endpoint check should reject (intersection at q0).
|
|
let hit = line_line_intersection((0.0, 0.0), (2.0, 0.0), (1.0, 0.0), (1.0, 1.0));
|
|
// shape: one endpoint sits on the other segment, no clean crossing.
|
|
// GetIntersection returns FALSE when an intersection lies within ee of an endpoint of the
|
|
// prospective line — here the meeting point IS the prospective q0, so no split is wanted.
|
|
assert!(hit.is_none());
|
|
}
|
|
|
|
#[test]
|
|
fn line_line_shared_endpoint_no_intersection() {
|
|
let hit = line_line_intersection((0.0, 0.0), (1.0, 0.0), (1.0, 0.0), (2.0, 1.0));
|
|
assert!(hit.is_none());
|
|
}
|
|
|
|
#[test]
|
|
fn circle_from_quarter_arc_unit() {
|
|
// a quarter circle (90 deg sweep) from (1, 0) to (0, 1) sits on the unit circle centered at origin.
|
|
let (cx, cy, r) = circle_from_arc((1.0, 0.0), (0.0, 1.0), 90.0);
|
|
assert!((cx - 0.0).abs() < 1e-9);
|
|
assert!((cy - 0.0).abs() < 1e-9);
|
|
assert!((r - 1.0).abs() < 1e-9);
|
|
}
|
|
|
|
#[test]
|
|
fn line_through_quarter_arc_crosses_once() {
|
|
// diagonal line from (-2, 0.5) to (2, 0.5) crosses the quarter circle (1,0)->(0,1) at one point.
|
|
let hits = line_arc_intersection((-2.0, 0.5), (2.0, 0.5), (1.0, 0.0), (0.0, 1.0), 90.0);
|
|
assert_eq!(hits.len(), 1);
|
|
let h = hits[0];
|
|
assert!((h.0 * h.0 + h.1 * h.1 - 1.0).abs() < 1e-9, "point should lie on unit circle: {h:?}");
|
|
assert!((h.1 - 0.5).abs() < 1e-9);
|
|
assert!(h.0 > 0.0);
|
|
}
|
|
|
|
#[test]
|
|
fn line_outside_arc_does_not_cross() {
|
|
// line clearly outside the arc swept region.
|
|
let hits = line_arc_intersection((-2.0, 2.5), (2.0, 2.5), (1.0, 0.0), (0.0, 1.0), 90.0);
|
|
assert!(hits.is_empty());
|
|
}
|
|
|
|
#[test]
|
|
fn arc_arc_two_intersections() {
|
|
// two unit circles offset by 1.0 along x, both swept 360 degrees... but we use partial arcs
|
|
// shaped to clip both crossings in their swept range.
|
|
// circle A: center (0,0), arcs covering full top half (180 deg from (1,0) to (-1,0)).
|
|
// circle B: center (1,0), arcs covering full top half (180 deg from (2,0) to (0,0)).
|
|
let hits = arc_arc_intersection(
|
|
(1.0, 0.0), (-1.0, 0.0), 180.0,
|
|
(2.0, 0.0), (0.0, 0.0), 180.0,
|
|
);
|
|
assert_eq!(hits.len(), 1, "two top-half arcs of overlapping unit circles meet at one upper crossing: {hits:?}");
|
|
let h = hits[0];
|
|
assert!((h.0 - 0.5).abs() < 1e-9, "x = 0.5 by symmetry, got {h:?}");
|
|
assert!(h.1 > 0.0, "intersection should be above the x-axis: {h:?}");
|
|
}
|
|
|
|
#[test]
|
|
fn arc_arc_disjoint() {
|
|
let hits = arc_arc_intersection(
|
|
(1.0, 0.0), (-1.0, 0.0), 180.0,
|
|
(11.0, 0.0), (9.0, 0.0), 180.0,
|
|
);
|
|
assert!(hits.is_empty());
|
|
}
|
|
|
|
#[test]
|
|
fn shortest_distance_from_segment_perpendicular() {
|
|
// point (0, 1) to segment (-1, 0)->(1, 0) is distance 1.
|
|
let d = shortest_distance_from_segment((0.0, 1.0), (-1.0, 0.0), (1.0, 0.0));
|
|
assert!((d - 1.0).abs() < 1e-9);
|
|
}
|
|
|
|
#[test]
|
|
fn shortest_distance_from_segment_clamps_to_endpoint() {
|
|
// point (-2, 0) to segment (-1, 0)->(1, 0) is distance 1 (clamps to left endpoint).
|
|
let d = shortest_distance_from_segment((-2.0, 0.0), (-1.0, 0.0), (1.0, 0.0));
|
|
assert!((d - 1.0).abs() < 1e-9);
|
|
}
|
|
|
|
#[test]
|
|
fn shortest_distance_from_arc_radial() {
|
|
// point at origin distance to unit-circle quarter arc is 1.
|
|
let d = shortest_distance_from_arc((0.0, 0.0), (1.0, 0.0), (0.0, 1.0), 90.0);
|
|
assert!((d - 1.0).abs() < 1e-9);
|
|
}
|
|
}
|