1use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};
38use ogeom_geom::{Curve, Curve3d as _, Surface as _, SurfaceGeometry};
39use ogeom_intersect::walk::Condition;
40use ogeom_intersect::{Marching, Stopped};
41use ogeom_math::{Point, Vector, solve};
42
43use crate::law::RadiusAlong;
44
45#[derive(Debug, Clone, Copy, PartialEq, Eq)]
53pub struct Sides {
54 pub first: i8,
56 pub second: i8,
58}
59
60impl Sides {
61 const ALL: [Self; 4] = [
63 Self {
64 first: 1,
65 second: 1,
66 },
67 Self {
68 first: 1,
69 second: -1,
70 },
71 Self {
72 first: -1,
73 second: 1,
74 },
75 Self {
76 first: -1,
77 second: -1,
78 },
79 ];
80}
81
82#[derive(Debug, Clone, Copy, PartialEq, Eq)]
86pub enum BlendStop {
87 Closed,
89 LeftTheFirstSupport,
91 LeftTheSecondSupport,
93 LeftBothSupports,
95 RanPastTheGuide,
97 SectionCollapsed,
100 Stalled,
103 RanOut,
106}
107
108#[derive(Debug, Clone)]
110pub struct MarchedBlend {
111 pub spine: Vec<Point>,
113 pub on_first: Vec<(f64, f64)>,
118 pub on_second: Vec<(f64, f64)>,
120 pub touch_first: Vec<Point>,
122 pub touch_second: Vec<Point>,
124 pub along: Vec<f64>,
127 pub sides: Sides,
129 pub stopped: BlendStop,
131}
132
133impl MarchedBlend {
134 #[must_use]
136 pub fn len(&self) -> usize {
137 self.spine.len()
138 }
139
140 #[must_use]
142 pub fn is_empty(&self) -> bool {
143 self.spine.is_empty()
144 }
145
146 #[must_use]
148 pub const fn complete(&self) -> bool {
149 !matches!(self.stopped, BlendStop::RanOut)
150 }
151}
152
153pub fn march_blend(
171 first: &SurfaceGeometry,
172 second: &SurfaceGeometry,
173 radius: f64,
174 guide: &Curve,
175 options: Marching,
176 tol: Tolerances,
177) -> OgeomResult<MarchedBlend> {
178 march_blend_on(
179 first,
180 second,
181 radius,
182 None,
183 guide,
184 &Sides::ALL,
185 None,
186 options,
187 tol,
188 )
189}
190
191pub fn march_blend_sided(
202 first: &SurfaceGeometry,
203 second: &SurfaceGeometry,
204 radius: f64,
205 guide: &Curve,
206 sides: Sides,
207 options: Marching,
208 tol: Tolerances,
209) -> OgeomResult<MarchedBlend> {
210 march_blend_on(
211 first,
212 second,
213 radius,
214 None,
215 guide,
216 &[sides],
217 None,
218 options,
219 tol,
220 )
221}
222
223#[allow(clippy::too_many_arguments, reason = "one construction, all its data")]
236pub fn march_blend_seeded(
237 first: &SurfaceGeometry,
238 second: &SurfaceGeometry,
239 radius: f64,
240 guide: &Curve,
241 sides: Sides,
242 seed: f64,
243 options: Marching,
244 tol: Tolerances,
245) -> OgeomResult<MarchedBlend> {
246 march_blend_on(
247 first,
248 second,
249 radius,
250 None,
251 guide,
252 &[sides],
253 Some(seed),
254 options,
255 tol,
256 )
257}
258
259#[allow(clippy::too_many_arguments, reason = "one construction, all its data")]
263pub(crate) fn march_blend_law(
264 first: &SurfaceGeometry,
265 second: &SurfaceGeometry,
266 law: &RadiusAlong,
267 guide: &Curve,
268 sides: Sides,
269 seed: f64,
270 options: Marching,
271 tol: Tolerances,
272) -> OgeomResult<MarchedBlend> {
273 march_blend_on(
274 first,
275 second,
276 law.largest(),
277 Some(law),
278 guide,
279 &[sides],
280 Some(seed),
281 options,
282 tol,
283 )
284}
285
286#[allow(clippy::too_many_arguments, reason = "one construction, all its data")]
287fn march_blend_on(
288 first: &SurfaceGeometry,
289 second: &SurfaceGeometry,
290 radius: f64,
291 law: Option<&RadiusAlong>,
292 guide: &Curve,
293 candidates: &[Sides],
294 seed: Option<f64>,
295 options: Marching,
296 tol: Tolerances,
297) -> OgeomResult<MarchedBlend> {
298 if !radius.is_finite() || radius <= tol.confusion() {
299 ogeom_bail!(Construction, "a blend of radius {radius} rounds nothing");
300 }
301 options.validate()?;
302 let (start, sides) = seat(
303 first, second, radius, law, guide, candidates, seed, None, tol,
304 )?;
305 let guide_loops = guide.is_periodic() || {
306 let (lo, hi) = guide.domain();
307 guide
308 .point_at(lo, tol)
309 .and_then(|p| guide.point_at(hi, tol).map(|q| p.distance(q)))
310 .is_ok_and(|d| d <= tol.confusion() * 10.0)
311 };
312 let contact = BallContact {
313 first,
314 second,
315 radius,
316 law,
317 guide,
318 sides,
319 guide_loops,
320 };
321 let walked = ogeom_intersect::walk::follow(&contact, &start, options, tol)?;
322
323 let mut blend = MarchedBlend {
324 spine: Vec::with_capacity(walked.states.len()),
325 on_first: Vec::with_capacity(walked.states.len()),
326 on_second: Vec::with_capacity(walked.states.len()),
327 touch_first: Vec::with_capacity(walked.states.len()),
328 touch_second: Vec::with_capacity(walked.states.len()),
329 along: Vec::with_capacity(walked.states.len()),
330 sides,
331 stopped: BlendStop::Stalled,
332 };
333 for x in &walked.states {
334 let (Ok(p1), Ok(p2)) = (
335 first.point_at(x[0], x[1], tol),
336 second.point_at(x[2], x[3], tol),
337 ) else {
338 continue;
339 };
340 let Some(centre) = contact.centre(x, tol) else {
341 continue;
342 };
343 blend.spine.push(centre);
344 blend.on_first.push((x[0], x[1]));
345 blend.on_second.push((x[2], x[3]));
346 blend.touch_first.push(p1);
347 blend.touch_second.push(p2);
348 blend.along.push(x[4]);
349 }
350 blend.stopped = why(&contact, &walked, tol);
351 Ok(blend)
352}
353
354fn why(
356 contact: &BallContact<'_>,
357 walked: &ogeom_intersect::walk::Walked,
358 tol: Tolerances,
359) -> BlendStop {
360 let Some(last) = walked.states.last() else {
361 return BlendStop::Stalled;
362 };
363 let collapsed = matches!(
367 (
368 contact.first.point_at(last[0], last[1], tol),
369 contact.second.point_at(last[2], last[3], tol),
370 ),
371 (Ok(p1), Ok(p2)) if p1.distance(p2) <= contact.radius * 1e-3
372 );
373 match walked.stopped {
374 Stopped::Closed => BlendStop::Closed,
375 Stopped::RanOut => BlendStop::RanOut,
376 Stopped::Stalled if collapsed => BlendStop::SectionCollapsed,
377 Stopped::Stalled => BlendStop::Stalled,
378 Stopped::LeftTheDomain => {
379 let left_first = at_edge(contact.first, (last[0], last[1]));
380 let left_second = at_edge(contact.second, (last[2], last[3]));
381 match (left_first, left_second) {
382 (true, true) => BlendStop::LeftBothSupports,
383 (true, false) => BlendStop::LeftTheFirstSupport,
384 (false, true) => BlendStop::LeftTheSecondSupport,
385 (false, false) => BlendStop::RanPastTheGuide,
386 }
387 }
388 }
389}
390
391#[allow(clippy::too_many_arguments, reason = "one construction, all its data")]
395pub(crate) fn seat_section(
396 first: &SurfaceGeometry,
397 second: &SurfaceGeometry,
398 radius: f64,
399 guide: &Curve,
400 sides: Sides,
401 at: f64,
402 near: [f64; 4],
403 tol: Tolerances,
404) -> OgeomResult<[f64; 5]> {
405 let (x, _) = seat(
406 first,
407 second,
408 radius,
409 None,
410 guide,
411 &[sides],
412 Some(at),
413 Some(near),
414 tol,
415 )?;
416 Ok(x)
417}
418
419#[allow(clippy::too_many_arguments, reason = "one construction, all its data")]
426fn seat(
427 first: &SurfaceGeometry,
428 second: &SurfaceGeometry,
429 radius: f64,
430 law: Option<&RadiusAlong>,
431 guide: &Curve,
432 candidates: &[Sides],
433 seed: Option<f64>,
434 near: Option<[f64; 4]>,
435 tol: Tolerances,
436) -> OgeomResult<([f64; 5], Sides)> {
437 let at = seed.unwrap_or_else(|| {
438 let (lo, hi) = guide.domain();
439 f64::midpoint(lo, hi)
440 });
441 let start = match near {
445 Some(uv) => [uv[0], uv[1], uv[2], uv[3], at],
446 None => {
447 let anchor = guide.point_at(at, tol)?;
448 let near_first = ogeom_algo::project_on_surface(first, anchor, 24, tol)?;
449 let near_second = ogeom_algo::project_on_surface(second, anchor, 24, tol)?;
450 [
451 near_first.parameters.0,
452 near_first.parameters.1,
453 near_second.parameters.0,
454 near_second.parameters.1,
455 at,
456 ]
457 }
458 };
459
460 for sides in candidates.iter().copied() {
461 let contact = BallContact {
462 first,
463 second,
464 radius,
465 law,
466 guide,
467 sides,
468 guide_loops: guide.is_periodic(),
471 };
472 let system = |x: &[f64; 4]| {
475 let mut full = [x[0], x[1], x[2], x[3], at];
476 contact.clamp(&mut full);
477 let mut residual = [0.0; 4];
478 let mut jacobian = [[0.0; 4]; 4];
479 if let Some((rows, matrix)) = contact.system(&full, tol) {
480 for (to, from) in residual.iter_mut().zip(&rows) {
481 *to = *from;
482 }
483 for (to, row) in jacobian.iter_mut().zip(&matrix) {
484 for (entry, from) in to.iter_mut().zip(row) {
485 *entry = *from;
486 }
487 }
488 }
489 (residual, jacobian)
490 };
491 let criteria = solve::Criteria {
492 residual: tol.confusion() * 0.01,
493 step: tol.parametric(),
494 max_iterations: 80,
495 };
496 let seed = [start[0], start[1], start[2], start[3]];
497 let Ok((found, norm, _, _)) = solve::newton_system_fixed(system, seed, criteria) else {
498 continue;
499 };
500 if norm > tol.confusion() {
501 continue;
502 }
503 let mut x = [found[0], found[1], found[2], found[3], at];
504 contact.clamp(&mut x);
505 let (Ok(p1), Ok(p2)) = (
506 first.point_at(x[0], x[1], tol),
507 second.point_at(x[2], x[3], tol),
508 ) else {
509 continue;
510 };
511 if p1.distance(p2) <= radius * 1e-3 {
513 continue;
514 }
515 let Some(centre) = contact.centre(&x, tol) else {
516 continue;
517 };
518 let held = contact.radius_at(at, tol)?.0;
519 if (centre.distance(p1) - held).abs() > tol.confusion() * 10.0
520 || (centre.distance(p2) - held).abs() > tol.confusion() * 10.0
521 {
522 continue;
523 }
524 return Ok((x, sides));
525 }
526 ogeom_bail!(
527 NotDone,
528 "no ball of radius {radius} seats between these supports at the \
529 guide's start; either the corner cannot hold one or the guide does \
530 not run along the seat"
531 );
532}
533
534struct BallContact<'s> {
536 first: &'s SurfaceGeometry,
537 second: &'s SurfaceGeometry,
538 radius: f64,
539 law: Option<&'s RadiusAlong>,
541 guide: &'s Curve,
542 sides: Sides,
543 guide_loops: bool,
548}
549
550impl BallContact<'_> {
551 fn centre(&self, x: &[f64], tol: Tolerances) -> Option<Point> {
553 let p = self.first.point_at(x[0], x[1], tol).ok()?;
554 let n = unit_normal(self.first, x[0], x[1], tol)?;
555 let radius = match self.law {
556 None => self.radius,
557 Some(_) => self.radius_at(self.wrapped(x[4]), tol).ok()?.0,
558 };
559 Some(p + n * (f64::from(self.sides.first) * radius))
560 }
561
562 fn radius_at(&self, t: f64, tol: Tolerances) -> OgeomResult<(f64, f64)> {
564 match self.law {
565 None => Ok((self.radius, 0.0)),
566 Some(law) => law.at(t, tol),
567 }
568 }
569
570 fn wrapped(&self, t: f64) -> f64 {
572 let (lo, hi) = self.guide.domain();
573 if self.guide_loops && hi > lo {
574 lo + (t - lo).rem_euclid(hi - lo)
575 } else {
576 t
577 }
578 }
579}
580
581impl Condition for BallContact<'_> {
582 fn unknowns(&self) -> usize {
583 5
584 }
585
586 fn position(&self, x: &[f64], tol: Tolerances) -> Option<Point> {
587 self.first.point_at(x[0], x[1], tol).ok()
590 }
591
592 fn position_gradient(&self, x: &[f64], tol: Tolerances) -> Option<Vec<Vector>> {
593 let (du, dv) = self.first.d1_at(x[0], x[1], tol).ok()?;
594 Some(vec![du, dv, Vector::ZERO, Vector::ZERO, Vector::ZERO])
595 }
596
597 fn system(&self, x: &[f64], tol: Tolerances) -> Option<(Vec<f64>, Vec<Vec<f64>>)> {
598 Some(self.system_at(x, tol)?.0)
599 }
600
601 fn system_at(
602 &self,
603 x: &[f64],
604 tol: Tolerances,
605 ) -> Option<((Vec<f64>, Vec<Vec<f64>>), Point, Vec<Vector>)> {
606 let first = self.first.jet_at(x[0], x[1], tol).ok()?;
609 let second = self.second.jet_at(x[2], x[3], tol).ok()?;
610 let (p1, a1, b1) = (first.point, first.du, first.dv);
611 let (p2, a2, b2) = (second.point, second.du, second.dv);
612 let (n1, dn1u, dn1v) = normal_and_derivatives_of(&first, tol)?;
613 let (n2, dn2u, dn2v) = normal_and_derivatives_of(&second, tol)?;
614 let w = self.wrapped(x[4]);
615 let (radius, rate) = self.radius_at(w, tol).ok()?;
616 let (s1, s2) = (
617 f64::from(self.sides.first) * radius,
618 f64::from(self.sides.second) * radius,
619 );
620 let lean = match self.law {
623 None => [0.0; 3],
624 Some(_) => {
625 let d = n1 * (f64::from(self.sides.first) * rate)
626 - n2 * (f64::from(self.sides.second) * rate);
627 [d.x, d.y, d.z]
628 }
629 };
630
631 let gap = (p1 + n1 * s1) - (p2 + n2 * s2);
633 let c1u = a1 + dn1u * s1;
634 let c1v = b1 + dn1v * s1;
635 let c2u = a2 + dn2u * s2;
636 let c2v = b2 + dn2v * s2;
637
638 let derivatives = self.guide.derivatives_at(w, 2, tol).ok()?;
646 let g = Point::ORIGIN + derivatives[0];
647 let (gd, gdd) = (derivatives[1], *derivatives.get(2).unwrap_or(&Vector::ZERO));
648 let square = (p1 - g).dot(gd);
649
650 Some((
651 (
652 vec![gap.x, gap.y, gap.z, square],
653 vec![
654 vec![c1u.x, c1v.x, -c2u.x, -c2v.x, lean[0]],
655 vec![c1u.y, c1v.y, -c2u.y, -c2v.y, lean[1]],
656 vec![c1u.z, c1v.z, -c2u.z, -c2v.z, lean[2]],
657 vec![
658 a1.dot(gd),
659 b1.dot(gd),
660 0.0,
661 0.0,
662 (p1 - g).dot(gdd) - gd.dot(gd),
663 ],
664 ],
665 ),
666 p1,
667 vec![a1, b1, Vector::ZERO, Vector::ZERO, Vector::ZERO],
668 ))
669 }
670
671 fn tangent_from(
672 &self,
673 _x: &[f64],
674 jacobian: &[Vec<f64>],
675 gradient: &[Vector],
676 tol: Tolerances,
677 ) -> Option<Vector> {
678 ogeom_intersect::walk::null_tangent(jacobian, gradient, tol)
681 }
682
683 fn clamp(&self, x: &mut [f64]) {
684 let (u1, v1) = clamp_to(self.first, x[0], x[1]);
685 let (u2, v2) = clamp_to(self.second, x[2], x[3]);
686 let (lo, hi) = self.guide.domain();
687 x[0] = u1;
688 x[1] = v1;
689 x[2] = u2;
690 x[3] = v2;
691 x[4] = if self.guide_loops && hi > lo {
695 lo + (x[4] - lo).rem_euclid(hi - lo)
696 } else {
697 x[4].clamp(lo, hi)
698 };
699 }
700
701 fn outside(&self, x: &[f64], tol: Tolerances) -> bool {
702 let (lo, hi) = self.guide.domain();
703 let band = tol.parametric();
704 beyond(self.first, (x[0], x[1]), tol)
705 || beyond(self.second, (x[2], x[3]), tol)
706 || (!self.guide_loops && (x[4] < lo - band || x[4] > hi + band))
707 }
708
709 fn near_edge(&self, x: &[f64]) -> bool {
710 let (lo, hi) = self.guide.domain();
711 let reach = (hi - lo) * 1e-6;
712 at_edge(self.first, (x[0], x[1]))
713 || at_edge(self.second, (x[2], x[3]))
714 || (!self.guide_loops && (x[4] <= lo + reach || x[4] >= hi - reach))
715 }
716
717 fn extent(&self) -> f64 {
718 let (lo, hi) = self.guide.domain();
722 let mut length = 0.0;
723 let mut previous = None;
724 for i in 0..=16 {
725 let t = (hi - lo).mul_add(f64::from(i) / 16.0, lo);
726 let Ok(p) = self.guide.point_at(t, Tolerances::millimetres()) else {
727 continue;
728 };
729 if let Some(last) = previous {
730 length += p.distance(last);
731 }
732 previous = Some(p);
733 }
734 length.max(self.radius * 8.0)
735 }
736}
737
738pub(crate) fn unit_normal(
740 surface: &SurfaceGeometry,
741 u: f64,
742 v: f64,
743 tol: Tolerances,
744) -> Option<Vector> {
745 let (du, dv) = surface.d1_at(u, v, tol).ok()?;
746 let cross = du.cross(dv);
747 let length = cross.magnitude();
748 if length <= tol.confusion() {
749 return None;
750 }
751 Some(cross / length)
752}
753
754fn normal_and_derivatives_of(
760 jet: &ogeom_geom::SurfaceJet,
761 tol: Tolerances,
762) -> Option<(Vector, Vector, Vector)> {
763 let (su, sv) = (jet.du, jet.dv);
764 let (suu, suv, svv) = (jet.d2u, jet.duv, jet.d2v);
765 let cross = su.cross(sv);
766 let length = cross.magnitude();
767 if length <= tol.confusion() {
768 return None;
769 }
770 let n = cross / length;
771 let dcu = suu.cross(sv) + su.cross(suv);
772 let dcv = suv.cross(sv) + su.cross(svv);
773 let across = |d: Vector| (d - n * d.dot(n)) / length;
774 Some((n, across(dcu), across(dcv)))
775}
776
777fn wraps(surface: &SurfaceGeometry) -> (bool, bool) {
782 let tol = Tolerances::millimetres();
783 (
784 surface.is_periodic_u() || surface.is_closed_u(tol),
785 surface.is_periodic_v() || surface.is_closed_v(tol),
786 )
787}
788
789fn clamp_to(surface: &SurfaceGeometry, u: f64, v: f64) -> (f64, f64) {
791 let ((ua, ub), (va, vb)) = surface.domain();
792 let (wrap_u, wrap_v) = wraps(surface);
793 let hold = |value: f64, lo: f64, hi: f64, periodic: bool| {
794 if periodic {
795 let span = hi - lo;
796 if span > 0.0 {
797 return lo + (value - lo).rem_euclid(span);
798 }
799 }
800 value.clamp(lo, hi)
801 };
802 (hold(u, ua, ub, wrap_u), hold(v, va, vb, wrap_v))
803}
804
805fn beyond(surface: &SurfaceGeometry, at: (f64, f64), tol: Tolerances) -> bool {
807 let ((ua, ub), (va, vb)) = surface.domain();
808 let (wrap_u, wrap_v) = wraps(surface);
809 let band = tol.parametric();
810 (!wrap_u && (at.0 < ua - band || at.0 > ub + band))
811 || (!wrap_v && (at.1 < va - band || at.1 > vb + band))
812}
813
814fn at_edge(surface: &SurfaceGeometry, at: (f64, f64)) -> bool {
817 let ((ua, ub), (va, vb)) = surface.domain();
818 let (wrap_u, wrap_v) = wraps(surface);
819 let near = |value: f64, lo: f64, hi: f64| {
820 let reach = (hi - lo) * 1e-6;
821 value <= lo + reach || value >= hi - reach
822 };
823 (!wrap_u && near(at.0, ua, ub)) || (!wrap_v && near(at.1, va, vb))
824}