1use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};
42use ogeom_geom::{Curve, Curve3d as _, Surface as _, SurfaceGeometry};
43use ogeom_intersect::walk::Condition;
44use ogeom_intersect::{Marching, Stopped};
45use ogeom_math::{Point, Vector, solve};
46
47#[derive(Debug, Clone, Copy, PartialEq, Eq)]
55pub struct Sides {
56 pub first: i8,
58 pub second: i8,
60}
61
62impl Sides {
63 const ALL: [Self; 4] = [
65 Self {
66 first: 1,
67 second: 1,
68 },
69 Self {
70 first: 1,
71 second: -1,
72 },
73 Self {
74 first: -1,
75 second: 1,
76 },
77 Self {
78 first: -1,
79 second: -1,
80 },
81 ];
82}
83
84#[derive(Debug, Clone, Copy, PartialEq, Eq)]
89pub enum BlendStop {
90 Closed,
92 LeftTheFirstSupport,
94 LeftTheSecondSupport,
96 LeftBothSupports,
98 RanPastTheGuide,
100 SectionCollapsed,
103 Stalled,
106 RanOut,
109}
110
111#[derive(Debug, Clone)]
113pub struct MarchedBlend {
114 pub spine: Vec<Point>,
116 pub on_first: Vec<(f64, f64)>,
122 pub on_second: Vec<(f64, f64)>,
124 pub touch_first: Vec<Point>,
126 pub touch_second: Vec<Point>,
128 pub along: Vec<f64>,
131 pub sides: Sides,
133 pub stopped: BlendStop,
135}
136
137impl MarchedBlend {
138 #[must_use]
140 pub fn len(&self) -> usize {
141 self.spine.len()
142 }
143
144 #[must_use]
146 pub fn is_empty(&self) -> bool {
147 self.spine.is_empty()
148 }
149
150 #[must_use]
152 pub const fn complete(&self) -> bool {
153 !matches!(self.stopped, BlendStop::RanOut)
154 }
155}
156
157pub fn march_blend(
175 first: &SurfaceGeometry,
176 second: &SurfaceGeometry,
177 radius: f64,
178 guide: &Curve,
179 options: Marching,
180 tol: Tolerances,
181) -> OgeomResult<MarchedBlend> {
182 march_blend_on(
183 first,
184 second,
185 radius,
186 guide,
187 &Sides::ALL,
188 None,
189 options,
190 tol,
191 )
192}
193
194pub fn march_blend_sided(
205 first: &SurfaceGeometry,
206 second: &SurfaceGeometry,
207 radius: f64,
208 guide: &Curve,
209 sides: Sides,
210 options: Marching,
211 tol: Tolerances,
212) -> OgeomResult<MarchedBlend> {
213 march_blend_on(first, second, radius, guide, &[sides], None, options, tol)
214}
215
216#[allow(clippy::too_many_arguments, reason = "one construction, all its data")]
229pub fn march_blend_seeded(
230 first: &SurfaceGeometry,
231 second: &SurfaceGeometry,
232 radius: f64,
233 guide: &Curve,
234 sides: Sides,
235 seed: f64,
236 options: Marching,
237 tol: Tolerances,
238) -> OgeomResult<MarchedBlend> {
239 march_blend_on(
240 first,
241 second,
242 radius,
243 guide,
244 &[sides],
245 Some(seed),
246 options,
247 tol,
248 )
249}
250
251#[allow(clippy::too_many_arguments, reason = "one construction, all its data")]
252fn march_blend_on(
253 first: &SurfaceGeometry,
254 second: &SurfaceGeometry,
255 radius: f64,
256 guide: &Curve,
257 candidates: &[Sides],
258 seed: Option<f64>,
259 options: Marching,
260 tol: Tolerances,
261) -> OgeomResult<MarchedBlend> {
262 if !radius.is_finite() || radius <= tol.confusion() {
263 ogeom_bail!(Construction, "a blend of radius {radius} rounds nothing");
264 }
265 options.validate()?;
266 let (start, sides) = seat(first, second, radius, guide, candidates, seed, None, tol)?;
267 let guide_loops = guide.is_periodic() || {
268 let (lo, hi) = guide.domain();
269 guide
270 .point_at(lo, tol)
271 .and_then(|p| guide.point_at(hi, tol).map(|q| p.distance(q)))
272 .is_ok_and(|d| d <= tol.confusion() * 10.0)
273 };
274 let contact = BallContact {
275 first,
276 second,
277 radius,
278 guide,
279 sides,
280 guide_loops,
281 };
282 let walked = ogeom_intersect::walk::follow(&contact, &start, options, tol)?;
283
284 let mut blend = MarchedBlend {
285 spine: Vec::with_capacity(walked.states.len()),
286 on_first: Vec::with_capacity(walked.states.len()),
287 on_second: Vec::with_capacity(walked.states.len()),
288 touch_first: Vec::with_capacity(walked.states.len()),
289 touch_second: Vec::with_capacity(walked.states.len()),
290 along: Vec::with_capacity(walked.states.len()),
291 sides,
292 stopped: BlendStop::Stalled,
293 };
294 for x in &walked.states {
295 let (Ok(p1), Ok(p2)) = (
296 first.point_at(x[0], x[1], tol),
297 second.point_at(x[2], x[3], tol),
298 ) else {
299 continue;
300 };
301 let Some(centre) = contact.centre(x, tol) else {
302 continue;
303 };
304 blend.spine.push(centre);
305 blend.on_first.push((x[0], x[1]));
306 blend.on_second.push((x[2], x[3]));
307 blend.touch_first.push(p1);
308 blend.touch_second.push(p2);
309 blend.along.push(x[4]);
310 }
311 blend.stopped = why(&contact, &walked, tol);
312 Ok(blend)
313}
314
315fn why(
317 contact: &BallContact<'_>,
318 walked: &ogeom_intersect::walk::Walked,
319 tol: Tolerances,
320) -> BlendStop {
321 let Some(last) = walked.states.last() else {
322 return BlendStop::Stalled;
323 };
324 let collapsed = matches!(
328 (
329 contact.first.point_at(last[0], last[1], tol),
330 contact.second.point_at(last[2], last[3], tol),
331 ),
332 (Ok(p1), Ok(p2)) if p1.distance(p2) <= contact.radius * 1e-3
333 );
334 match walked.stopped {
335 Stopped::Closed => BlendStop::Closed,
336 Stopped::RanOut => BlendStop::RanOut,
337 Stopped::Stalled if collapsed => BlendStop::SectionCollapsed,
338 Stopped::Stalled => BlendStop::Stalled,
339 Stopped::LeftTheDomain => {
340 let left_first = at_edge(contact.first, (last[0], last[1]));
341 let left_second = at_edge(contact.second, (last[2], last[3]));
342 match (left_first, left_second) {
343 (true, true) => BlendStop::LeftBothSupports,
344 (true, false) => BlendStop::LeftTheFirstSupport,
345 (false, true) => BlendStop::LeftTheSecondSupport,
346 (false, false) => BlendStop::RanPastTheGuide,
347 }
348 }
349 }
350}
351
352#[allow(clippy::too_many_arguments, reason = "one construction, all its data")]
356pub(crate) fn seat_section(
357 first: &SurfaceGeometry,
358 second: &SurfaceGeometry,
359 radius: f64,
360 guide: &Curve,
361 sides: Sides,
362 at: f64,
363 near: [f64; 4],
364 tol: Tolerances,
365) -> OgeomResult<[f64; 5]> {
366 let (x, _) = seat(
367 first,
368 second,
369 radius,
370 guide,
371 &[sides],
372 Some(at),
373 Some(near),
374 tol,
375 )?;
376 Ok(x)
377}
378
379#[allow(clippy::too_many_arguments, reason = "one construction, all its data")]
386fn seat(
387 first: &SurfaceGeometry,
388 second: &SurfaceGeometry,
389 radius: f64,
390 guide: &Curve,
391 candidates: &[Sides],
392 seed: Option<f64>,
393 near: Option<[f64; 4]>,
394 tol: Tolerances,
395) -> OgeomResult<([f64; 5], Sides)> {
396 let at = seed.unwrap_or_else(|| {
397 let (lo, hi) = guide.domain();
398 f64::midpoint(lo, hi)
399 });
400 let start = match near {
404 Some(uv) => [uv[0], uv[1], uv[2], uv[3], at],
405 None => {
406 let anchor = guide.point_at(at, tol)?;
407 let near_first = ogeom_algo::project_on_surface(first, anchor, 24, tol)?;
408 let near_second = ogeom_algo::project_on_surface(second, anchor, 24, tol)?;
409 [
410 near_first.parameters.0,
411 near_first.parameters.1,
412 near_second.parameters.0,
413 near_second.parameters.1,
414 at,
415 ]
416 }
417 };
418
419 for sides in candidates.iter().copied() {
420 let contact = BallContact {
421 first,
422 second,
423 radius,
424 guide,
425 sides,
426 guide_loops: guide.is_periodic(),
429 };
430 let system = |x: &[f64]| {
433 let mut full = [x[0], x[1], x[2], x[3], at];
434 contact.clamp(&mut full);
435 let (mut residual, jacobian) = contact
436 .system(&full, tol)
437 .unwrap_or_else(|| (vec![0.0; 4], vec![vec![0.0; 5]; 4]));
438 residual.truncate(4);
439 let jacobian = jacobian
440 .into_iter()
441 .take(4)
442 .map(|row| row[..4].to_vec())
443 .collect();
444 (residual, jacobian)
445 };
446 let criteria = solve::Criteria {
447 residual: tol.confusion() * 0.01,
448 step: tol.parametric(),
449 max_iterations: 80,
450 };
451 let Ok(found) = solve::newton_system(system, &start[..4], criteria) else {
452 continue;
453 };
454 if found.residual > tol.confusion() {
455 continue;
456 }
457 let mut x = [
458 found.value[0],
459 found.value[1],
460 found.value[2],
461 found.value[3],
462 at,
463 ];
464 contact.clamp(&mut x);
465 let (Ok(p1), Ok(p2)) = (
466 first.point_at(x[0], x[1], tol),
467 second.point_at(x[2], x[3], tol),
468 ) else {
469 continue;
470 };
471 if p1.distance(p2) <= radius * 1e-3 {
473 continue;
474 }
475 let Some(centre) = contact.centre(&x, tol) else {
476 continue;
477 };
478 if (centre.distance(p1) - radius).abs() > tol.confusion() * 10.0
479 || (centre.distance(p2) - radius).abs() > tol.confusion() * 10.0
480 {
481 continue;
482 }
483 return Ok((x, sides));
484 }
485 ogeom_bail!(
486 NotDone,
487 "no ball of radius {radius} seats between these supports at the \
488 guide's start; either the corner cannot hold one or the guide does \
489 not run along the seat"
490 );
491}
492
493struct BallContact<'s> {
495 first: &'s SurfaceGeometry,
496 second: &'s SurfaceGeometry,
497 radius: f64,
498 guide: &'s Curve,
499 sides: Sides,
500 guide_loops: bool,
505}
506
507impl BallContact<'_> {
508 fn centre(&self, x: &[f64], tol: Tolerances) -> Option<Point> {
510 let p = self.first.point_at(x[0], x[1], tol).ok()?;
511 let n = unit_normal(self.first, x[0], x[1], tol)?;
512 Some(p + n * (f64::from(self.sides.first) * self.radius))
513 }
514}
515
516impl Condition for BallContact<'_> {
517 fn unknowns(&self) -> usize {
518 5
519 }
520
521 fn position(&self, x: &[f64], tol: Tolerances) -> Option<Point> {
522 self.first.point_at(x[0], x[1], tol).ok()
525 }
526
527 fn position_gradient(&self, x: &[f64], tol: Tolerances) -> Option<Vec<Vector>> {
528 let (du, dv) = self.first.d1_at(x[0], x[1], tol).ok()?;
529 Some(vec![du, dv, Vector::ZERO, Vector::ZERO, Vector::ZERO])
530 }
531
532 fn system(&self, x: &[f64], tol: Tolerances) -> Option<(Vec<f64>, Vec<Vec<f64>>)> {
533 let p1 = self.first.point_at(x[0], x[1], tol).ok()?;
534 let p2 = self.second.point_at(x[2], x[3], tol).ok()?;
535 let (a1, b1) = self.first.d1_at(x[0], x[1], tol).ok()?;
536 let (a2, b2) = self.second.d1_at(x[2], x[3], tol).ok()?;
537 let (n1, dn1u, dn1v) = normal_and_derivatives(self.first, x[0], x[1], tol)?;
538 let (n2, dn2u, dn2v) = normal_and_derivatives(self.second, x[2], x[3], tol)?;
539 let (s1, s2) = (
540 f64::from(self.sides.first) * self.radius,
541 f64::from(self.sides.second) * self.radius,
542 );
543
544 let gap = (p1 + n1 * s1) - (p2 + n2 * s2);
546 let c1u = a1 + dn1u * s1;
547 let c1v = b1 + dn1v * s1;
548 let c2u = a2 + dn2u * s2;
549 let c2v = b2 + dn2v * s2;
550
551 let w = {
559 let (lo, hi) = self.guide.domain();
560 if self.guide_loops && hi > lo {
561 lo + (x[4] - lo).rem_euclid(hi - lo)
562 } else {
563 x[4]
564 }
565 };
566 let derivatives = self.guide.derivatives_at(w, 2, tol).ok()?;
567 let g = Point::ORIGIN + derivatives[0];
568 let (gd, gdd) = (derivatives[1], *derivatives.get(2).unwrap_or(&Vector::ZERO));
569 let square = (p1 - g).dot(gd);
570
571 Some((
572 vec![gap.x, gap.y, gap.z, square],
573 vec![
574 vec![c1u.x, c1v.x, -c2u.x, -c2v.x, 0.0],
575 vec![c1u.y, c1v.y, -c2u.y, -c2v.y, 0.0],
576 vec![c1u.z, c1v.z, -c2u.z, -c2v.z, 0.0],
577 vec![
578 a1.dot(gd),
579 b1.dot(gd),
580 0.0,
581 0.0,
582 (p1 - g).dot(gdd) - gd.dot(gd),
583 ],
584 ],
585 ))
586 }
587
588 fn clamp(&self, x: &mut [f64]) {
589 let (u1, v1) = clamp_to(self.first, x[0], x[1]);
590 let (u2, v2) = clamp_to(self.second, x[2], x[3]);
591 let (lo, hi) = self.guide.domain();
592 x[0] = u1;
593 x[1] = v1;
594 x[2] = u2;
595 x[3] = v2;
596 x[4] = if self.guide_loops && hi > lo {
600 lo + (x[4] - lo).rem_euclid(hi - lo)
601 } else {
602 x[4].clamp(lo, hi)
603 };
604 }
605
606 fn outside(&self, x: &[f64], tol: Tolerances) -> bool {
607 let (lo, hi) = self.guide.domain();
608 let band = tol.parametric();
609 beyond(self.first, (x[0], x[1]), tol)
610 || beyond(self.second, (x[2], x[3]), tol)
611 || (!self.guide_loops && (x[4] < lo - band || x[4] > hi + band))
612 }
613
614 fn near_edge(&self, x: &[f64]) -> bool {
615 let (lo, hi) = self.guide.domain();
616 let reach = (hi - lo) * 1e-6;
617 at_edge(self.first, (x[0], x[1]))
618 || at_edge(self.second, (x[2], x[3]))
619 || (!self.guide_loops && (x[4] <= lo + reach || x[4] >= hi - reach))
620 }
621
622 fn extent(&self) -> f64 {
623 let (lo, hi) = self.guide.domain();
627 let mut length = 0.0;
628 let mut previous = None;
629 for i in 0..=16 {
630 let t = (hi - lo).mul_add(f64::from(i) / 16.0, lo);
631 let Ok(p) = self.guide.point_at(t, Tolerances::millimetres()) else {
632 continue;
633 };
634 if let Some(last) = previous {
635 length += p.distance(last);
636 }
637 previous = Some(p);
638 }
639 length.max(self.radius * 8.0)
640 }
641}
642
643fn unit_normal(surface: &SurfaceGeometry, u: f64, v: f64, tol: Tolerances) -> Option<Vector> {
645 let (du, dv) = surface.d1_at(u, v, tol).ok()?;
646 let cross = du.cross(dv);
647 let length = cross.magnitude();
648 if length <= tol.confusion() {
649 return None;
650 }
651 Some(cross / length)
652}
653
654fn normal_and_derivatives(
660 surface: &SurfaceGeometry,
661 u: f64,
662 v: f64,
663 tol: Tolerances,
664) -> Option<(Vector, Vector, Vector)> {
665 let (su, sv) = surface.d1_at(u, v, tol).ok()?;
666 let (suu, suv, svv) = surface.d2_at(u, v, tol).ok()?;
667 let cross = su.cross(sv);
668 let length = cross.magnitude();
669 if length <= tol.confusion() {
670 return None;
671 }
672 let n = cross / length;
673 let dcu = suu.cross(sv) + su.cross(suv);
674 let dcv = suv.cross(sv) + su.cross(svv);
675 let across = |d: Vector| (d - n * d.dot(n)) / length;
676 Some((n, across(dcu), across(dcv)))
677}
678
679fn wraps(surface: &SurfaceGeometry) -> (bool, bool) {
684 let tol = Tolerances::millimetres();
685 (
686 surface.is_periodic_u() || surface.is_closed_u(tol),
687 surface.is_periodic_v() || surface.is_closed_v(tol),
688 )
689}
690
691fn clamp_to(surface: &SurfaceGeometry, u: f64, v: f64) -> (f64, f64) {
693 let ((ua, ub), (va, vb)) = surface.domain();
694 let (wrap_u, wrap_v) = wraps(surface);
695 let hold = |value: f64, lo: f64, hi: f64, periodic: bool| {
696 if periodic {
697 let span = hi - lo;
698 if span > 0.0 {
699 return lo + (value - lo).rem_euclid(span);
700 }
701 }
702 value.clamp(lo, hi)
703 };
704 (hold(u, ua, ub, wrap_u), hold(v, va, vb, wrap_v))
705}
706
707fn beyond(surface: &SurfaceGeometry, at: (f64, f64), tol: Tolerances) -> bool {
709 let ((ua, ub), (va, vb)) = surface.domain();
710 let (wrap_u, wrap_v) = wraps(surface);
711 let band = tol.parametric();
712 (!wrap_u && (at.0 < ua - band || at.0 > ub + band))
713 || (!wrap_v && (at.1 < va - band || at.1 > vb + band))
714}
715
716fn at_edge(surface: &SurfaceGeometry, at: (f64, f64)) -> bool {
719 let ((ua, ub), (va, vb)) = surface.domain();
720 let (wrap_u, wrap_v) = wraps(surface);
721 let near = |value: f64, lo: f64, hi: f64| {
722 let reach = (hi - lo) * 1e-6;
723 value <= lo + reach || value >= hi - reach
724 };
725 (!wrap_u && near(at.0, ua, ub)) || (!wrap_v && near(at.1, va, vb))
726}