1use axiolid_core::{Point2, Scalar};
91use axiolid_exact::{Arith, Dyadic, Tower};
92use axiolid_guarantees::Sign;
93
94use crate::arc::ArcRing;
95use crate::exact_arc::boxes::BoxTree;
96use crate::exact_arc::edges_of;
97
98#[derive(Debug, Clone, Copy, PartialEq, Default)]
103#[non_exhaustive]
104pub struct ArcCheckOptions {
105 pub clearance: Option<Scalar>,
108}
109
110impl ArcCheckOptions {
111 #[must_use]
113 pub const fn new() -> Self {
114 Self { clearance: None }
115 }
116
117 #[must_use]
124 pub const fn with_clearance(mut self, distance: Scalar) -> Self {
125 self.clearance = Some(distance);
126 self
127 }
128}
129
130#[derive(Debug, Clone, Copy, PartialEq)]
133pub(crate) struct NearPair {
134 pub(crate) edges: [(usize, usize); 2],
135 pub(crate) lower_bound: f64,
136 pub(crate) undecided: bool,
137}
138
139pub(crate) fn near_pairs(rings: &[&ArcRing], usable: &[bool], clearance: f64) -> Vec<NearPair> {
142 near_pairs_by(rings, usable, clearance, |a, b| a.closer_than(b, clearance))
143}
144
145fn near_pairs_by(
148 rings: &[&ArcRing],
149 usable: &[bool],
150 clearance: f64,
151 decide: impl Fn(&Shape, &Shape) -> Verdict,
152) -> Vec<NearPair> {
153 let mut edges = Vec::new();
154 let mut shapes = Vec::new();
155 let mut owner: Vec<(usize, usize)> = Vec::new();
156 for (index, ring) in rings.iter().enumerate() {
157 if !usable[index] {
158 continue;
159 }
160 let n = ring.vertices.len();
161 for (e, edge) in edges_of(ring).into_iter().enumerate() {
162 let from = ring.vertices[e];
163 let to = ring.vertices[(e + 1) % n];
164 shapes.push(Shape::new(from.point, to.point, from.bulge));
165 edges.push(edge);
166 owner.push((index, e));
167 }
168 }
169 let tree = BoxTree::new(edges.iter().map(|e| e.bounds).collect());
170 let (mut hits, mut stack) = (Vec::new(), Vec::new());
171 let mut found = Vec::new();
172 for i in 0..edges.len() {
173 let grown = edges[i].bounds.grown(clearance);
174 tree.query_into(|b| b.overlaps(&grown), &mut hits, &mut stack);
175 for &j in hits.iter().filter(|&&j| j > i) {
176 let ((ra, ea), (rb, eb)) = (owner[i], owner[j]);
177 let lower_bound = edges[i].bounds.gap(&edges[j].bounds);
178 if lower_bound >= clearance {
179 continue;
180 }
181 let undecided = match decide(&shapes[i], &shapes[j]) {
182 Verdict::Clear => continue,
183 Verdict::Below => false,
184 Verdict::Undecided => true,
185 };
186 found.push(NearPair {
187 edges: [(ra, ea), (rb, eb)],
188 lower_bound,
189 undecided,
190 });
191 }
192 }
193 found
194}
195
196#[derive(Debug, Clone, Copy, PartialEq, Eq)]
197enum Verdict {
198 Below,
200 Clear,
202 Undecided,
204}
205
206type V = (Dyadic, Dyadic);
207
208fn dy(value: f64) -> Dyadic {
209 Dyadic::from_f64(value)
210}
211
212fn point(p: Point2) -> V {
213 (dy(p.x), dy(p.y))
214}
215
216fn sub(a: &V, b: &V) -> V {
217 (a.0.sub(&b.0), a.1.sub(&b.1))
218}
219
220fn dot(a: &V, b: &V) -> Dyadic {
221 a.0.mul(&b.0).add(&a.1.mul(&b.1))
222}
223
224fn cross(a: &V, b: &V) -> Dyadic {
225 a.0.mul(&b.1).sub(&a.1.mul(&b.0))
226}
227
228fn scale(a: &V, by: &Dyadic) -> V {
229 (a.0.mul(by), a.1.mul(by))
230}
231
232fn is_zero(a: &V) -> bool {
233 sign(&a.0) == Sign::Zero && sign(&a.1) == Sign::Zero
234}
235
236fn sign(value: &Dyadic) -> Sign {
238 value.sign().expect("a dyadic has a sign")
239}
240
241fn same_direction(a: &V, b: &V) -> bool {
243 sign(&cross(a, b)) == Sign::Zero && sign(&dot(a, b)) == Sign::Positive
244}
245
246fn at_least_zero(value: &Dyadic) -> bool {
247 sign(value) != Sign::Negative
248}
249
250#[derive(Debug, Clone)]
252struct Shape {
253 from: V,
254 to: V,
255 kind: Kind,
256}
257
258#[derive(Debug, Clone)]
259enum Kind {
260 Segment,
261 Arc(Box<ArcShape>),
262}
263
264#[derive(Debug, Clone)]
269struct ArcShape {
270 ka: Dyadic,
271 ks: Dyadic,
272 k_centre: V,
273 r2: Dyadic,
275 start: V,
277 end: V,
278 major: bool,
279}
280
281impl Shape {
282 fn new(from: Point2, to: Point2, bulge: f64) -> Self {
283 let (f, t) = (point(from), point(to));
284 if bulge == 0.0 {
285 return Self {
286 from: f,
287 to: t,
288 kind: Kind::Segment,
289 };
290 }
291 let b = dy(bulge);
292 let k = b.mul(&dy(4.0));
293 let one_minus = dy(1.0).sub(&b.square());
294 let d = sub(&t, &f);
295 let half = dy(0.5);
296 let mid = (f.0.add(&t.0).mul(&half), f.1.add(&t.1).mul(&half));
297 let k_centre = (
298 k.mul(&mid.0).sub(&one_minus.mul(&d.1)),
299 k.mul(&mid.1).add(&one_minus.mul(&d.0)),
300 );
301 let one_plus = dy(1.0).add(&b.square());
302 let r2 = dot(&d, &d).mul(&one_plus.square());
303 let positive = sign(&k) == Sign::Positive;
304 let (ka, ks) = if positive {
305 (k, dy(1.0))
306 } else {
307 (k.neg(), dy(-1.0))
308 };
309 let mut arc = ArcShape {
310 ka,
311 ks,
312 k_centre,
313 r2,
314 start: (dy(0.0), dy(0.0)),
315 end: (dy(0.0), dy(0.0)),
316 major: sign(&b.square().sub(&dy(1.0))) == Sign::Positive,
317 };
318 let (a, e) = (arc.scaled(&f), arc.scaled(&t));
319 (arc.start, arc.end) = if positive { (a, e) } else { (e, a) };
322 Self {
323 from: f,
324 to: t,
325 kind: Kind::Arc(Box::new(arc)),
326 }
327 }
328
329 fn closer_than(&self, other: &Self, t: f64) -> Verdict {
331 let t = dy(t);
332 let mut undecided = false;
333 let mut hit = |result: Option<bool>| match result {
334 Some(true) => true,
335 Some(false) => false,
336 None => {
337 undecided = true;
338 false
339 }
340 };
341 for end in [&self.from, &self.to] {
343 if hit(other.near_point(end, &t)) {
344 return Verdict::Below;
345 }
346 }
347 for end in [&other.from, &other.to] {
348 if hit(self.near_point(end, &t)) {
349 return Verdict::Below;
350 }
351 }
352 let interior = match (&self.kind, &other.kind) {
354 (Kind::Segment, Kind::Segment) => Some(Dist::Far),
355 (Kind::Segment, Kind::Arc(arc)) => segment_arc(self, arc, &t),
356 (Kind::Arc(arc), Kind::Segment) => segment_arc(other, arc, &t),
357 (Kind::Arc(a), Kind::Arc(b)) => arc_arc(a, b, &t),
358 };
359 if hit(interior.map(|dist| dist != Dist::Far)) {
361 return Verdict::Below;
362 }
363 if self.cusp_with(other) || other.cusp_with(self) {
365 return Verdict::Below;
366 }
367 if undecided {
368 Verdict::Undecided
369 } else {
370 Verdict::Clear
371 }
372 }
373
374 fn cusp_with(&self, other: &Self) -> bool {
377 [&self.from, &self.to].into_iter().any(|end| {
378 other.holds_point(end)
379 && self.leaving(end).iter().any(|mine| {
380 other
381 .leaving(end)
382 .iter()
383 .any(|theirs| same_direction(mine, theirs))
384 })
385 })
386 }
387
388 fn holds_point(&self, x: &V) -> bool {
390 match &self.kind {
391 Kind::Segment => {
392 let d = sub(&self.to, &self.from);
393 let w = sub(x, &self.from);
394 let along = dot(&w, &d);
395 sign(&cross(&d, &w)) == Sign::Zero
396 && at_least_zero(&along)
397 && at_least_zero(&dot(&d, &d).sub(&along))
398 }
399 Kind::Arc(arc) => {
400 let u = arc.scaled(x);
401 dot(&u, &u) == arc.r2 && arc.holds(&u)
402 }
403 }
404 }
405
406 fn leaving(&self, x: &V) -> Vec<V> {
409 let travel = match &self.kind {
410 Kind::Segment => sub(&self.to, &self.from),
411 Kind::Arc(arc) => {
412 let u = arc.scaled(x);
413 if arc.ccw() {
414 (u.1.neg(), u.0)
415 } else {
416 (u.1, u.0.neg())
417 }
418 }
419 };
420 let back = (travel.0.neg(), travel.1.neg());
421 match (*x == self.from, *x == self.to) {
422 (true, _) => vec![travel],
423 (_, true) => vec![back],
424 _ => vec![travel, back],
425 }
426 }
427
428 fn near_point(&self, x: &V, t: &Dyadic) -> Option<bool> {
430 let t2 = t.square();
431 let ends_near = |p: &V| {
432 let w = sub(x, p);
433 let d2 = dot(&w, &w);
435 sign(&d2) != Sign::Zero && sign(&t2.sub(&d2)) == Sign::Positive
436 };
437 match &self.kind {
438 Kind::Segment => {
439 let d = sub(&self.to, &self.from);
440 let w = sub(x, &self.from);
441 let along = dot(&w, &d);
442 let length2 = dot(&d, &d);
443 if !at_least_zero(&along) {
444 return Some(ends_near(&self.from));
445 }
446 if sign(&along.sub(&length2)) != Sign::Negative {
447 return Some(ends_near(&self.to));
448 }
449 let c = cross(&d, &w).square();
452 Some(sign(&c) != Sign::Zero && sign(&t2.mul(&length2).sub(&c)) == Sign::Positive)
453 }
454 Kind::Arc(arc) => {
455 let u = arc.scaled(x);
456 let tk = t.mul(&arc.ka);
457 if is_zero(&u) {
458 return Some(sign(&tk.square().sub(&arc.r2)) == Sign::Positive);
460 }
461 if arc.holds(&u) {
462 return abs_below(
465 &dy(0.0),
466 &[(dy(1.0), dot(&u, &u)), (dy(-1.0), arc.r2.clone())],
467 &(tk, None),
468 )
469 .map(|dist| dist == Dist::Near);
470 }
471 Some(ends_near(&self.from) || ends_near(&self.to))
472 }
473 }
474 }
475}
476
477impl ArcShape {
478 fn ccw(&self) -> bool {
480 sign(&self.ks) == Sign::Positive
481 }
482
483 fn holds_open(&self, v: &V) -> bool {
486 self.holds(v) && !same_direction(v, &self.start) && !same_direction(v, &self.end)
487 }
488
489 fn scaled(&self, y: &V) -> V {
491 sub(&scale(y, &self.ka), &scale(&self.k_centre, &self.ks))
492 }
493
494 fn holds(&self, v: &V) -> bool {
497 let (a, e) = (&self.start, &self.end);
498 if self.major {
499 !(sign(&cross(e, v)) == Sign::Positive && sign(&cross(v, a)) == Sign::Positive)
501 } else {
502 at_least_zero(&cross(a, v)) && at_least_zero(&cross(v, e))
503 }
504 }
505}
506
507#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
509enum Dist {
510 Far,
512 Near,
514 Zero,
516}
517
518fn abs_below(
522 p: &Dyadic,
523 roots: &[(Dyadic, Dyadic)],
524 tau: &(Dyadic, Option<Dyadic>),
525) -> Option<Dist> {
526 let mut tower = Tower::<Dyadic>::new();
527 let mut sum = tower.value(p.clone());
528 for (q, c) in roots {
529 let radicand = tower.value(c.clone());
530 let root = tower.sqrt(&radicand).ok()?;
531 sum = tower.add(&sum, &tower.mul(&tower.value(q.clone()), &root));
532 }
533 let mut bound = tower.value(tau.0.clone());
534 if let Some(e) = &tau.1 {
535 let radicand = tower.value(e.clone());
536 let root = tower.sqrt(&radicand).ok()?;
537 bound = tower.mul(&bound, &root);
538 }
539 let below = tower.sign(&tower.sub(&sum, &bound))?;
540 let above = tower.sign(&tower.add(&sum, &bound))?;
541 if tower.sign(&sum)? == Sign::Zero {
542 return Some(Dist::Zero);
543 }
544 let near = below == Sign::Negative && above == Sign::Positive;
545 Some(if near { Dist::Near } else { Dist::Far })
546}
547
548fn segment_arc(segment: &Shape, arc: &ArcShape, t: &Dyadic) -> Option<Dist> {
550 let d = sub(&segment.to, &segment.from);
551 let length2 = dot(&d, &d);
552 if sign(&length2) == Sign::Zero {
553 return Some(Dist::Far);
554 }
555 let c = sub(
557 &scale(&arc.k_centre, &arc.ks),
558 &scale(&segment.from, &arc.ka),
559 );
560 let along = dot(&c, &d);
561 if !at_least_zero(&along) || !at_least_zero(&arc.ka.mul(&length2).sub(&along)) {
562 return Some(Dist::Far);
563 }
564 let height = cross(&d, &c);
565 let normal = (d.1.neg(), d.0.clone());
566 let tk = t.mul(&arc.ka);
567 let inside =
568 sign(&along) == Sign::Positive && sign(&arc.ka.mul(&length2).sub(&along)) == Sign::Positive;
569 let mut best = Dist::Far;
570 let mut undecided = false;
571 for sigma in [dy(1.0), dy(-1.0)] {
572 let direction = scale(&normal, &sigma);
573 if !arc.holds(&direction) {
574 continue;
575 }
576 let interior = inside && arc.holds_open(&direction);
579 let roots = [(sigma, arc.r2.mul(&length2))];
582 match abs_below(&height, &roots, &(tk.clone(), Some(length2.clone()))) {
583 Some(Dist::Zero) if !interior => {}
584 Some(dist) => best = best.max(dist),
585 None => undecided = true,
586 }
587 }
588 decided_unless(best, undecided)
589}
590
591fn decided_unless(best: Dist, undecided: bool) -> Option<Dist> {
594 if best == Dist::Far && undecided {
595 None
596 } else {
597 Some(best)
598 }
599}
600
601fn arc_arc(a: &ArcShape, b: &ArcShape, t: &Dyadic) -> Option<Dist> {
603 let w = sub(
605 &scale(&b.k_centre, &a.ka.mul(&b.ks)),
606 &scale(&a.k_centre, &b.ka.mul(&a.ks)),
607 );
608 if is_zero(&w) {
609 return Some(Dist::Far);
610 }
611 let w2 = dot(&w, &w);
612 let tk = t.mul(&a.ka).mul(&b.ka);
613 let mut best = Dist::Far;
614 let mut undecided = false;
615 for sa in [dy(1.0), dy(-1.0)] {
616 let da = scale(&w, &sa);
617 if !a.holds(&da) {
618 continue;
619 }
620 for sb in [dy(1.0), dy(-1.0)] {
621 let db = scale(&w, &sb);
622 if !b.holds(&db) {
623 continue;
624 }
625 let interior = a.holds_open(&da) && b.holds_open(&db);
626 let roots = [
628 (dy(1.0), w2.clone()),
629 (sb, a.ka.square().mul(&b.r2)),
630 (sa.neg(), b.ka.square().mul(&a.r2)),
631 ];
632 match abs_below(&dy(0.0), &roots, &(tk.clone(), None)) {
633 Some(Dist::Zero) if !interior => {}
634 Some(dist) => best = best.max(dist),
635 None => undecided = true,
636 }
637 }
638 }
639 decided_unless(best, undecided)
640}
641
642#[cfg(test)]
643mod tests {
644 use super::*;
645
646 fn p(x: f64, y: f64) -> Point2 {
647 Point2::new(x, y)
648 }
649
650 #[test]
651 fn a_scaled_arc_end_lies_on_the_scaled_circle() {
652 for bulge in [0.5, -0.5, 1.0, 2.0, -3.0, 0.1] {
653 let shape = Shape::new(p(0.0, 0.0), p(2.0, 1.0), bulge);
654 let Kind::Arc(arc) = &shape.kind else {
655 panic!("an arc");
656 };
657 assert_eq!(dot(&arc.start, &arc.start), arc.r2, "bulge {bulge}");
658 assert_eq!(dot(&arc.end, &arc.end), arc.r2, "bulge {bulge}");
659 }
660 }
661
662 #[test]
663 fn a_negative_radicand_is_undecided_not_a_guess() {
664 let result = abs_below(&dy(0.0), &[(dy(1.0), dy(-1.0))], &(dy(1.0), None));
665 assert_eq!(result, None);
666 }
667
668 fn square_and_hole(gap: f64) -> Vec<ArcRing> {
669 let ring = |x0: f64, y0: f64, x1: f64, y1: f64| {
670 ArcRing::from_points(&[p(x0, y0), p(x1, y0), p(x1, y1), p(x0, y1)])
671 };
672 vec![ring(0.0, 0.0, 10.0, 10.0), ring(gap, 1.0, 5.0, 4.0)]
673 }
674
675 #[test]
676 fn an_undecided_pair_is_reported_as_undecided_and_a_clear_one_is_not() {
677 let rings = square_and_hole(0.0005);
678 let rings: Vec<&ArcRing> = rings.iter().collect();
679 let run = |verdict: Verdict| near_pairs_by(&rings, &[true, true], 0.001, |_, _| verdict);
680 let undecided = run(Verdict::Undecided);
682 assert!(!undecided.is_empty());
683 assert!(undecided.iter().all(|pair| pair.undecided));
684 let below = run(Verdict::Below);
685 assert_eq!(below.len(), undecided.len());
686 assert!(below.iter().all(|pair| !pair.undecided));
687 assert!(run(Verdict::Clear).is_empty());
688 }
689
690 #[test]
691 fn a_ring_that_is_not_usable_is_left_out() {
692 let rings = square_and_hole(0.0005);
693 let rings: Vec<&ArcRing> = rings.iter().collect();
694 let found = near_pairs_by(&rings, &[true, false], 0.001, |_, _| Verdict::Below);
695 assert!(!found.is_empty());
696 assert!(found
697 .iter()
698 .all(|pair| pair.edges.iter().all(|edge| edge.0 == 0)));
699 }
700
701 #[test]
702 fn parallel_segments_are_below_only_within_their_distance() {
703 let a = Shape::new(p(0.0, 0.0), p(1.0, 0.0), 0.0);
704 let b = Shape::new(p(0.0, 1.0), p(1.0, 1.0), 0.0);
705 assert_eq!(a.closer_than(&b, 2.0), Verdict::Below);
706 assert_eq!(a.closer_than(&b, 0.5), Verdict::Clear);
707 }
708}