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