1use std::cmp::Reverse;
198use std::collections::BinaryHeap;
199
200use axiolid_brep::ExactBRep;
201use axiolid_core::{Point2, Point3, Scalar, Tolerance, Vec3};
202use axiolid_curve::Curve3;
203use axiolid_evaluate::evaluate3;
204use axiolid_evaluate::surface::{evaluate, normal};
205use axiolid_surface::Surface;
206
207use crate::exact::ExactMeasureError;
208use crate::exact_domain::Domain;
209
210mod touch;
211pub(crate) mod witness;
212
213const MAX_STEPS: usize = 400_000;
215
216pub(crate) const OVERLAP_STEPS: usize = 20_000;
218
219pub(crate) const POINT_EPSILONS: Scalar = 16.0;
225pub(crate) const PROJECTION_EPSILONS: Scalar = 8.0;
227pub(crate) const RATIO_EPSILONS: Scalar = 8.0;
230pub(crate) const FLOOR_EPSILONS: Scalar = 128.0;
232
233#[derive(Debug, Clone, PartialEq)]
241pub struct DistanceBounds {
242 pub lower: Scalar,
244 pub upper: Scalar,
247 pub point_a: Point3,
249 pub point_b: Point3,
251}
252
253#[derive(Debug, Clone, Copy, PartialEq, Eq)]
255pub enum Clearance {
256 Below,
259 Above,
261 Indeterminate,
263}
264
265impl DistanceBounds {
266 #[must_use]
268 pub fn against(&self, limit: Scalar) -> Clearance {
269 if self.upper < limit {
270 Clearance::Below
271 } else if self.lower > limit {
272 Clearance::Above
273 } else {
274 Clearance::Indeterminate
275 }
276 }
277}
278
279pub fn boundary_distance(
291 a: &ExactBRep,
292 b: &ExactBRep,
293 accuracy: Scalar,
294 tolerance: Tolerance,
295) -> Result<DistanceBounds, ExactMeasureError> {
296 let accuracy = accuracy.max(0.0);
297 Ok(search(
298 a,
299 b,
300 tolerance,
301 Metric::Space,
302 &mut |lower, upper, floor| closed(lower, upper, accuracy, floor),
303 )?
304 .bounds)
305}
306
307pub fn plan_boundary_distance(
325 a: &ExactBRep,
326 b: &ExactBRep,
327 accuracy: Scalar,
328 tolerance: Tolerance,
329) -> Result<DistanceBounds, ExactMeasureError> {
330 let accuracy = accuracy.max(0.0);
331 Ok(
332 search(a, b, tolerance, Metric::Plan, &mut |lower, upper, floor| {
333 closed(lower, upper, accuracy, floor)
334 })?
335 .bounds,
336 )
337}
338
339pub fn plan_boundary_clearance(
345 a: &ExactBRep,
346 b: &ExactBRep,
347 limit: Scalar,
348 tolerance: Tolerance,
349) -> Result<(DistanceBounds, Clearance), ExactMeasureError> {
350 let bounds = search(a, b, tolerance, Metric::Plan, &mut |lower, upper, floor| {
351 upper < limit || lower > limit || closed(lower, upper, 0.0, floor)
352 })?
353 .bounds;
354 let clearance = bounds.against(limit);
355 Ok((bounds, clearance))
356}
357
358#[derive(Debug, Clone, Copy, PartialEq)]
360pub enum PlanOverlap {
361 Overlapping {
364 at: Point2,
366 },
367 Disjoint {
369 gap: Scalar,
371 },
372 Undecided,
375}
376
377pub fn plan_overlap(
389 a: &ExactBRep,
390 b: &ExactBRep,
391 tolerance: Tolerance,
392) -> Result<PlanOverlap, ExactMeasureError> {
393 Ok(overlap_search(a, b, tolerance)?.0)
394}
395
396pub(crate) type Pair = (Shape, Shape);
398
399pub(crate) fn overlap_search(
408 a: &ExactBRep,
409 b: &ExactBRep,
410 tolerance: Tolerance,
411) -> Result<(PlanOverlap, Option<Pair>), ExactMeasureError> {
412 let found = search_within(
415 a,
416 b,
417 tolerance,
418 Metric::Plan,
419 OVERLAP_STEPS,
420 &mut |lower, _, _| lower > 0.0,
421 )?;
422 if let Some(at) = found.overlap {
423 return Ok((PlanOverlap::Overlapping { at }, Some(found.on)));
424 }
425 if found.bounds.lower > 0.0 {
426 return Ok((
427 PlanOverlap::Disjoint {
428 gap: found.bounds.lower,
429 },
430 None,
431 ));
432 }
433 Ok(match level_overlap(a, b, tolerance, OVERLAP_STEPS)? {
434 Some((at, on)) => (PlanOverlap::Overlapping { at }, Some(on)),
435 None => (PlanOverlap::Undecided, None),
436 })
437}
438
439fn level_overlap(
443 a: &ExactBRep,
444 b: &ExactBRep,
445 tolerance: Tolerance,
446 max_steps: usize,
447) -> Result<Option<(Point2, Pair)>, ExactMeasureError> {
448 let linear = tolerance.linear().max(1e-12);
449 let side_a = Side::new(a, linear, Metric::Plan)?;
450 let side_b = Side::new(b, linear, Metric::Plan)?;
451 let level = |side: &Side<'_>| -> Result<Vec<Element>, ExactMeasureError> {
452 let mut out = Vec::new();
453 for element in &side.elements {
454 if let Shape::Face { face, .. } = element.shape {
455 if level_frame(side, face)?.is_some() {
456 out.push(*element);
457 }
458 }
459 }
460 Ok(out)
461 };
462 let mut elements_a = level(&side_a)?;
463 let mut elements_b = level(&side_b)?;
464 let mut heap = BinaryHeap::new();
466 let push = |heap: &mut BinaryHeap<(Key, usize, usize)>,
467 ea: &Element,
468 eb: &Element,
469 i: usize,
470 j: usize|
471 -> Result<(), ExactMeasureError> {
472 if lower_bound(&side_a, ea, &side_b, eb)? <= 0.0 {
473 heap.push((Key(ea.plan_radius + eb.plan_radius), i, j));
474 }
475 Ok(())
476 };
477 for (i, ea) in elements_a.iter().enumerate() {
478 for (j, eb) in elements_b.iter().enumerate() {
479 push(&mut heap, ea, eb, i, j)?;
480 }
481 }
482 let mut steps = 0;
483 while let Some((_, i, j)) = heap.pop() {
484 let (ea, eb) = (elements_a[i], elements_b[j]);
485 if let Some((at, _, _)) = plan_patches_overlap(&side_a, &ea, &side_b, &eb)? {
486 return Ok(Some((at, (ea.shape, eb.shape))));
487 }
488 steps += 1;
489 if steps > max_steps {
490 break;
491 }
492 let split_a = side_a.size(&ea) >= side_b.size(&eb);
493 let (side, parent) = if split_a {
494 (&side_a, ea)
495 } else {
496 (&side_b, eb)
497 };
498 let children = side.split(&parent)?;
499 if children
501 .iter()
502 .any(|child| side.size(child) >= side.size(&parent))
503 {
504 continue;
505 }
506 for child in children {
507 if split_a {
508 elements_a.push(child);
509 push(&mut heap, &child, &eb, elements_a.len() - 1, j)?;
510 } else {
511 elements_b.push(child);
512 push(&mut heap, &ea, &child, i, elements_b.len() - 1)?;
513 }
514 }
515 }
516 Ok(None)
517}
518
519fn level_frame(
522 side: &Side<'_>,
523 face: usize,
524) -> Result<Option<axiolid_core::Frame3>, ExactMeasureError> {
525 let surface = surface_of(side.brep, side.brep.topology().faces()[face].surface)?;
526 let Surface::Plane(plane) = surface else {
527 return Ok(None);
528 };
529 let f = plane.frame;
530 let det = f.x.x * f.y.y - f.x.y * f.y.x;
531 let scale = f.x.length() * f.y.length();
532 if det.abs().is_nan() || det.abs() <= 1e-9 * scale {
534 return Ok(None);
535 }
536 Ok(Some(f))
537}
538
539pub fn boundary_clearance(
549 a: &ExactBRep,
550 b: &ExactBRep,
551 limit: Scalar,
552 tolerance: Tolerance,
553) -> Result<(DistanceBounds, Clearance), ExactMeasureError> {
554 let bounds = search(
555 a,
556 b,
557 tolerance,
558 Metric::Space,
559 &mut |lower, upper, floor| {
560 upper < limit || lower > limit || closed(lower, upper, 0.0, floor)
561 },
562 )?
563 .bounds;
564 let clearance = bounds.against(limit);
565 Ok((bounds, clearance))
566}
567
568#[derive(Debug, Clone, Copy, PartialEq, Eq)]
570pub(crate) enum Metric {
571 Space,
573 Plan,
576}
577
578fn flat(v: Vec3) -> Vec3 {
580 Vec3::new(v.x, v.y, 0.0)
581}
582
583fn flat_length(v: Vec3) -> Scalar {
585 v.x.hypot(v.y)
586}
587
588#[derive(Debug, Clone, Copy)]
590pub(crate) enum Shape {
591 Face {
594 face: usize,
595 lo: Point2,
596 hi: Point2,
597 inside: bool,
598 },
599 Edge { edge: usize, t0: Scalar, t1: Scalar },
601}
602
603#[derive(Debug, Clone, Copy)]
605pub(crate) struct Element {
606 pub(crate) shape: Shape,
607 pub(crate) centre: Point3,
608 pub(crate) radius: Scalar,
609 pub(crate) plan_radius: Scalar,
612 pub(crate) witness: Option<Point3>,
614 pub(crate) normal: Option<Vec3>,
616 pub(crate) spread: Option<Scalar>,
620}
621
622pub(crate) struct Side<'a> {
624 pub(crate) brep: &'a ExactBRep,
625 metric: Metric,
626 domains: Vec<Option<Domain<'a>>>,
627 edges_bounded: Vec<bool>,
630 pub(crate) elements: Vec<Element>,
631 pub(crate) scale: Scalar,
635 pub(crate) witness_scale: Scalar,
637}
638
639impl<'a> Side<'a> {
640 pub(crate) fn new(
641 brep: &'a ExactBRep,
642 linear: Scalar,
643 metric: Metric,
644 ) -> Result<Self, ExactMeasureError> {
645 let topology = brep.topology();
646 let mut side = Side {
647 brep,
648 metric,
649 domains: Vec::with_capacity(topology.faces().len()),
650 edges_bounded: Vec::with_capacity(topology.faces().len()),
651 elements: Vec::new(),
652 scale: 0.0,
653 witness_scale: 0.0,
654 };
655 for face in topology.faces() {
656 let mut bounded = true;
657 for bound in &face.bounds {
658 let wire = topology
659 .loops()
660 .get(bound.loop_id.index())
661 .ok_or(ExactMeasureError::DanglingReference)?;
662 for use_ in &wire.edges {
663 let curve = topology.edges()[use_.edge.index()]
664 .curve
665 .and_then(|id| brep.curves3().get(id.index()));
666 bounded &= matches!(
667 curve,
668 Some(Curve3::Line(_) | Curve3::Circle(_) | Curve3::Ellipse(_))
669 );
670 }
671 }
672 side.edges_bounded.push(bounded);
673 }
674 for face in topology.faces() {
675 let surface = surface_of(brep, face.surface)?;
676 side.domains.push(Domain::new(brep, face, surface, linear)?);
677 }
678 let mut roots = Vec::with_capacity(topology.faces().len());
681 for (index, face) in topology.faces().iter().enumerate() {
682 let surface = surface_of(brep, face.surface)?;
683 let (lo, hi) = match &side.domains[index] {
684 Some(domain) => (domain.min, domain.max),
685 None => natural_range(surface).ok_or(ExactMeasureError::NonPlanarFace(
686 "an unbounded face trimmed by a pcurve family the distance query cannot bound",
687 ))?,
688 };
689 side.scale = side.scale.max(patch_scale(surface, lo, hi)?);
690 side.witness_scale = side.witness_scale.max(witness_scale(surface, lo, hi)?);
691 roots.push((index, lo, hi));
692 }
693 let mut spans = Vec::new();
694 for (index, edge) in topology.edges().iter().enumerate() {
695 let Some(curve) = edge.curve.and_then(|id| brep.curves3().get(id.index())) else {
696 continue;
697 };
698 let Some(span) = topology
699 .edge_id_at(index)
700 .and_then(|id| brep.edge_interval(id))
701 else {
702 continue;
703 };
704 if let Some(scale) = span_scale(curve, span.start, span.end) {
705 side.scale = side.scale.max(scale);
706 side.witness_scale = side.witness_scale.max(scale);
707 }
708 spans.push((curve, index, span));
709 }
710 if !(side.scale.is_finite() && side.witness_scale.is_finite()) {
711 return Err(crate::exact::EVALUATION);
712 }
713 for (index, lo, hi) in roots {
714 if let Some(element) = side.face_element(index, lo, hi, false)? {
715 side.elements.push(element);
716 }
717 }
718 for (curve, index, span) in spans {
719 if let Some(element) = edge_element(curve, index, span.start, span.end, side.scale)? {
720 side.elements.push(element);
721 }
722 }
723 if side.elements.is_empty() {
724 return Err(ExactMeasureError::Degenerate);
725 }
726 Ok(side)
727 }
728
729 fn face_element(
731 &self,
732 face: usize,
733 lo: Point2,
734 hi: Point2,
735 inside: bool,
736 ) -> Result<Option<Element>, ExactMeasureError> {
737 let topology = self.brep.topology();
738 let surface = surface_of(self.brep, topology.faces()[face].surface)?;
739 let mid = (lo + hi) * 0.5;
740 let mut inside = inside;
741 if !inside {
742 if let Some(domain) = &self.domains[face] {
743 if !domain.touches(lo, hi)? {
744 match domain.contains(mid)? {
745 Some(false) => return Ok(None),
746 Some(true) => inside = true,
747 None => {}
748 }
749 }
750 }
751 }
752 let (centre, radius) = patch_sphere(surface, lo, hi, self.scale)?;
753 let plan_radius = patch_plan_radius(surface, lo, hi, centre, radius, self.scale)?;
754 let witness = if inside {
755 Some(point_on(surface, mid)?)
756 } else {
757 None
758 };
759 let normal = normal(surface, mid.x, mid.y).ok();
760 let spread = if self.edges_bounded[face] {
761 normal_spread(surface, lo, hi)
762 } else {
763 None
764 };
765 Ok(Some(Element {
766 normal,
767 spread,
768 shape: Shape::Face {
769 face,
770 lo,
771 hi,
772 inside,
773 },
774 centre,
775 radius,
776 plan_radius,
777 witness,
778 }))
779 }
780
781 pub(crate) fn witness_error(&self) -> Scalar {
783 POINT_EPSILONS * Scalar::EPSILON * self.witness_scale
784 }
785
786 pub(crate) fn witness_excess(&self) -> Scalar {
789 POINT_EPSILONS * Scalar::EPSILON * (self.witness_scale - self.scale)
790 }
791
792 pub(crate) fn size(&self, element: &Element) -> Scalar {
794 match self.metric {
795 Metric::Space => element.radius,
796 Metric::Plan => element.plan_radius,
797 }
798 }
799
800 pub(crate) fn split(&self, element: &Element) -> Result<Vec<Element>, ExactMeasureError> {
804 match element.shape {
805 Shape::Face {
806 face,
807 lo,
808 hi,
809 inside,
810 } => {
811 let surface = surface_of(self.brep, self.brep.topology().faces()[face].surface)?;
812 let (lu, lv) = match self.metric {
813 Metric::Space => lipschitz(surface, lo, hi)?,
814 Metric::Plan => plan_lipschitz(surface, lo, hi)?,
815 };
816 let mid = (lo + hi) * 0.5;
817 let halves = if (hi.x - lo.x).abs() * lu >= (hi.y - lo.y).abs() * lv {
818 [
819 (lo, Point2::new(mid.x, hi.y)),
820 (Point2::new(mid.x, lo.y), hi),
821 ]
822 } else {
823 [
824 (lo, Point2::new(hi.x, mid.y)),
825 (Point2::new(lo.x, mid.y), hi),
826 ]
827 };
828 let mut out = Vec::with_capacity(2);
829 for (lo, hi) in halves {
830 if let Some(child) = self.face_element(face, lo, hi, inside)? {
831 out.push(child);
832 }
833 }
834 Ok(out)
835 }
836 Shape::Edge { edge, t0, t1 } => {
837 let curve = self.brep.topology().edges()[edge]
838 .curve
839 .and_then(|id| self.brep.curves3().get(id.index()))
840 .ok_or(ExactMeasureError::DanglingReference)?;
841 let tm = 0.5 * (t0 + t1);
842 let mut out = Vec::with_capacity(2);
843 for (a, b) in [(t0, tm), (tm, t1)] {
844 if let Some(child) = edge_element(curve, edge, a, b, self.scale)? {
845 out.push(child);
846 }
847 }
848 Ok(out)
849 }
850 }
851 }
852}
853
854pub(crate) fn surface_of(
855 brep: &ExactBRep,
856 id: Option<axiolid_brep::SurfaceId>,
857) -> Result<&Surface, ExactMeasureError> {
858 let id = id.ok_or(ExactMeasureError::MissingSurface)?;
859 brep.surfaces()
860 .get(id.index())
861 .ok_or(ExactMeasureError::DanglingReference)
862}
863
864fn point_on(surface: &Surface, at: Point2) -> Result<Point3, ExactMeasureError> {
865 evaluate(surface, at.x, at.y).map_err(|_| crate::exact::EVALUATION)
866}
867
868fn natural_range(surface: &Surface) -> Option<(Point2, Point2)> {
871 use core::f64::consts::{FRAC_PI_2, TAU};
872 match surface {
873 Surface::Sphere(_) => Some((Point2::new(0.0, -FRAC_PI_2), Point2::new(TAU, FRAC_PI_2))),
874 Surface::Torus(_) => Some((Point2::ZERO, Point2::new(TAU, TAU))),
875 _ => None,
876 }
877}
878
879fn frame_scale(frame: &axiolid_core::Frame3) -> Scalar {
881 frame.x.length().max(frame.y.length()).max(frame.z.length())
882}
883
884fn lipschitz(
886 surface: &Surface,
887 lo: Point2,
888 hi: Point2,
889) -> Result<(Scalar, Scalar), ExactMeasureError> {
890 let bounds = match surface {
891 Surface::Plane(p) => (p.frame.x.length(), p.frame.y.length()),
892 Surface::Cylinder(c) => (c.radius.abs() * frame_scale(&c.frame), c.frame.z.length()),
893 Surface::EllipticalCylinder(c) => (
894 c.semi_axis_x.abs().max(c.semi_axis_y.abs()) * frame_scale(&c.frame),
895 c.frame.z.length(),
896 ),
897 Surface::Cone(c) => {
898 let slope = c.semi_angle.tan();
899 let radius = (c.radius + lo.y * slope)
900 .abs()
901 .max((c.radius + hi.y * slope).abs());
902 let scale = frame_scale(&c.frame);
903 (radius * scale, scale * (1.0 + slope * slope).sqrt())
904 }
905 Surface::Sphere(s) => {
906 let r = s.radius.abs() * frame_scale(&s.frame);
909 (r * max_cos(lo.y, hi.y), r)
910 }
911 Surface::Torus(t) => {
912 let scale = frame_scale(&t.frame);
913 (
914 (t.major_radius.abs() + t.minor_radius.abs() * max_cos(lo.y, hi.y)) * scale,
915 t.minor_radius.abs() * scale,
916 )
917 }
918 Surface::BSpline(_) => (0.0, 0.0),
920 _ => {
921 return Err(ExactMeasureError::NonPlanarFace(crate::exact::family(
922 surface,
923 )))
924 }
925 };
926 if bounds.0.is_finite() && bounds.1.is_finite() {
927 Ok(bounds)
928 } else {
929 Err(crate::exact::EVALUATION)
930 }
931}
932
933fn plan_lipschitz(
936 surface: &Surface,
937 lo: Point2,
938 hi: Point2,
939) -> Result<(Scalar, Scalar), ExactMeasureError> {
940 let full = lipschitz(surface, lo, hi)?;
941 let turning = |frame: &axiolid_core::Frame3, a: Scalar, b: Scalar| {
943 (a * flat_length(frame.x)).hypot(b * flat_length(frame.y))
944 };
945 let bounds = match surface {
946 Surface::Plane(p) => (flat_length(p.frame.x), flat_length(p.frame.y)),
947 Surface::Cylinder(c) => (
948 turning(&c.frame, c.radius.abs(), c.radius.abs()),
949 flat_length(c.frame.z),
950 ),
951 Surface::EllipticalCylinder(c) => (
952 turning(&c.frame, c.semi_axis_x.abs(), c.semi_axis_y.abs()),
953 flat_length(c.frame.z),
954 ),
955 Surface::Cone(c) => {
956 let slope = c.semi_angle.tan();
957 let radius = (c.radius + lo.y * slope)
958 .abs()
959 .max((c.radius + hi.y * slope).abs());
960 (
961 turning(&c.frame, radius, radius),
962 turning(&c.frame, slope.abs(), slope.abs()) + flat_length(c.frame.z),
963 )
964 }
965 _ => return Ok(full),
966 };
967 if bounds.0.is_finite() && bounds.1.is_finite() {
968 Ok((bounds.0.min(full.0), bounds.1.min(full.1)))
969 } else {
970 Ok(full)
971 }
972}
973
974fn patch_plan_radius(
978 surface: &Surface,
979 lo: Point2,
980 hi: Point2,
981 centre: Point3,
982 radius: Scalar,
983 scale: Scalar,
984) -> Result<Scalar, ExactMeasureError> {
985 if let Surface::BSpline(spline) = surface {
986 let reach = spline
987 .control_points
988 .iter()
989 .flatten()
990 .map(|p| flat_length(*p - centre))
991 .fold(0.0, Scalar::max);
992 return Ok(pad(reach, scale).min(radius));
993 }
994 let (lu, lv) = plan_lipschitz(surface, lo, hi)?;
995 let plan = 0.5 * ((hi.x - lo.x).abs() * lu + (hi.y - lo.y).abs() * lv);
996 Ok(pad(plan, scale).min(radius))
997}
998
999fn max_cos(a: Scalar, b: Scalar) -> Scalar {
1001 let (a, b) = (a.min(b), a.max(b));
1002 let k = (a / core::f64::consts::PI).ceil();
1003 if k * core::f64::consts::PI <= b {
1004 1.0
1005 } else {
1006 a.cos().abs().max(b.cos().abs())
1007 }
1008}
1009
1010fn patch_sphere(
1017 surface: &Surface,
1018 lo: Point2,
1019 hi: Point2,
1020 scale: Scalar,
1021) -> Result<(Point3, Scalar), ExactMeasureError> {
1022 if let Surface::BSpline(spline) = surface {
1023 if let Some(weights) = &spline.weights {
1024 if weights.iter().flatten().any(|w| w.is_nan() || *w <= 0.0) {
1025 return Err(ExactMeasureError::NonPlanarFace(
1026 "non-positive-weight B-spline",
1027 ));
1028 }
1029 }
1030 let points: Vec<Point3> = spline.control_points.iter().flatten().copied().collect();
1031 if points.is_empty() {
1032 return Err(ExactMeasureError::Degenerate);
1033 }
1034 let (mut min, mut max) = (points[0], points[0]);
1035 for p in &points {
1036 min = min.min(*p);
1037 max = max.max(*p);
1038 }
1039 let centre = (min + max) * 0.5;
1040 let radius = points
1041 .iter()
1042 .map(|p| (*p - centre).length())
1043 .fold(0.0, Scalar::max);
1044 return Ok((centre, pad(radius, scale)));
1045 }
1046 let centre = point_on(surface, (lo + hi) * 0.5)?;
1047 let (lu, lv) = lipschitz(surface, lo, hi)?;
1048 let radius = 0.5 * ((hi.x - lo.x).abs() * lu + (hi.y - lo.y).abs() * lv);
1049 Ok((centre, pad(radius, scale)))
1050}
1051
1052pub(crate) fn pad(radius: Scalar, scale: Scalar) -> Scalar {
1057 radius * (1.0 + RATIO_EPSILONS * Scalar::EPSILON)
1058 + POINT_EPSILONS * Scalar::EPSILON * scale
1059 + Scalar::MIN_POSITIVE
1060}
1061
1062pub(crate) fn floor(scale: Scalar, upper: Scalar) -> Scalar {
1067 FLOOR_EPSILONS * Scalar::EPSILON * scale + RATIO_EPSILONS * Scalar::EPSILON * upper
1068}
1069
1070pub(crate) fn closed(lower: Scalar, upper: Scalar, accuracy: Scalar, floor: Scalar) -> bool {
1074 upper - lower <= accuracy.max(floor)
1075}
1076
1077fn patch_scale(surface: &Surface, lo: Point2, hi: Point2) -> Result<Scalar, ExactMeasureError> {
1081 if let Surface::BSpline(spline) = surface {
1082 let scale = spline
1083 .control_points
1084 .iter()
1085 .flatten()
1086 .map(|p| p.length())
1087 .fold(0.0, Scalar::max);
1088 return if scale.is_finite() {
1089 Ok(scale)
1090 } else {
1091 Err(crate::exact::EVALUATION)
1092 };
1093 }
1094 let (lu, lv) = lipschitz(surface, lo, hi)?;
1095 let params = lo.x.abs().max(hi.x.abs()) * lu + lo.y.abs().max(hi.y.abs()) * lv;
1096 let scale = match surface {
1097 Surface::Plane(p) => p.frame.origin.length() + params,
1098 Surface::Cylinder(c) => {
1099 c.frame.origin.length() + 2.0 * c.radius.abs() * frame_scale(&c.frame) + params
1100 }
1101 Surface::EllipticalCylinder(c) => {
1102 c.frame.origin.length()
1103 + (c.semi_axis_x.abs() + c.semi_axis_y.abs()) * frame_scale(&c.frame)
1104 + params
1105 }
1106 Surface::Cone(c) => {
1107 let reach = lo.y.abs().max(hi.y.abs());
1108 c.frame.origin.length()
1109 + 2.0 * (c.radius.abs() + reach * c.semi_angle.tan().abs()) * frame_scale(&c.frame)
1110 + params
1111 }
1112 Surface::Sphere(s) => {
1113 s.frame.origin.length() + 3.0 * s.radius.abs() * frame_scale(&s.frame) + params
1114 }
1115 Surface::Torus(t) => {
1116 t.frame.origin.length()
1117 + (2.0 * t.major_radius.abs() + 3.0 * t.minor_radius.abs()) * frame_scale(&t.frame)
1118 + params
1119 }
1120 _ => {
1121 return Err(ExactMeasureError::NonPlanarFace(crate::exact::family(
1122 surface,
1123 )))
1124 }
1125 };
1126 if scale.is_finite() {
1127 Ok(scale)
1128 } else {
1129 Err(crate::exact::EVALUATION)
1130 }
1131}
1132
1133fn witness_scale(surface: &Surface, lo: Point2, hi: Point2) -> Result<Scalar, ExactMeasureError> {
1136 let Surface::BSpline(spline) = surface else {
1137 return patch_scale(surface, lo, hi);
1138 };
1139 let largest = spline
1140 .control_points
1141 .iter()
1142 .flatten()
1143 .map(|p| p.length())
1144 .fold(0.0, Scalar::max);
1145 let ratio = match &spline.weights {
1146 None => 1.0,
1147 Some(weights) => {
1148 let (mut lo, mut hi) = (Scalar::INFINITY, 0.0_f64);
1149 for w in weights.iter().flatten() {
1150 if w.is_nan() || *w <= 0.0 {
1151 return Err(ExactMeasureError::NonPlanarFace(
1152 "non-positive-weight B-spline",
1153 ));
1154 }
1155 lo = lo.min(*w);
1156 hi = hi.max(*w);
1157 }
1158 hi / lo
1159 }
1160 };
1161 let degrees = Scalar::from(spline.u_degree) + Scalar::from(spline.v_degree);
1162 if ratio > 1.0 / (64.0 * degrees.max(1.0) * Scalar::EPSILON) {
1164 return Err(ExactMeasureError::NonPlanarFace(
1165 "extreme weight ratio B-spline",
1166 ));
1167 }
1168 let levels = degrees + 1.0;
1169 let scale = levels * ratio * largest;
1170 if scale.is_finite() {
1171 Ok(scale)
1172 } else {
1173 Err(crate::exact::EVALUATION)
1174 }
1175}
1176
1177pub(crate) fn witnessed_upper(distance: Scalar, error: Scalar) -> Scalar {
1179 distance * (1.0 + RATIO_EPSILONS * Scalar::EPSILON) + error
1180}
1181
1182fn span_scale(curve: &Curve3, t0: Scalar, t1: Scalar) -> Option<Scalar> {
1184 let t = t0.abs().max(t1.abs());
1185 match curve {
1186 Curve3::Line(line) => Some(line.origin.length() + t * line.direction.length()),
1187 Curve3::Circle(c) => {
1188 Some(c.frame.origin.length() + (2.0 + t) * c.radius.abs() * frame_scale(&c.frame))
1189 }
1190 Curve3::Ellipse(e) => {
1191 let (a, b) = (e.semi_axis_x.abs(), e.semi_axis_y.abs());
1192 Some(e.frame.origin.length() + (a + b + t * a.max(b)) * frame_scale(&e.frame))
1193 }
1194 _ => None,
1195 }
1196}
1197
1198fn edge_element(
1201 curve: &Curve3,
1202 edge: usize,
1203 t0: Scalar,
1204 t1: Scalar,
1205 scale: Scalar,
1206) -> Result<Option<Element>, ExactMeasureError> {
1207 let speed = match curve {
1208 Curve3::Line(line) => line.direction.length(),
1209 Curve3::Circle(circle) => circle.radius.abs() * frame_scale(&circle.frame),
1210 Curve3::Ellipse(ellipse) => {
1211 ellipse.semi_axis_x.abs().max(ellipse.semi_axis_y.abs()) * frame_scale(&ellipse.frame)
1212 }
1213 _ => return Ok(None),
1214 };
1215 let plan_speed = match curve {
1217 Curve3::Line(line) => flat_length(line.direction),
1218 Curve3::Circle(c) => {
1219 (c.radius.abs() * flat_length(c.frame.x)).hypot(c.radius.abs() * flat_length(c.frame.y))
1220 }
1221 Curve3::Ellipse(e) => (e.semi_axis_x.abs() * flat_length(e.frame.x))
1222 .hypot(e.semi_axis_y.abs() * flat_length(e.frame.y)),
1223 _ => speed,
1224 };
1225 let centre = evaluate3(curve, 0.5 * (t0 + t1)).map_err(|_| crate::exact::EVALUATION)?;
1226 let radius = pad(0.5 * (t1 - t0).abs() * speed, scale);
1227 let plan_radius = if plan_speed.is_finite() {
1228 pad(0.5 * (t1 - t0).abs() * plan_speed, scale).min(radius)
1229 } else {
1230 radius
1231 };
1232 Ok(Some(Element {
1233 normal: None,
1234 spread: None,
1235 shape: Shape::Edge { edge, t0, t1 },
1236 centre,
1237 radius,
1238 plan_radius,
1239 witness: Some(centre),
1240 }))
1241}
1242
1243fn normal_spread(surface: &Surface, lo: Point2, hi: Point2) -> Option<Scalar> {
1245 let (du, dv) = ((hi.x - lo.x).abs(), (hi.y - lo.y).abs());
1246 let spread = match surface {
1247 Surface::Plane(_) => 0.0,
1248 Surface::Cylinder(_) => 0.5 * du,
1250 Surface::Cone(c) => {
1251 let slope = c.semi_angle.tan();
1255 let apex = -c.radius / slope;
1256 if !apex.is_finite() || (apex >= lo.y.min(hi.y) - 1e-9 && apex <= lo.y.max(hi.y) + 1e-9)
1257 {
1258 return None;
1259 }
1260 0.5 * du
1261 }
1262 Surface::EllipticalCylinder(c) => {
1264 let (a, b) = (c.semi_axis_x.abs(), c.semi_axis_y.abs());
1265 0.5 * du * a.max(b) / a.min(b)
1266 }
1267 Surface::Sphere(_) | Surface::Torus(_) => 0.5 * (du + dv),
1268 _ => return None,
1269 };
1270 spread.is_finite().then_some(spread + 1e-9)
1271}
1272
1273fn critical_possible(face: &Element, other: &Element, metric: Metric) -> bool {
1291 let (offset, reach) = match metric {
1292 Metric::Space => (other.centre - face.centre, face.radius + other.radius),
1293 Metric::Plan => (
1294 flat(other.centre - face.centre),
1295 face.plan_radius + other.plan_radius,
1296 ),
1297 };
1298 critical_along(face, offset, reach)
1299}
1300
1301pub(crate) fn critical_toward(face: &Element, p: Point3) -> bool {
1304 critical_along(face, p - face.centre, face.radius)
1305}
1306
1307fn critical_along(face: &Element, offset: Vec3, reach: Scalar) -> bool {
1310 let (Some(normal), Some(spread)) = (face.normal, face.spread) else {
1311 return true;
1312 };
1313 let gap = offset.length();
1314 if gap.is_nan() || gap <= reach {
1315 return true;
1316 }
1317 let aperture = (reach / gap).asin();
1318 let angle = spread + aperture + 1e-9;
1319 if angle >= core::f64::consts::FRAC_PI_2 {
1320 return true;
1321 }
1322 (offset / gap).dot(normal).abs() >= angle.cos() * normal.length()
1323}
1324
1325fn trig_range(a: Scalar, b: Scalar, t0: Scalar, t1: Scalar) -> (Scalar, Scalar) {
1327 let (t0, t1) = (t0.min(t1), t0.max(t1));
1328 let f = |t: Scalar| a * t.cos() + b * t.sin();
1329 let (mut lo, mut hi) = (f(t0).min(f(t1)), f(t0).max(f(t1)));
1330 let amplitude = a.hypot(b);
1331 let peak = b.atan2(a);
1332 let reaches = |angle: Scalar| {
1333 let k = ((t0 - angle) / core::f64::consts::TAU).ceil();
1334 angle + k * core::f64::consts::TAU <= t1
1335 };
1336 if reaches(peak) {
1337 hi = amplitude;
1338 }
1339 if reaches(peak + core::f64::consts::PI) {
1340 lo = -amplitude;
1341 }
1342 (lo, hi)
1343}
1344
1345fn sphere_range(element: &Element, d: Vec3) -> (Scalar, Scalar) {
1347 let c = element.centre.dot(d);
1348 (c - element.radius, c + element.radius)
1349}
1350
1351impl Side<'_> {
1352 pub(crate) fn project(
1355 &self,
1356 element: &Element,
1357 d: Vec3,
1358 ) -> Result<(Scalar, Scalar), ExactMeasureError> {
1359 let pad = PROJECTION_EPSILONS * Scalar::EPSILON * self.scale;
1362 let sphere = match self.metric {
1363 Metric::Space => sphere_range(element, d),
1364 Metric::Plan => {
1367 let c = element.centre.dot(d);
1368 (c - element.plan_radius, c + element.plan_radius)
1369 }
1370 };
1371 let exact = match element.shape {
1372 Shape::Face { face, lo, hi, .. } => {
1373 let surface = surface_of(self.brep, self.brep.topology().faces()[face].surface)?;
1374 match surface {
1375 Surface::Plane(p) => {
1376 let base = p.frame.origin.dot(d);
1377 let (x, y) = (p.frame.x.dot(d), p.frame.y.dot(d));
1378 let values = [
1379 base + x * lo.x + y * lo.y,
1380 base + x * hi.x + y * lo.y,
1381 base + x * lo.x + y * hi.y,
1382 base + x * hi.x + y * hi.y,
1383 ];
1384 Some(
1385 values
1386 .iter()
1387 .fold((Scalar::INFINITY, Scalar::NEG_INFINITY), |(a, b), v| {
1388 (a.min(*v), b.max(*v))
1389 }),
1390 )
1391 }
1392 Surface::Cylinder(c) => {
1393 let (a, b) = trig_range(
1394 c.radius * c.frame.x.dot(d),
1395 c.radius * c.frame.y.dot(d),
1396 lo.x,
1397 hi.x,
1398 );
1399 let z = c.frame.z.dot(d);
1400 let base = c.frame.origin.dot(d);
1401 Some((
1402 base + a + (z * lo.y).min(z * hi.y),
1403 base + b + (z * lo.y).max(z * hi.y),
1404 ))
1405 }
1406 Surface::EllipticalCylinder(c) => {
1407 let (a, b) = trig_range(
1408 c.semi_axis_x * c.frame.x.dot(d),
1409 c.semi_axis_y * c.frame.y.dot(d),
1410 lo.x,
1411 hi.x,
1412 );
1413 let z = c.frame.z.dot(d);
1414 let base = c.frame.origin.dot(d);
1415 Some((
1416 base + a + (z * lo.y).min(z * hi.y),
1417 base + b + (z * lo.y).max(z * hi.y),
1418 ))
1419 }
1420 Surface::Cone(c) => {
1421 let slope = c.semi_angle.tan();
1424 let base = c.frame.origin.dot(d);
1425 let (x, y, z) = (c.frame.x.dot(d), c.frame.y.dot(d), c.frame.z.dot(d));
1426 let mut range = (Scalar::INFINITY, Scalar::NEG_INFINITY);
1427 for v in [lo.y, hi.y] {
1428 let r = c.radius + v * slope;
1429 let (a, b) = trig_range(r * x, r * y, lo.x, hi.x);
1430 range = (range.0.min(base + a + z * v), range.1.max(base + b + z * v));
1431 }
1432 Some(range)
1433 }
1434 Surface::Sphere(sphere) => {
1435 let r = sphere.radius;
1438 let base = sphere.frame.origin.dot(d);
1439 let (w_lo, w_hi) =
1440 trig_range(sphere.frame.x.dot(d), sphere.frame.y.dot(d), lo.x, hi.x);
1441 let z = sphere.frame.z.dot(d);
1442 let mut range = (Scalar::INFINITY, Scalar::NEG_INFINITY);
1443 for w in [w_lo, w_hi] {
1444 let (a, b) = trig_range(r * w, r * z, lo.y, hi.y);
1445 range = (range.0.min(base + a), range.1.max(base + b));
1446 }
1447 Some(range)
1448 }
1449 Surface::Torus(torus) => {
1450 let (big, small) = (torus.major_radius, torus.minor_radius);
1454 if big.is_nan() || big <= small.abs() {
1455 return Ok((sphere.0 - pad, sphere.1 + pad));
1456 }
1457 let base = torus.frame.origin.dot(d);
1458 let (w_lo, w_hi) =
1459 trig_range(torus.frame.x.dot(d), torus.frame.y.dot(d), lo.x, hi.x);
1460 let z = torus.frame.z.dot(d);
1461 let mut range = (Scalar::INFINITY, Scalar::NEG_INFINITY);
1462 for w in [w_lo, w_hi] {
1463 let (a, b) = trig_range(small * w, small * z, lo.y, hi.y);
1464 range = (
1465 range.0.min(base + big * w + a),
1466 range.1.max(base + big * w + b),
1467 );
1468 }
1469 Some(range)
1470 }
1471 _ => None,
1472 }
1473 }
1474 Shape::Edge { edge, t0, t1 } => {
1475 let curve = self.brep.topology().edges()[edge]
1476 .curve
1477 .and_then(|id| self.brep.curves3().get(id.index()))
1478 .ok_or(ExactMeasureError::DanglingReference)?;
1479 match curve {
1480 Curve3::Line(line) => {
1481 let (a, b) = (
1482 (line.origin + line.direction * t0).dot(d),
1483 (line.origin + line.direction * t1).dot(d),
1484 );
1485 Some((a.min(b), a.max(b)))
1486 }
1487 Curve3::Circle(c) => {
1488 let (a, b) = trig_range(
1489 c.radius * c.frame.x.dot(d),
1490 c.radius * c.frame.y.dot(d),
1491 t0,
1492 t1,
1493 );
1494 let base = c.frame.origin.dot(d);
1495 Some((base + a, base + b))
1496 }
1497 Curve3::Ellipse(e) => {
1498 let (a, b) = trig_range(
1499 e.semi_axis_x * e.frame.x.dot(d),
1500 e.semi_axis_y * e.frame.y.dot(d),
1501 t0,
1502 t1,
1503 );
1504 let base = e.frame.origin.dot(d);
1505 Some((base + a, base + b))
1506 }
1507 _ => None,
1508 }
1509 }
1510 };
1511 let (lo, hi) = exact.map_or(sphere, |(lo, hi)| (lo.max(sphere.0), hi.min(sphere.1)));
1512 Ok((lo - pad, hi + pad))
1513 }
1514}
1515
1516fn lower_bound(
1519 side_a: &Side<'_>,
1520 a: &Element,
1521 side_b: &Side<'_>,
1522 b: &Element,
1523) -> Result<Scalar, ExactMeasureError> {
1524 let plan = side_a.metric == Metric::Plan;
1525 let (offset, reach) = if plan {
1526 (flat(b.centre - a.centre), a.plan_radius + b.plan_radius)
1527 } else {
1528 (b.centre - a.centre, a.radius + b.radius)
1529 };
1530 let gap = offset.length();
1531 let rounding = RATIO_EPSILONS * Scalar::EPSILON * (gap + reach);
1532 let mut best = (gap - reach - rounding).max(0.0);
1533 let direction = |n: Option<Vec3>| -> Option<Vec3> {
1536 let n = n?;
1537 if !plan {
1538 return Some(n);
1539 }
1540 let h = flat(n);
1541 let length = h.length();
1542 (length > 1e-6 * n.length()).then(|| h / length)
1543 };
1544 let directions = [
1545 (gap > 0.0).then(|| offset / gap),
1546 direction(a.normal),
1547 direction(b.normal),
1548 ];
1549 for d in directions.into_iter().flatten() {
1550 let (a_lo, a_hi) = side_a.project(a, d)?;
1551 let (b_lo, b_hi) = side_b.project(b, d)?;
1552 best = best.max(b_lo - a_hi).max(a_lo - b_hi);
1553 }
1554 Ok(best * (1.0 - RATIO_EPSILONS * Scalar::EPSILON))
1555}
1556
1557pub(crate) fn point_lower_bound(
1561 p: Point3,
1562 side: &Side<'_>,
1563 element: &Element,
1564) -> Result<Scalar, ExactMeasureError> {
1565 let offset = element.centre - p;
1566 let gap = offset.length();
1567 let rounding = RATIO_EPSILONS * Scalar::EPSILON * (gap + element.radius);
1568 let mut best = (gap - element.radius - rounding).max(0.0);
1569 let directions = [(gap > 0.0).then(|| offset / gap), element.normal];
1570 for d in directions.into_iter().flatten() {
1571 let length = d.length();
1572 if length.is_nan() || length <= 0.0 {
1573 continue;
1574 }
1575 let d = d / length;
1576 let (lo, hi) = side.project(element, d)?;
1577 let x = p.dot(d);
1578 let pad = PROJECTION_EPSILONS * Scalar::EPSILON * p.length();
1579 best = best.max(lo - x - pad).max(x - hi - pad);
1580 }
1581 Ok(best * (1.0 - RATIO_EPSILONS * Scalar::EPSILON))
1582}
1583
1584#[derive(Debug, Clone, Copy, PartialEq)]
1586pub(crate) struct Key(pub(crate) Scalar);
1587
1588impl Eq for Key {}
1589
1590impl PartialOrd for Key {
1591 fn partial_cmp(&self, other: &Self) -> Option<core::cmp::Ordering> {
1592 Some(self.cmp(other))
1593 }
1594}
1595
1596impl Ord for Key {
1597 fn cmp(&self, other: &Self) -> core::cmp::Ordering {
1598 self.0.total_cmp(&other.0)
1599 }
1600}
1601
1602pub(crate) struct Found {
1606 pub(crate) bounds: DistanceBounds,
1607 pub(crate) on: (Shape, Shape),
1609 pub(crate) overlap: Option<Point2>,
1610 #[cfg(test)]
1612 pub(crate) steps: usize,
1613}
1614
1615pub(crate) fn search(
1616 a: &ExactBRep,
1617 b: &ExactBRep,
1618 tolerance: Tolerance,
1619 metric: Metric,
1620 done: &mut dyn FnMut(Scalar, Scalar, Scalar) -> bool,
1621) -> Result<Found, ExactMeasureError> {
1622 search_within(a, b, tolerance, metric, MAX_STEPS, done)
1623}
1624
1625pub(crate) fn search_within(
1626 a: &ExactBRep,
1627 b: &ExactBRep,
1628 tolerance: Tolerance,
1629 metric: Metric,
1630 max_steps: usize,
1631 done: &mut dyn FnMut(Scalar, Scalar, Scalar) -> bool,
1632) -> Result<Found, ExactMeasureError> {
1633 search_with(a, b, tolerance, metric, max_steps, done, true)
1634}
1635
1636fn search_with(
1639 a: &ExactBRep,
1640 b: &ExactBRep,
1641 tolerance: Tolerance,
1642 metric: Metric,
1643 max_steps: usize,
1644 done: &mut dyn FnMut(Scalar, Scalar, Scalar) -> bool,
1645 early_touch: bool,
1646) -> Result<Found, ExactMeasureError> {
1647 let linear = tolerance.linear().max(1e-12);
1648 let side_a = Side::new(a, linear, metric)?;
1649 let side_b = Side::new(b, linear, metric)?;
1650 let scale = side_a.scale + side_b.scale;
1651 let error = side_a.witness_error() + side_b.witness_error();
1652 let excess = side_a.witness_excess() + side_b.witness_excess();
1653 let apart = |p: Point3, q: Point3| match metric {
1654 Metric::Space => (p - q).length(),
1655 Metric::Plan => flat_length(p - q),
1656 };
1657 let offer =
1661 |best: &mut Option<(Scalar, Point3, Point3, (Shape, Shape))>, a: &Element, b: &Element| {
1662 if let (Some(wa), Some(wb)) = (a.witness, b.witness) {
1663 let d = apart(wa, wb);
1664 if best.is_none_or(|(current, ..)| d < current) {
1665 *best = Some((d, wa, wb, (a.shape, b.shape)));
1666 }
1667 }
1668 };
1669 let mut elements_a = side_a.elements.clone();
1670 let mut elements_b = side_b.elements.clone();
1671
1672 let mut best: Option<(Scalar, Point3, Point3, (Shape, Shape))> = None;
1673 let mut heap = BinaryHeap::new();
1674 let viable = |a: &Element, b: &Element| {
1675 critical_possible(a, b, metric) && critical_possible(b, a, metric)
1676 };
1677 for (i, ea) in elements_a.iter().enumerate() {
1678 for (j, eb) in elements_b.iter().enumerate() {
1679 if viable(ea, eb) {
1680 heap.push(Reverse((Key(lower_bound(&side_a, ea, &side_b, eb)?), i, j)));
1681 offer(&mut best, ea, eb);
1682 }
1683 }
1684 }
1685
1686 let mut lower = 0.0;
1687 let mut steps = 0;
1688 let mut overlap = None;
1689 while let Some(Reverse((Key(bound), i, j))) = heap.pop() {
1690 lower = bound;
1691 let (ea, eb) = (elements_a[i], elements_b[j]);
1692 if metric == Metric::Plan && bound == 0.0 {
1693 if let Some((at, wa, wb)) = plan_patches_overlap(&side_a, &ea, &side_b, &eb)? {
1694 overlap = Some(at);
1695 best = Some((0.0, wa, wb, (ea.shape, eb.shape)));
1696 break;
1697 }
1698 }
1699 if early_touch && metric == Metric::Space && bound == 0.0 {
1702 if let Some((wa, wb)) = touch::touch(&side_a, &ea, &side_b, &eb)? {
1703 let d = apart(wa, wb);
1704 if best.is_none_or(|(current, ..)| d < current) {
1705 best = Some((d, wa, wb, (ea.shape, eb.shape)));
1706 }
1707 break;
1708 }
1709 }
1710 if bound > 0.0 && best.is_none_or(|(current, ..)| bound < current) {
1714 if let Some((wa, wb, on)) = witness::pair(&side_a, &ea, &side_b, &eb)? {
1715 let d = apart(wa, wb);
1716 if best.is_none_or(|(current, ..)| d < current) {
1717 best = Some((d, wa, wb, on));
1718 }
1719 }
1720 }
1721 let upper = best.map_or(Scalar::INFINITY, |(d, ..)| witnessed_upper(d, error));
1722 if best.is_some_and(|(d, ..)| bound >= d)
1726 || (upper.is_finite()
1727 && (bound >= upper || done(bound, upper, floor(scale, upper) + excess)))
1728 {
1729 break;
1730 }
1731 steps += 1;
1732 if steps > max_steps {
1733 break;
1734 }
1735 let split_a = side_a.size(&ea) >= side_b.size(&eb);
1738 let children = if split_a {
1739 side_a.split(&ea)?
1740 } else {
1741 side_b.split(&eb)?
1742 };
1743 let (side, parent) = if split_a {
1744 (&side_a, side_a.size(&ea))
1745 } else {
1746 (&side_b, side_b.size(&eb))
1747 };
1748 if children.iter().any(|child| side.size(child) >= parent) {
1749 heap.push(Reverse((Key(bound), i, j)));
1750 break;
1751 }
1752 for child in children {
1753 if split_a {
1754 elements_a.push(child);
1755 let index = elements_a.len() - 1;
1756 if viable(&child, &eb) {
1757 heap.push(Reverse((
1758 Key(lower_bound(&side_a, &child, &side_b, &eb)?),
1759 index,
1760 j,
1761 )));
1762 offer(&mut best, &child, &eb);
1763 }
1764 } else {
1765 elements_b.push(child);
1766 let index = elements_b.len() - 1;
1767 if viable(&ea, &child) {
1768 heap.push(Reverse((
1769 Key(lower_bound(&side_a, &ea, &side_b, &child)?),
1770 i,
1771 index,
1772 )));
1773 offer(&mut best, &ea, &child);
1774 }
1775 }
1776 }
1777 }
1778 if let Some(Reverse((Key(bound), _, _))) = heap.peek() {
1780 lower = lower.min(*bound);
1781 }
1782 let (distance, point_a, point_b, on) = best.ok_or(crate::exact::NOT_CONVERGED)?;
1783 let upper = if overlap.is_some() {
1784 lower = 0.0;
1785 0.0
1786 } else {
1787 witnessed_upper(distance, error)
1788 };
1789 Ok(Found {
1790 bounds: DistanceBounds {
1791 lower: lower.min(upper),
1792 upper,
1793 point_a,
1794 point_b,
1795 },
1796 on,
1797 overlap,
1798 #[cfg(test)]
1799 steps,
1800 })
1801}
1802
1803fn plan_patches_overlap(
1808 side_a: &Side<'_>,
1809 a: &Element,
1810 side_b: &Side<'_>,
1811 b: &Element,
1812) -> Result<Option<(Point2, Point3, Point3)>, ExactMeasureError> {
1813 let shadow = |side: &Side<'_>,
1816 e: &Element|
1817 -> Result<Option<([Point2; 4], axiolid_core::Frame3)>, ExactMeasureError> {
1818 let Shape::Face {
1819 face,
1820 lo,
1821 hi,
1822 inside: true,
1823 } = e.shape
1824 else {
1825 return Ok(None);
1826 };
1827 let Some(f) = level_frame(side, face)? else {
1828 return Ok(None);
1829 };
1830 let map = |u: Scalar, v: Scalar| {
1831 let p = f.origin + f.x * u + f.y * v;
1832 Point2::new(p.x, p.y)
1833 };
1834 let mut corners = [
1835 map(lo.x, lo.y),
1836 map(hi.x, lo.y),
1837 map(hi.x, hi.y),
1838 map(lo.x, hi.y),
1839 ];
1840 let det = f.x.x * f.y.y - f.x.y * f.y.x;
1841 if (hi.x - lo.x) * (hi.y - lo.y) * det < 0.0 {
1842 corners.reverse();
1843 }
1844 Ok(Some((corners, f)))
1845 };
1846 let (Some((pa, fa)), Some((pb, fb))) = (shadow(side_a, a)?, shadow(side_b, b)?) else {
1847 return Ok(None);
1848 };
1849 let cross =
1852 |o: Point2, p: Point2, q: Point2| (p.x - o.x) * (q.y - o.y) - (p.y - o.y) * (q.x - o.x);
1853 let mut poly: Vec<Point2> = pa.to_vec();
1854 for k in 0..4 {
1855 let (e0, e1) = (pb[k], pb[(k + 1) % 4]);
1856 let input = core::mem::take(&mut poly);
1857 for m in 0..input.len() {
1858 let (p, q) = (input[m], input[(m + 1) % input.len()]);
1859 let (sp, sq) = (cross(e0, e1, p), cross(e0, e1, q));
1860 if sp >= 0.0 {
1861 poly.push(p);
1862 }
1863 if (sp >= 0.0) != (sq >= 0.0) {
1864 let t = sp / (sp - sq);
1865 poly.push(p + (q - p) * t);
1866 }
1867 }
1868 if poly.is_empty() {
1869 return Ok(None);
1870 }
1871 }
1872 let at = poly.iter().fold(Point2::ZERO, |s, p| s + *p) / poly.len() as Scalar;
1873 let scale = pa
1874 .iter()
1875 .chain(pb.iter())
1876 .fold(1.0f64, |m, p| m.max(p.x.abs()).max(p.y.abs()));
1877 let margin = 1e-9 * scale;
1878 let clears = |ring: &[Point2; 4]| {
1879 (0..4).all(|k| {
1880 let (e0, e1) = (ring[k], ring[(k + 1) % 4]);
1881 let length = (e1 - e0).length();
1882 length > 0.0 && cross(e0, e1, at) / length > margin
1883 })
1884 };
1885 if !(clears(&pa) && clears(&pb)) {
1886 return Ok(None);
1887 }
1888 let lift = |f: axiolid_core::Frame3| {
1890 let det = f.x.x * f.y.y - f.x.y * f.y.x;
1891 let (dx, dy) = (at.x - f.origin.x, at.y - f.origin.y);
1892 let u = (dx * f.y.y - dy * f.y.x) / det;
1893 let v = (f.x.x * dy - f.x.y * dx) / det;
1894 f.origin + f.x * u + f.y * v
1895 };
1896 Ok(Some((at, lift(fa), lift(fb))))
1897}
1898
1899#[cfg(test)]
1900pub(crate) mod fixtures {
1901 use axiolid_brep::{ExactBRep, ExactBRepBuilder};
1904 use axiolid_core::{Frame3, Interval, Point2, Point3, Vec2, Vec3};
1905 use axiolid_curve::{Curve2, Curve3, Line2, Line3};
1906 use axiolid_surface::{Plane, Surface};
1907 use axiolid_topology::{Edge, EdgeUse, Face, FaceBound, Loop, Orientation, Vertex};
1908
1909 pub(crate) fn cuboid(o: Point3, x: Vec3, y: Vec3, z: Vec3) -> ExactBRep {
1912 let mut builder = ExactBRepBuilder::default();
1913 let axes = [x, y, z];
1914 let corner = |i: usize| {
1915 o + x * (i & 1) as f64 + y * ((i >> 1) & 1) as f64 + z * ((i >> 2) & 1) as f64
1916 };
1917 let vertices: Vec<_> = (0..8)
1918 .map(|i| {
1919 builder.topology_mut().add_vertex(Vertex {
1920 position: corner(i),
1921 })
1922 })
1923 .collect();
1924 let mut edges = std::collections::HashMap::new();
1926 for i in 0..8usize {
1927 for (k, axis) in axes.iter().enumerate() {
1928 if i & (1 << k) != 0 {
1929 continue;
1930 }
1931 let length = axis.length();
1932 let curve = builder.add_curve3(Curve3::Line(Line3 {
1933 origin: corner(i),
1934 direction: *axis / length,
1935 }));
1936 let edge = builder.topology_mut().add_edge(Edge {
1937 start: vertices[i],
1938 end: vertices[i | (1 << k)],
1939 curve: Some(curve),
1940 });
1941 builder.set_edge_interval(edge, Interval::new(0.0, length));
1942 edges.insert((i, i | (1 << k)), edge);
1943 }
1944 }
1945 for (fixed, side) in [(2, 0), (2, 1), (1, 0), (1, 1), (0, 0), (0, 1)] {
1947 let (ka, kb) = match fixed {
1948 0 => (1, 2),
1949 1 => (0, 2),
1950 _ => (0, 1),
1951 };
1952 let base = side << fixed;
1953 let (la, lb) = (axes[ka].length(), axes[kb].length());
1954 let (ux, uy) = (axes[ka] / la, axes[kb] / lb);
1955 let surface = builder.add_surface(Surface::Plane(Plane {
1956 frame: Frame3 {
1957 origin: corner(base),
1958 x: ux,
1959 y: uy,
1960 z: ux.cross(uy),
1961 },
1962 }));
1963 let ring = [
1965 base,
1966 base | (1 << ka),
1967 base | (1 << ka) | (1 << kb),
1968 base | (1 << kb),
1969 ];
1970 let params = [
1971 Point2::new(0.0, 0.0),
1972 Point2::new(la, 0.0),
1973 Point2::new(la, lb),
1974 Point2::new(0.0, lb),
1975 ];
1976 let mut uses = Vec::new();
1977 let mut intervals = Vec::new();
1978 for m in 0..4 {
1979 let (from, to) = (ring[m], ring[(m + 1) % 4]);
1980 let (edge, orientation) = match edges.get(&(from, to)) {
1981 Some(edge) => (*edge, Orientation::Forward),
1982 None => (edges[&(to, from)], Orientation::Reversed),
1983 };
1984 let (p, q) = (params[m], params[(m + 1) % 4]);
1985 let length = (q - p).length();
1986 let pcurve = builder.add_curve2(Curve2::Line(Line2 {
1987 origin: p,
1988 direction: Vec2::new((q.x - p.x) / length, (q.y - p.y) / length),
1989 }));
1990 uses.push(EdgeUse {
1991 edge,
1992 orientation,
1993 pcurve: Some(pcurve),
1994 });
1995 intervals.push(Interval::new(0.0, length));
1996 }
1997 let loop_id = builder.topology_mut().add_loop(Loop { edges: uses });
1998 for (index, interval) in intervals.into_iter().enumerate() {
1999 builder.set_pcurve_interval(loop_id, index, interval);
2000 }
2001 builder.topology_mut().add_face(Face {
2002 surface: Some(surface),
2003 bounds: vec![FaceBound {
2004 loop_id,
2005 orientation: Orientation::Forward,
2006 outer: true,
2007 }],
2008 orientation: Orientation::Forward,
2009 });
2010 }
2011 builder.finish().expect("a valid box")
2012 }
2013
2014 pub(crate) fn unit_box(o: Point3) -> ExactBRep {
2015 cuboid(o, Vec3::X, Vec3::Y, Vec3::Z)
2016 }
2017}
2018
2019#[cfg(test)]
2020mod tests {
2021 use super::{
2026 critical_possible, flat_length, floor, normal_spread, pad, patch_plan_radius, patch_scale,
2027 patch_sphere, plan_lipschitz, witness_scale, witnessed_upper, Element, Metric, Shape, Side,
2028 PROJECTION_EPSILONS,
2029 };
2030 use axiolid_core::{Frame3, Point2, Point3, Vec3};
2031 use axiolid_evaluate::surface::partials;
2032 use axiolid_evaluate::surface::{evaluate, normal};
2033 use axiolid_surface::{Cone, Cylinder, EllipticalCylinder, Plane, Sphere, Surface, Torus};
2034
2035 const FAR: Vec3 = Vec3::new(450_000.0, 5_600_000.0, 0.0);
2037
2038 fn frame_at(shift: Vec3) -> Frame3 {
2039 let x = Vec3::new(0.6, 0.8, 0.0);
2041 let z = Vec3::new(0.0, 0.0, 1.0);
2042 Frame3 {
2043 origin: Point3::new(1.5, -2.0, 0.75) + shift,
2044 x,
2045 y: z.cross(x),
2046 z,
2047 }
2048 }
2049
2050 fn families() -> Vec<(Surface, Point2, Point2)> {
2051 families_at(Vec3::ZERO)
2052 }
2053
2054 fn families_at(shift: Vec3) -> Vec<(Surface, Point2, Point2)> {
2055 let f = frame_at(shift);
2056 vec![
2057 (
2058 Surface::Plane(Plane { frame: f }),
2059 Point2::new(-1.0, 0.5),
2060 Point2::new(2.0, 1.25),
2061 ),
2062 (
2063 Surface::Cylinder(Cylinder {
2064 frame: f,
2065 radius: 2.5,
2066 }),
2067 Point2::new(0.3, -1.0),
2068 Point2::new(1.9, 2.0),
2069 ),
2070 (
2071 Surface::EllipticalCylinder(EllipticalCylinder {
2072 frame: f,
2073 semi_axis_x: 3.0,
2074 semi_axis_y: 1.0,
2075 }),
2076 Point2::new(0.2, 0.0),
2077 Point2::new(1.4, 1.0),
2078 ),
2079 (
2080 Surface::Cone(Cone {
2081 frame: f,
2082 radius: 1.5,
2083 semi_angle: 0.4,
2084 }),
2085 Point2::new(0.5, 0.2),
2086 Point2::new(2.5, 1.5),
2087 ),
2088 (
2089 Surface::Sphere(Sphere {
2090 frame: f,
2091 radius: 2.0,
2092 }),
2093 Point2::new(0.4, -0.3),
2094 Point2::new(2.0, 1.1),
2095 ),
2096 (
2097 Surface::Torus(Torus {
2098 frame: f,
2099 major_radius: 3.0,
2100 minor_radius: 1.0,
2101 }),
2102 Point2::new(0.1, 0.5),
2103 Point2::new(1.7, 2.9),
2104 ),
2105 ]
2106 }
2107
2108 fn samples(lo: Point2, hi: Point2) -> impl Iterator<Item = Point2> {
2109 const N: usize = 24;
2110 (0..=N).flat_map(move |i| {
2111 (0..=N).map(move |j| {
2112 Point2::new(
2113 lo.x + (hi.x - lo.x) * i as f64 / N as f64,
2114 lo.y + (hi.y - lo.y) * j as f64 / N as f64,
2115 )
2116 })
2117 })
2118 }
2119
2120 #[test]
2128 fn side_by_side_boxes_in_plan_stop_at_once() {
2129 use super::fixtures::cuboid;
2130 use axiolid_core::Tolerance;
2131 let a = cuboid(Point3::ZERO, Vec3::X, Vec3::Y, Vec3::Z);
2132 let b = cuboid(Point3::new(1.0, 0.0, 0.0), Vec3::X, Vec3::Y, Vec3::Z);
2133 let found = super::search(
2134 &a,
2135 &b,
2136 Tolerance::METRE,
2137 Metric::Plan,
2138 &mut |lower, _, _| lower > 0.0,
2139 )
2140 .expect("bounded");
2141 assert!(found.steps <= 10, "{} steps", found.steps);
2142 }
2143
2144 #[test]
2145 fn an_extreme_weight_ratio_witness_scale_is_refused() {
2146 use crate::exact::ExactMeasureError;
2147 use axiolid_curve::{BSplineSurface, KnotSpec};
2148 let net = |high: f64| {
2149 let mut weights = vec![vec![1.0; 2]; 2];
2150 weights[1][1] = high;
2151 Surface::BSpline(BSplineSurface {
2152 u_degree: 1,
2153 v_degree: 1,
2154 control_points: vec![
2155 vec![Point3::new(0.0, 0.0, 0.0), Point3::new(0.0, 1.0, 0.0)],
2156 vec![Point3::new(1.0, 0.0, 0.0), Point3::new(1.0, 1.0, 0.0)],
2157 ],
2158 u_knots: vec![0.0, 1.0],
2159 u_multiplicities: vec![2, 2],
2160 v_knots: vec![0.0, 1.0],
2161 v_multiplicities: vec![2, 2],
2162 weights: Some(weights),
2163 u_closed: false,
2164 v_closed: false,
2165 knot_spec: KnotSpec::Unspecified,
2166 self_intersect: None,
2167 })
2168 };
2169 let (lo, hi) = (Point2::new(0.0, 0.0), Point2::new(1.0, 1.0));
2170 assert!(witness_scale(&net(1e6), lo, hi).is_ok());
2171 assert!(matches!(
2173 witness_scale(&net(1e14), lo, hi),
2174 Err(ExactMeasureError::NonPlanarFace(
2175 "extreme weight ratio B-spline"
2176 ))
2177 ));
2178 }
2179
2180 #[test]
2181 fn a_far_clearance_is_never_below_a_limit_the_distance_reaches() {
2182 use super::fixtures::cuboid;
2183 use super::{boundary_clearance, boundary_distance, Clearance};
2184 use axiolid_core::Tolerance;
2185 let (x, y, z) = (Vec3::new(0.6, 0.8, 0.0), Vec3::new(-0.8, 0.6, 0.0), Vec3::Z);
2186 for k in 0..40_i32 {
2187 let o = Point3::new(
2188 450_000.0 + f64::from(k) * 0.0137,
2189 5_600_000.0 + f64::from(k) * 0.0291,
2190 0.0,
2191 );
2192 let a = cuboid(o, x * 4.0, y, z * 2.0);
2193 let ob = o
2194 + y * 1.5
2195 + x * (0.5 + f64::from(k % 3) * 0.75)
2196 + z * (0.25 + f64::from(k % 4) * 0.125);
2197 let b = cuboid(ob, x, y, z * 0.5);
2198 let ux = (x * 4.0) / (x * 4.0).length();
2201 let n = Vec3::new(ux.y, -ux.x, 0.0);
2202 let truth = (ob - (o + y)).dot(n / n.length()).abs();
2203 let d = boundary_distance(&a, &b, 1e-9, Tolerance::METRE).expect("bounded");
2204 assert!(d.lower <= truth && truth <= d.upper, "{k}: {truth} {d:?}");
2205 assert!((d.point_a - d.point_b).length() <= d.upper, "{k}: {d:?}");
2206 let verdict = |limit: f64| {
2207 boundary_clearance(&a, &b, limit, Tolerance::METRE)
2208 .expect("bounded")
2209 .1
2210 };
2211 assert_eq!(
2212 verdict(truth - 1e-15),
2213 Clearance::Indeterminate,
2214 "{k}: {truth}"
2215 );
2216 assert_eq!(verdict(truth + 1e-6), Clearance::Below, "{k}");
2217 assert_eq!(verdict(truth - 1e-6), Clearance::Above, "{k}");
2218 }
2219 }
2220
2221 #[test]
2225 fn a_spline_witness_scale_counts_its_degrees_and_weights() {
2226 use axiolid_curve::{BSplineSurface, KnotSpec};
2227 let net = |weights: Option<Vec<Vec<f64>>>| {
2228 Surface::BSpline(BSplineSurface {
2229 u_degree: 1,
2230 v_degree: 1,
2231 control_points: vec![
2232 vec![Point3::new(0.0, 0.0, 0.0), Point3::new(0.0, 1.0, 0.0)],
2233 vec![Point3::new(1.0, 0.0, 0.0), Point3::new(3.0, 4.0, 0.0)],
2234 ],
2235 u_knots: vec![0.0, 1.0],
2236 u_multiplicities: vec![2, 2],
2237 v_knots: vec![0.0, 1.0],
2238 v_multiplicities: vec![2, 2],
2239 weights,
2240 u_closed: false,
2241 v_closed: false,
2242 knot_spec: KnotSpec::Unspecified,
2243 self_intersect: None,
2244 })
2245 };
2246 let (lo, hi) = (Point2::new(0.0, 0.0), Point2::new(1.0, 1.0));
2247 assert_eq!(patch_scale(&net(None), lo, hi).expect("bounded"), 5.0);
2248 assert_eq!(witness_scale(&net(None), lo, hi).expect("bounded"), 15.0);
2249 let rational = net(Some(vec![vec![1.0, 2.0], vec![4.0, 1.0]]));
2250 assert_eq!(witness_scale(&rational, lo, hi).expect("bounded"), 60.0);
2251 for (surface, lo, hi) in families_at(FAR) {
2253 assert_eq!(
2254 witness_scale(&surface, lo, hi).expect("bounded"),
2255 patch_scale(&surface, lo, hi).expect("bounded")
2256 );
2257 }
2258 }
2259
2260 #[test]
2264 fn far_spline_points_lie_within_their_witness_error() {
2265 use axiolid_curve::{BSplineSurface, KnotSpec};
2266 let net = |shift: Vec3| {
2267 let control_points = (0..4)
2268 .map(|i| {
2269 (0..4)
2270 .map(|j| {
2271 let (u, v) = (f64::from(i) * 0.37, f64::from(j) * 0.29);
2272 Point3::new(u, v, 0.1 * (u * 3.1).sin() * (v * 2.3).cos()) + shift
2273 })
2274 .collect()
2275 })
2276 .collect();
2277 let weights = (0..4)
2278 .map(|i| {
2279 (0..4)
2280 .map(|j| 1.0 + 0.25 * f64::from((i * 3 + j) % 5))
2281 .collect()
2282 })
2283 .collect();
2284 Surface::BSpline(BSplineSurface {
2285 u_degree: 3,
2286 v_degree: 3,
2287 control_points,
2288 u_knots: vec![0.0, 1.0],
2289 u_multiplicities: vec![4, 4],
2290 v_knots: vec![0.0, 1.0],
2291 v_multiplicities: vec![4, 4],
2292 weights: Some(weights),
2293 u_closed: false,
2294 v_closed: false,
2295 knot_spec: KnotSpec::Unspecified,
2296 self_intersect: None,
2297 })
2298 };
2299 let (near, far) = (net(Vec3::ZERO), net(FAR));
2300 let (lo, hi) = (Point2::new(0.0, 0.0), Point2::new(1.0, 1.0));
2301 let error = 16.0 * f64::EPSILON * witness_scale(&far, lo, hi).expect("bounded");
2302 let mut worst: f64 = 0.0;
2303 for p in samples(lo, hi) {
2304 let there = evaluate(&far, p.x, p.y).expect("point");
2305 let here = evaluate(&near, p.x, p.y).expect("point") + FAR;
2306 worst = worst.max((there - here).length());
2307 }
2308 assert!(worst <= error, "{worst} > {error}");
2310 }
2311
2312 #[test]
2313 fn rounding_margins_are_the_derived_constants() {
2314 assert_eq!(pad(0.0, 1.0), 16.0 * f64::EPSILON + f64::MIN_POSITIVE);
2315 assert_eq!(floor(3.0, 0.0), 128.0 * f64::EPSILON * 3.0);
2316 assert_eq!(
2317 witnessed_upper(0.5, 1e-9),
2318 0.5 * (1.0 + 8.0 * f64::EPSILON) + 1e-9
2319 );
2320 let origin = Point3::new(450_000.0, 5_600_000.0, 0.0);
2321 let plane = Surface::Plane(Plane {
2322 frame: Frame3 {
2323 origin,
2324 x: Vec3::X,
2325 y: Vec3::Y,
2326 z: Vec3::Z,
2327 },
2328 });
2329 let scale = patch_scale(&plane, Point2::new(0.0, 0.0), Point2::new(1.0, 1.0));
2330 assert_eq!(scale.expect("bounded"), origin.length() + 2.0);
2331 }
2332
2333 #[test]
2336 fn child_scales_never_exceed_their_parent() {
2337 for shift in [Vec3::ZERO, FAR] {
2338 for (surface, lo, hi) in families_at(shift) {
2339 let whole = patch_scale(&surface, lo, hi).expect("bounded");
2340 let mid = (lo + hi) * 0.5;
2341 for (a, b) in [
2342 (lo, mid),
2343 (Point2::new(mid.x, lo.y), Point2::new(hi.x, mid.y)),
2344 (Point2::new(lo.x, mid.y), Point2::new(mid.x, hi.y)),
2345 (mid, hi),
2346 ] {
2347 let part = patch_scale(&surface, a, b).expect("bounded");
2348 assert!(part <= whole, "{surface:?}: {part} > {whole}");
2349 }
2350 }
2351 }
2352 }
2353
2354 #[test]
2355 fn every_far_patch_point_lies_in_its_sphere() {
2356 for (surface, lo, hi) in families_at(FAR) {
2357 let scale = patch_scale(&surface, lo, hi).expect("bounded");
2358 let (centre, radius) = patch_sphere(&surface, lo, hi, scale).expect("bounded");
2359 let mut reach: f64 = 0.0;
2360 for p in samples(lo, hi) {
2361 let point = evaluate(&surface, p.x, p.y).expect("point");
2362 reach = reach.max((point - centre).length());
2363 }
2364 assert!(
2365 reach <= radius,
2366 "{surface:?}: reach {reach} > radius {radius}"
2367 );
2368 assert!(
2369 radius <= 4.0 * reach + 1e-9,
2370 "{surface:?}: {radius} vs {reach}"
2371 );
2372 }
2373 }
2374
2375 #[test]
2376 fn a_far_projection_carries_its_pad() {
2377 use super::fixtures::unit_box;
2378 let b = unit_box(Point3::ZERO + FAR);
2379 let side = Side::new(&b, 1e-12, Metric::Space).expect("a side");
2380 let top = side
2381 .elements
2382 .iter()
2383 .find(|e| matches!(e.shape, Shape::Face { .. }) && e.centre.z == 1.0)
2384 .expect("the top face");
2385 let (lo, hi) = side.project(top, Vec3::Z).expect("projects");
2386 let pad = PROJECTION_EPSILONS * f64::EPSILON * side.scale;
2387 assert!(pad > 1e-9, "{pad}");
2388 assert!(hi - lo >= 2.0 * pad - 4.0 * f64::EPSILON, "[{lo}, {hi}]");
2389 }
2390
2391 #[test]
2392 fn every_patch_point_lies_in_its_sphere() {
2393 for (surface, lo, hi) in families() {
2394 let scale = patch_scale(&surface, lo, hi).expect("bounded");
2395 let (centre, radius) = patch_sphere(&surface, lo, hi, scale).expect("bounded");
2396 let mut reach: f64 = 0.0;
2397 for p in samples(lo, hi) {
2398 let point = evaluate(&surface, p.x, p.y).expect("point");
2399 reach = reach.max((point - centre).length());
2400 }
2401 assert!(
2402 reach <= radius,
2403 "{surface:?}: reach {reach} > radius {radius}"
2404 );
2405 assert!(
2407 radius <= 4.0 * reach + 1e-9,
2408 "{surface:?}: {radius} vs {reach}"
2409 );
2410 }
2411 }
2412
2413 #[test]
2418 fn every_patch_projection_lies_in_its_plan_disc() {
2419 let tilt = |surface: Surface| -> Surface {
2420 let x = Vec3::new(0.6, 0.64, 0.48);
2421 let z = Vec3::new(0.0, -0.6, 0.8);
2422 let y = z.cross(x);
2423 let f = Frame3 {
2424 origin: Point3::new(1.5, -2.0, 0.75),
2425 x,
2426 y,
2427 z,
2428 };
2429 match surface {
2430 Surface::Plane(_) => Surface::Plane(Plane { frame: f }),
2431 Surface::Cylinder(c) => Surface::Cylinder(Cylinder { frame: f, ..c }),
2432 Surface::EllipticalCylinder(c) => {
2433 Surface::EllipticalCylinder(EllipticalCylinder { frame: f, ..c })
2434 }
2435 Surface::Cone(c) => Surface::Cone(Cone { frame: f, ..c }),
2436 Surface::Sphere(c) => Surface::Sphere(Sphere { frame: f, ..c }),
2437 Surface::Torus(c) => Surface::Torus(Torus { frame: f, ..c }),
2438 other => other,
2439 }
2440 };
2441 let cases = families()
2442 .into_iter()
2443 .flat_map(|(s, lo, hi)| [(s.clone(), lo, hi), (tilt(s), lo, hi)]);
2444 for (surface, lo, hi) in cases {
2445 let scale = patch_scale(&surface, lo, hi).expect("bounded");
2446 let (centre, radius) = patch_sphere(&surface, lo, hi, scale).expect("bounded");
2447 let plan = patch_plan_radius(&surface, lo, hi, centre, radius, scale).expect("bounded");
2448 assert!(plan <= radius);
2449 let mut reach: f64 = 0.0;
2450 for p in samples(lo, hi) {
2451 let point = evaluate(&surface, p.x, p.y).expect("point");
2452 reach = reach.max((point.x - centre.x).hypot(point.y - centre.y));
2453 }
2454 assert!(
2455 reach <= plan,
2456 "{surface:?}: reach {reach} > plan radius {plan}"
2457 );
2458 assert!(plan <= 4.0 * reach + 1e-9, "{surface:?}: {plan} vs {reach}");
2459 }
2460 }
2461
2462 #[test]
2466 fn plan_speeds_bound_every_horizontal_derivative() {
2467 let x = Vec3::new(0.6, 0.64, 0.48);
2468 let z = Vec3::new(0.0, -0.6, 0.8);
2469 let f = Frame3 {
2470 origin: Point3::new(1.5, -2.0, 0.75),
2471 x,
2472 y: z.cross(x),
2473 z,
2474 };
2475 let (lo, hi) = (
2476 Point2::new(0.0, 0.2),
2477 Point2::new(core::f64::consts::TAU, 1.5),
2478 );
2479 for surface in [
2480 Surface::Cylinder(Cylinder {
2481 frame: f,
2482 radius: 2.5,
2483 }),
2484 Surface::EllipticalCylinder(EllipticalCylinder {
2485 frame: f,
2486 semi_axis_x: 3.0,
2487 semi_axis_y: 1.0,
2488 }),
2489 Surface::Cone(Cone {
2490 frame: f,
2491 radius: 1.5,
2492 semi_angle: 0.4,
2493 }),
2494 ] {
2495 let (lu, lv) = plan_lipschitz(&surface, lo, hi).expect("bounded");
2496 let (mut su, mut sv) = (0.0f64, 0.0f64);
2497 for i in 0..=720 {
2498 for j in 0..=8 {
2499 let u = lo.x + (hi.x - lo.x) * f64::from(i) / 720.0;
2500 let v = lo.y + (hi.y - lo.y) * f64::from(j) / 8.0;
2501 let (du, dv) = partials(&surface, u, v).expect("partials");
2502 su = su.max(flat_length(du));
2503 sv = sv.max(flat_length(dv));
2504 }
2505 }
2506 assert!(su <= lu * (1.0 + 1e-12), "{surface:?}: {su} > {lu}");
2507 assert!(sv <= lv * (1.0 + 1e-12), "{surface:?}: {sv} > {lv}");
2508 }
2509 }
2510
2511 #[test]
2514 fn a_wall_patch_faces_an_element_high_above_in_plan() {
2515 let element = |centre: Point3, normal: Option<Vec3>, spread: Option<f64>| Element {
2516 shape: Shape::Edge {
2517 edge: 0,
2518 t0: 0.0,
2519 t1: 1.0,
2520 },
2521 centre,
2522 radius: 0.1,
2523 plan_radius: 0.1,
2524 witness: None,
2525 normal,
2526 spread,
2527 };
2528 let wall = element(Point3::ZERO, Some(Vec3::X), Some(0.01));
2529 let above = element(Point3::new(10.0, 0.0, 10.0), None, None);
2530 assert!(critical_possible(&wall, &above, Metric::Plan));
2531 assert!(!critical_possible(&wall, &above, Metric::Space));
2533 }
2534
2535 #[test]
2536 fn every_patch_normal_lies_in_its_cone() {
2537 let mid = |lo: Point2, hi: Point2| (lo + hi) * 0.5;
2538 for (surface, lo, hi) in families() {
2539 let Some(spread) = normal_spread(&surface, lo, hi) else {
2540 continue;
2541 };
2542 let c = mid(lo, hi);
2543 let axis = normal(&surface, c.x, c.y).expect("normal");
2544 let mut widest: f64 = 0.0;
2545 for p in samples(lo, hi) {
2546 let n = normal(&surface, p.x, p.y).expect("normal");
2547 widest = widest.max(n.dot(axis).clamp(-1.0, 1.0).acos());
2548 }
2549 assert!(
2550 widest <= spread,
2551 "{surface:?}: normals turn {widest} > {spread}"
2552 );
2553 }
2554 }
2555}