Another project
1use bone_types::{AngleTolerance, Point2, Tolerance};
2use core::f64::consts::TAU;
3
4use crate::angles::wrap_delta;
5use crate::arc2::Arc2;
6use crate::circle2::Circle2;
7use crate::curve2::Curve2Kind;
8use crate::line2::Line2;
9
10#[derive(Copy, Clone, Debug, PartialEq)]
11pub enum IntersectionSet<P> {
12 Empty,
13 One(P),
14 Two(P, P),
15 Coincident,
16}
17
18pub type IntersectionSet2 = IntersectionSet<Point2>;
19
20impl<P: core::fmt::Display> core::fmt::Display for IntersectionSet<P> {
21 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
22 match self {
23 Self::Empty => write!(f, "empty"),
24 Self::One(p) => write!(f, "one{{ {p} }}"),
25 Self::Two(a, b) => write!(f, "two{{ {a}, {b} }}"),
26 Self::Coincident => write!(f, "coincident"),
27 }
28 }
29}
30
31#[must_use]
32pub fn intersect_curves(a: &Curve2Kind, b: &Curve2Kind, tolerance: Tolerance) -> IntersectionSet2 {
33 match (a, b) {
34 (Curve2Kind::Line(l), Curve2Kind::Line(m)) => line_line(l, m, tolerance),
35 (Curve2Kind::Line(l), Curve2Kind::Circle(c))
36 | (Curve2Kind::Circle(c), Curve2Kind::Line(l)) => line_circle(l, c, tolerance),
37 (Curve2Kind::Line(l), Curve2Kind::Arc(a)) | (Curve2Kind::Arc(a), Curve2Kind::Line(l)) => {
38 line_arc(l, a, tolerance)
39 }
40 (Curve2Kind::Circle(c), Curve2Kind::Circle(d)) => circle_circle(c, d, tolerance),
41 (Curve2Kind::Circle(c), Curve2Kind::Arc(a))
42 | (Curve2Kind::Arc(a), Curve2Kind::Circle(c)) => circle_arc(c, a, tolerance),
43 (Curve2Kind::Arc(a), Curve2Kind::Arc(b)) => arc_arc(a, b, tolerance),
44 }
45}
46
47fn line_line(a: &Line2, b: &Line2, tolerance: Tolerance) -> IntersectionSet2 {
48 let (ax, ay) = a.start().coords_mm();
49 let (ux, uy) = a.unit_direction().components();
50 let la = a.length_mm();
51 let (cx, cy) = b.start().coords_mm();
52 let (vx, vy) = b.unit_direction().components();
53 let lb = b.length_mm();
54 let eps = tolerance.value();
55
56 let det = ux * vy - uy * vx;
57 let angle_eps = eps / la.max(lb);
58
59 if det.abs() < angle_eps {
60 let dx = cx - ax;
61 let dy = cy - ay;
62 let perp = dx * uy - dy * ux;
63 if perp.abs() > eps {
64 return IntersectionSet::Empty;
65 }
66 let (ex, ey) = b.end().coords_mm();
67 let t_c = dx * ux + dy * uy;
68 let t_d = (ex - ax) * ux + (ey - ay) * uy;
69 let (t_lo, t_hi) = if t_c <= t_d { (t_c, t_d) } else { (t_d, t_c) };
70 let overlap_lo = t_lo.max(0.0);
71 let overlap_hi = t_hi.min(la);
72 let width = overlap_hi - overlap_lo;
73 if width < -eps {
74 return IntersectionSet::Empty;
75 }
76 if width <= eps {
77 let t = f64::midpoint(overlap_lo, overlap_hi);
78 return IntersectionSet::One(Point2::from_mm(ax + t * ux, ay + t * uy));
79 }
80 return IntersectionSet::Coincident;
81 }
82
83 let t = ((cx - ax) * vy - (cy - ay) * vx) / det;
84 let s = ((cx - ax) * uy - (cy - ay) * ux) / det;
85 if !in_segment(t, la, eps) || !in_segment(s, lb, eps) {
86 return IntersectionSet::Empty;
87 }
88 IntersectionSet::One(Point2::from_mm(ax + t * ux, ay + t * uy))
89}
90
91fn line_circle(line: &Line2, circle: &Circle2, tolerance: Tolerance) -> IntersectionSet2 {
92 let (ax, ay) = line.start().coords_mm();
93 let (ux, uy) = line.unit_direction().components();
94 let la = line.length_mm();
95 let (cx, cy) = circle.center().coords_mm();
96 let r = circle.radius_mm();
97 let eps = tolerance.value();
98
99 let fx = ax - cx;
100 let fy = ay - cy;
101 let b_coef = 2.0 * (fx * ux + fy * uy);
102 let c_coef = fx * fx + fy * fy - r * r;
103 let disc = b_coef * b_coef - 4.0 * c_coef;
104 if disc < -eps * eps {
105 return IntersectionSet::Empty;
106 }
107 let sqrt_disc = disc.max(0.0).sqrt();
108 let t1 = f64::midpoint(-b_coef, -sqrt_disc);
109 let t2 = f64::midpoint(-b_coef, sqrt_disc);
110
111 let p = |t: f64| Point2::from_mm(ax + t * ux, ay + t * uy);
112 let in1 = in_segment(t1, la, eps);
113 let in2 = in_segment(t2, la, eps);
114 if sqrt_disc < eps {
115 if in1 {
116 IntersectionSet::One(p(t1))
117 } else {
118 IntersectionSet::Empty
119 }
120 } else {
121 match (in1, in2) {
122 (true, true) => IntersectionSet::Two(p(t1), p(t2)),
123 (true, false) => IntersectionSet::One(p(t1)),
124 (false, true) => IntersectionSet::One(p(t2)),
125 (false, false) => IntersectionSet::Empty,
126 }
127 }
128}
129
130fn line_arc(line: &Line2, arc: &Arc2, tolerance: Tolerance) -> IntersectionSet2 {
131 let circle = arc.as_full_circle();
132 match line_circle(line, &circle, tolerance) {
133 IntersectionSet::Empty => IntersectionSet::Empty,
134 IntersectionSet::One(p) => {
135 if arc.contains_point(p, tolerance) {
136 IntersectionSet::One(p)
137 } else {
138 IntersectionSet::Empty
139 }
140 }
141 IntersectionSet::Two(p, q) => filter_two_by_arc(p, q, arc, tolerance),
142 IntersectionSet::Coincident => unreachable!("line_circle never returns Coincident"),
143 }
144}
145
146fn circle_circle(a: &Circle2, b: &Circle2, tolerance: Tolerance) -> IntersectionSet2 {
147 let (ax, ay) = a.center().coords_mm();
148 let (bx, by) = b.center().coords_mm();
149 let ra = a.radius_mm();
150 let rb = b.radius_mm();
151 let eps = tolerance.value();
152
153 let dx = bx - ax;
154 let dy = by - ay;
155 let d_sq = dx * dx + dy * dy;
156 let d = d_sq.sqrt();
157
158 if d < eps && (ra - rb).abs() < eps {
159 return IntersectionSet::Coincident;
160 }
161 if d > ra + rb + eps {
162 return IntersectionSet::Empty;
163 }
164 if d + ra.min(rb) < ra.max(rb) - eps {
165 return IntersectionSet::Empty;
166 }
167 if d < eps {
168 return IntersectionSet::Empty;
169 }
170
171 let a_proj = (d_sq + ra * ra - rb * rb) / (2.0 * d);
172 let h_sq = ra * ra - a_proj * a_proj;
173 let mx = ax + a_proj * dx / d;
174 let my = ay + a_proj * dy / d;
175
176 if h_sq.abs() < eps * eps {
177 return IntersectionSet::One(Point2::from_mm(mx, my));
178 }
179 if h_sq < 0.0 {
180 return IntersectionSet::Empty;
181 }
182 let h = h_sq.sqrt();
183 let px = -dy / d * h;
184 let py = dx / d * h;
185 IntersectionSet::Two(
186 Point2::from_mm(mx + px, my + py),
187 Point2::from_mm(mx - px, my - py),
188 )
189}
190
191fn circle_arc(circle: &Circle2, arc: &Arc2, tolerance: Tolerance) -> IntersectionSet2 {
192 let arc_full = arc.as_full_circle();
193 match circle_circle(circle, &arc_full, tolerance) {
194 IntersectionSet::Empty => IntersectionSet::Empty,
195 IntersectionSet::Coincident => IntersectionSet::Coincident,
196 IntersectionSet::One(p) => {
197 if arc.contains_point(p, tolerance) {
198 IntersectionSet::One(p)
199 } else {
200 IntersectionSet::Empty
201 }
202 }
203 IntersectionSet::Two(p, q) => filter_two_by_arc(p, q, arc, tolerance),
204 }
205}
206
207fn arc_arc(a: &Arc2, b: &Arc2, tolerance: Tolerance) -> IntersectionSet2 {
208 let a_full = a.as_full_circle();
209 let b_full = b.as_full_circle();
210 match circle_circle(&a_full, &b_full, tolerance) {
211 IntersectionSet::Empty => IntersectionSet::Empty,
212 IntersectionSet::Coincident => same_circle_arc_overlap(a, b, tolerance),
213 IntersectionSet::One(p) => {
214 if a.contains_point(p, tolerance) && b.contains_point(p, tolerance) {
215 IntersectionSet::One(p)
216 } else {
217 IntersectionSet::Empty
218 }
219 }
220 IntersectionSet::Two(p, q) => {
221 let p_ok = a.contains_point(p, tolerance) && b.contains_point(p, tolerance);
222 let q_ok = a.contains_point(q, tolerance) && b.contains_point(q, tolerance);
223 match (p_ok, q_ok) {
224 (true, true) => IntersectionSet::Two(p, q),
225 (true, false) => IntersectionSet::One(p),
226 (false, true) => IntersectionSet::One(q),
227 (false, false) => IntersectionSet::Empty,
228 }
229 }
230 }
231}
232
233fn same_circle_arc_overlap(a: &Arc2, b: &Arc2, tolerance: Tolerance) -> IntersectionSet2 {
234 let eps_angle = AngleTolerance::from_arc_length(tolerance, a.radius()).radians();
235 let (lo_a, mag_a) = canonical_range(a);
236 let (lo_b, mag_b) = canonical_range(b);
237
238 if mag_a >= TAU - eps_angle || mag_b >= TAU - eps_angle {
239 return IntersectionSet::Coincident;
240 }
241
242 let hi_a = lo_a + mag_a;
243
244 let overlaps: [Option<(f64, f64)>; 3] = [-TAU, 0.0, TAU].map(|shift| {
245 let lo_b_s = lo_b + shift;
246 let hi_b_s = lo_b_s + mag_b;
247 let o_lo = lo_a.max(lo_b_s);
248 let o_hi = hi_a.min(hi_b_s);
249 (o_hi + eps_angle >= o_lo).then_some((o_lo, o_hi))
250 });
251
252 let has_1d = overlaps
253 .iter()
254 .any(|o| matches!(o, Some((s, e)) if e - s > eps_angle));
255 if has_1d {
256 return IntersectionSet::Coincident;
257 }
258
259 let points: Vec<f64> = overlaps
260 .iter()
261 .filter_map(|o| o.map(|(s, e)| (0.5 * (s + e)).rem_euclid(TAU)))
262 .fold(Vec::new(), |mut acc, t| {
263 if !acc.iter().any(|&p: &f64| angle_close(p, t, eps_angle)) {
264 acc.push(t);
265 }
266 acc
267 });
268
269 match points.as_slice() {
270 [] => IntersectionSet::Empty,
271 [t] => IntersectionSet::One(arc_point_at_angle(a, *t)),
272 [t1, t2, ..] => {
273 IntersectionSet::Two(arc_point_at_angle(a, *t1), arc_point_at_angle(a, *t2))
274 }
275 }
276}
277
278fn canonical_range(arc: &Arc2) -> (f64, f64) {
279 let start = arc.start_rad();
280 let sweep = arc.sweep_rad();
281 let lo = if sweep >= 0.0 { start } else { start + sweep };
282 (lo.rem_euclid(TAU), sweep.abs())
283}
284
285fn arc_point_at_angle(arc: &Arc2, theta: f64) -> Point2 {
286 let (cx, cy) = arc.center().coords_mm();
287 let r = arc.radius_mm();
288 Point2::from_mm(cx + r * theta.cos(), cy + r * theta.sin())
289}
290
291fn angle_close(a: f64, b: f64, eps: f64) -> bool {
292 wrap_delta(a - b).abs() < eps
293}
294
295fn filter_two_by_arc(p: Point2, q: Point2, arc: &Arc2, tolerance: Tolerance) -> IntersectionSet2 {
296 let p_in = arc.contains_point(p, tolerance);
297 let q_in = arc.contains_point(q, tolerance);
298 match (p_in, q_in) {
299 (true, true) => IntersectionSet::Two(p, q),
300 (true, false) => IntersectionSet::One(p),
301 (false, true) => IntersectionSet::One(q),
302 (false, false) => IntersectionSet::Empty,
303 }
304}
305
306fn in_segment(t: f64, length: f64, eps: f64) -> bool {
307 t >= -eps && t <= length + eps
308}