1use geop_core_math::{
40 geop_error::{GeopError, GeopResult, WithContext},
41 scalars::Scalar,
42 vector::Vector3,
43};
44
45use geop_core_math::{
46 matrix::Matrix, solvers::linear::solve_linear_system, union_find::UnionFind, vector::Vector,
47};
48
49use crate::{nearest::SurfacePatch, nurb_surface::NurbSurface3D};
50
51const HANDOFF_PARTS: i64 = 64;
56
57const TANGENT_PARTS: i64 = 8;
62
63#[derive(Clone)]
66struct Piece {
67 patch: SurfacePatch,
68 normal: Vector3,
69}
70
71fn normal_directions(patch: &NurbSurface3D) -> Vector3 {
84 let (nu, nv) = (patch.num_u, patch.num_v);
85 let at = |i: usize, j: usize| patch.control_points[i * nv + j];
86 let origin =
87 Vector3::from_array([0, 1, 2].map(|k| at(0, 0)[k].div(at(0, 0)[3]).unwrap_or(at(0, 0)[k])))
88 .sharpen();
89 let local = |i: usize, j: usize| -> [Scalar; 4] {
91 let q = at(i, j);
92 [
93 q[0].sub(q[3].mul(origin[0])),
94 q[1].sub(q[3].mul(origin[1])),
95 q[2].sub(q[3].mul(origin[2])),
96 q[3],
97 ]
98 };
99 let union = |acc: Option<[Scalar; 4]>, x: [Scalar; 4]| -> Option<[Scalar; 4]> {
100 Some(match acc {
101 None => x,
102 Some(a) => std::array::from_fn(|k| a[k].union(x[k])),
103 })
104 };
105 let mut points: Option<[Scalar; 4]> = None;
106 for i in 0..nu {
107 for j in 0..nv {
108 let q = local(i, j);
109 let cartesian = [0, 1, 2].map(|k| q[k].div(q[3]).unwrap_or(Scalar::ENTIRE));
110 points = union(
111 points,
112 [cartesian[0], cartesian[1], cartesian[2], Scalar::ZERO],
113 );
114 }
115 }
116 let points = points.expect("a net has a point");
117 let hodograph = |along_u: bool| -> [Scalar; 4] {
118 let mut acc = None;
119 let (ni, nj) = if along_u { (nu - 1, nv) } else { (nu, nv - 1) };
120 for i in 0..ni {
121 for j in 0..nj {
122 let (a, b) = if along_u {
123 (local(i, j), local(i + 1, j))
124 } else {
125 (local(i, j), local(i, j + 1))
126 };
127 acc = union(acc, std::array::from_fn(|k| b[k].sub(a[k])));
128 }
129 }
130 acc.unwrap_or([Scalar::ZERO; 4])
131 };
132 let direction = |d: [Scalar; 4]| {
133 Vector3::from_array(std::array::from_fn(|k| d[k].sub(d[3].mul(points[k]))))
134 };
135 direction(hodograph(true)).prod_cross(&direction(hodograph(false)))
136}
137
138impl Piece {
139 fn new(surface: NurbSurface3D) -> GeopResult<Self> {
140 Ok(Self {
141 normal: normal_directions(&surface),
142 patch: SurfacePatch(surface),
143 })
144 }
145
146 fn size(&self) -> GeopResult<Scalar> {
147 self.patch.0.size()
148 }
149
150 fn halves(&self) -> GeopResult<[Piece; 2]> {
151 let (a, b) = self.patch.halves()?;
152 Ok([Piece::new(a.0)?, Piece::new(b.0)?])
153 }
154
155 fn middle(&self) -> GeopResult<[(Scalar, Scalar); 2]> {
157 let (u, v) = self.patch.middle()?;
158 let over = |(lo, hi): (Scalar, Scalar)| lo.union(hi);
159 Ok([
160 (u, over(self.patch.0.domain_u())),
161 (v, over(self.patch.0.domain_v())),
162 ])
163 }
164}
165
166fn spans(surface: &NurbSurface3D) -> GeopResult<Vec<Piece>> {
169 let mut rows = vec![surface.clone()];
170 for (along_u, breaks) in [
171 (true, surface.breakpoints_u()),
172 (false, surface.breakpoints_v()),
173 ] {
174 let inner = &breaks[1..breaks.len() - 1];
175 let mut cut = Vec::new();
176 for row in rows {
177 let mut rest = row;
178 for &t in inner {
179 let (lo, hi) = if along_u {
180 rest.split_u(t)?
181 } else {
182 rest.split_v(t)?
183 };
184 cut.push(lo);
185 rest = hi;
186 }
187 cut.push(rest);
188 }
189 rows = cut;
190 }
191 rows.into_iter().map(Piece::new).collect()
192}
193
194#[derive(Clone, Debug)]
196pub struct TurningPoints {
197 pub direction: Vector3,
200 pub candidates: Vec<Candidate>,
206 pub tangent: Vec<Candidate>,
211}
212
213#[derive(Clone, Debug)]
215pub struct Candidate {
216 pub seed: [Scalar; 4],
218 pub cluster: [Scalar; 4],
220}
221
222fn direction(a: &NurbSurface3D, b: &NurbSurface3D) -> Vector3 {
230 let normal = |s: &NurbSurface3D| -> Option<Vector3> {
231 let (u, v) = SurfacePatch(s.clone()).middle().ok()?;
232 s.normal(u, v).ok().map(|n| n.sharpen())
233 };
234 let axes = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
235 .map(|a| Vector3::from_array(a.map(Scalar::from_f64)));
236 let across = |n: &Vector3| -> Vector3 {
237 let least = axes
238 .iter()
239 .min_by(|x, y| {
240 let (dx, dy) = (n.prod_dot(x).abs(), n.prod_dot(y).abs());
241 dx.to_f64().total_cmp(&dy.to_f64())
242 })
243 .expect("three axes");
244 n.prod_cross(least)
245 };
246 let (na, nb) = (normal(a), normal(b));
247 let mut options: Vec<(f64, Vector3)> = Vec::new();
248 if let (Some(na), Some(nb)) = (&na, &nb) {
249 let d = na.prod_cross(nb);
250 options.push((2.0 * d.norm().to_f64(), d));
251 }
252 for n in [&na, &nb].into_iter().flatten() {
253 let d = across(n);
254 options.push((d.norm().to_f64(), d));
255 }
256 options
257 .into_iter()
258 .max_by(|x, y| x.0.total_cmp(&y.0))
259 .and_then(|(_, d)| d.normalize().ok())
260 .map(|d| d.sharpen())
261 .unwrap_or(axes[0])
262}
263
264pub fn turning_points(
270 a: &NurbSurface3D,
271 b: &NurbSurface3D,
272 max_nodes: usize,
273) -> GeopResult<TurningPoints> {
274 let ctx = |e: GeopError| e.with_context("turning_points(a, b)");
275 let d = direction(a, b);
276 let mut found = TurningPoints {
277 direction: d,
278 candidates: Vec::new(),
279 tangent: Vec::new(),
280 };
281 if !a.could_overlap(b) {
282 return Ok(found);
283 }
284 let size = a
285 .size()
286 .with_context(&ctx)?
287 .min(b.size().with_context(&ctx)?);
288 let parts = |n: i64| -> GeopResult<Scalar> { size.div(Scalar::from_i64(n)).map(|s| s.upper()) };
289 let (handoff, tangent_handoff) = (
290 parts(HANDOFF_PARTS).with_context(&ctx)?,
291 parts(TANGENT_PARTS).with_context(&ctx)?,
292 );
293
294 let (pieces_a, pieces_b) = (spans(a).with_context(&ctx)?, spans(b).with_context(&ctx)?);
295 let mut pending: Vec<(Piece, Piece)> = Vec::new();
296 for pa in &pieces_a {
297 for pb in &pieces_b {
298 pending.push((pa.clone(), pb.clone()));
299 }
300 }
301 let mut leaves: [Vec<([Scalar; 4], [Scalar; 4])>; 2] = [Vec::new(), Vec::new()];
303 let mut visited = 0usize;
304 while let Some((pa, pb)) = pending.pop() {
305 if !pa.patch.0.could_overlap(&pb.patch.0) {
306 continue;
307 }
308 let cross = pa.normal.prod_cross(&pb.normal);
309 if !cross.prod_dot(&d).could_be_equal(Scalar::ZERO) {
310 continue;
311 }
312 if visited >= max_nodes {
313 return Err(ctx(GeopError::new(format!(
314 "searched {max_nodes} pairs of patches with {} left, {} candidates so far: the search did not finish",
315 pending.len() + 1,
316 leaves[0].len()
317 ))));
318 }
319 visited += 1;
320 let regular = (0..3).any(|k| !cross[k].could_be_equal(Scalar::ZERO));
321 let limit = if regular { handoff } else { tangent_handoff };
322 let (size_a, size_b) = (pa.size()?, pb.size()?);
323 let (split_a, split_b) = (
324 size_a.definitely_greater(limit),
325 size_b.definitely_greater(limit),
326 );
327 if !split_a && !split_b {
328 let ([(ua, ua_box), (va, va_box)], [(ub, ub_box), (vb, vb_box)]) =
329 (pa.middle()?, pb.middle()?);
330 leaves[usize::from(!regular)]
331 .push(([ua, va, ub, vb], [ua_box, va_box, ub_box, vb_box]));
332 continue;
333 }
334 let cut_a = split_a && (!split_b || size_a.to_f64() >= size_b.to_f64());
335 if cut_a {
336 for half in pa.halves().with_context(&ctx)? {
337 pending.push((half, pb.clone()));
338 }
339 } else {
340 for half in pb.halves().with_context(&ctx)? {
341 pending.push((pa.clone(), half));
342 }
343 }
344 }
345
346 let [regular, tangent] = leaves;
347 found.candidates = clusters(®ular);
348 found.tangent = clusters(&tangent);
349 Ok(found)
350}
351
352fn clusters(leaves: &[([Scalar; 4], [Scalar; 4])]) -> Vec<Candidate> {
357 let mut sets = UnionFind::new(leaves.len());
358 for i in 0..leaves.len() {
359 for j in 0..i {
360 let touch = (0..4).all(|k| {
361 !leaves[i].1[k].definitely_less(leaves[j].1[k].lower())
362 && !leaves[i].1[k].definitely_greater(leaves[j].1[k].upper())
363 });
364 if touch {
365 sets.union(i, j);
366 }
367 }
368 }
369 let mut groups = sets.groups();
370 for group in &mut groups {
371 group.sort();
372 }
373 groups.sort_by_key(|g| g[0]);
374 groups
375 .into_iter()
376 .map(|members| {
377 let whole: [Scalar; 4] = std::array::from_fn(|k| {
378 members
379 .iter()
380 .map(|&i| leaves[i].1[k])
381 .reduce(Scalar::union)
382 .expect("a cluster has a member")
383 });
384 let middle = whole.map(|x| x.midpoint().to_f64());
385 let seed = members
386 .iter()
387 .map(|&i| leaves[i].0)
388 .min_by(|x, y| {
389 let dist = |s: &[Scalar; 4]| {
390 (0..4)
391 .map(|k| (s[k].to_f64() - middle[k]).powi(2))
392 .sum::<f64>()
393 };
394 dist(x).total_cmp(&dist(y))
395 })
396 .expect("a cluster has a member");
397 Candidate {
398 seed,
399 cluster: whole,
400 }
401 })
402 .collect()
403}
404
405const NODE_ITERATIONS: usize = 30;
407
408fn node_system(
413 a: &NurbSurface3D,
414 b: &NurbSurface3D,
415 [ua, va, ub, vb]: [Scalar; 4],
416 t: &[Vector3; 2],
417) -> GeopResult<([Scalar; 5], [[Scalar; 4]; 5])> {
418 let (pa, pb) = (a.evaluate(ua, va)?, b.evaluate(ub, vb)?);
419 let (sau, sav) = a.derivatives(ua, va)?;
420 let (sbu, sbv) = b.derivatives(ub, vb)?;
421 let [sauu, sauv, savv] = a.second_derivatives(ua, va)?;
422 let [sbuu, sbuv, sbvv] = b.second_derivatives(ub, vb)?;
423 let (na, nb) = (sau.prod_cross(&sav), sbu.prod_cross(&sbv));
424 let na_u = sauu.prod_cross(&sav).add(&sau.prod_cross(&sauv));
425 let na_v = sauv.prod_cross(&sav).add(&sau.prod_cross(&savv));
426 let nb_u = sbuu.prod_cross(&sbv).add(&sbu.prod_cross(&sbuv));
427 let nb_v = sbuv.prod_cross(&sbv).add(&sbu.prod_cross(&sbvv));
428 let c = na.prod_cross(&nb);
429 let dc = [
430 na_u.prod_cross(&nb),
431 na_v.prod_cross(&nb),
432 na.prod_cross(&nb_u),
433 na.prod_cross(&nb_v),
434 ];
435 let delta = pa.sub(&pb);
436 let mut f = [Scalar::ZERO; 5];
437 let mut j = [[Scalar::ZERO; 4]; 5];
438 for k in 0..3 {
439 f[k] = delta[k];
440 j[k] = [sau[k], sav[k], sbu[k].neg(), sbv[k].neg()];
441 }
442 for (row, tangent) in t.iter().enumerate() {
443 f[3 + row] = c.prod_dot(tangent);
444 j[3 + row] = dc.map(|d| d.prod_dot(tangent));
445 }
446 Ok((f, j))
447}
448
449fn node_step(f: &[Scalar; 5], j: &[[Scalar; 4]; 5], sharp: bool) -> GeopResult<[Scalar; 4]> {
454 let mut jtj = [[Scalar::ZERO; 4]; 4];
455 let mut jtf = [Scalar::ZERO; 4];
456 for r in 0..4 {
457 for c in 0..4 {
458 let sum = (0..5).fold(Scalar::ZERO, |sum, k| sum.add(j[k][r].mul(j[k][c])));
459 jtj[r][c] = if sharp { sum.sharpen() } else { sum };
460 }
461 jtf[r] = (0..5)
462 .fold(Scalar::ZERO, |sum, k| sum.add(j[k][r].mul(f[k])))
463 .neg();
464 }
465 let step = solve_linear_system(&Matrix::from_rows(jtj), &Vector::from_array(jtf))?;
466 Ok(step.to_array())
467}
468
469pub fn refine_node(
483 a: &NurbSurface3D,
484 b: &NurbSurface3D,
485 seed: [Scalar; 4],
486) -> Option<([Scalar; 4], Vector3)> {
487 let clamp = |x: [Scalar; 4]| -> [Scalar; 4] {
488 let domains = [a.domain_u(), a.domain_v(), b.domain_u(), b.domain_v()];
489 std::array::from_fn(|k| x[k].clamped(domains[k].0, domains[k].1))
490 };
491 let mut x = clamp(seed.map(Scalar::sharpen));
492 let tangents = |x: &[Scalar; 4]| -> Option<[Vector3; 2]> {
493 let (su, sv) = a.derivatives(x[0], x[1]).ok()?;
494 Some([su.sharpen(), sv.sharpen()])
495 };
496 let t = tangents(&x)?;
497 let mut answer = None;
498 for _ in 0..NODE_ITERATIONS {
499 let (f, j) = node_system(a, b, x, &t).ok()?;
500 let step = node_step(&f, &j, true).ok()?;
501 let honest = clamp(std::array::from_fn(|k| x[k].add(step[k])));
502 if f.iter().all(|r| r.could_be_equal(Scalar::ZERO)) {
503 answer = Some(honest);
504 break;
505 }
506 x = clamp(honest.map(Scalar::sharpen));
507 }
508 let x = answer?;
509 let (pa, pb) = (a.evaluate(x[0], x[1]).ok()?, b.evaluate(x[2], x[3]).ok()?);
510 if !pa.could_be_equal(&pb) {
511 return None;
512 }
513 let (f, j) = node_system(a, b, x, &t).ok()?;
514 if !f.iter().all(|r| r.could_be_equal(Scalar::ZERO)) {
515 return None;
516 }
517 node_step(&f, &j, false).ok()?;
518 Some((x, pa.union(&pb)))
519}
520
521#[cfg(test)]
522mod tests {
523 use super::*;
524 use crate::{nurb_surface::NurbSurface, test_fixtures::knots};
525 use geop_core_math::vector::Vector4;
526
527 fn h(x: f64, y: f64, z: f64) -> Vector4 {
528 Vector4::from_array([x, y, z, 1.0].map(Scalar::from_f64))
529 }
530
531 fn dome(top: f64) -> NurbSurface3D {
534 let mut cps = Vec::new();
535 for i in 0..3 {
536 for j in 0..3 {
537 let z = if i == 1 && j == 1 { top } else { 0.0 };
538 cps.push(h(i as f64 / 2.0, j as f64 / 2.0, z));
539 }
540 }
541 let k = knots(&[0., 0., 0., 1., 1., 1.]);
542 NurbSurface::try_new(2, 2, cps, k.clone(), k).unwrap()
543 }
544
545 fn level(z: f64) -> NurbSurface3D {
547 NurbSurface::try_new(
548 1,
549 1,
550 vec![h(-1., -1., z), h(-1., 2., z), h(2., -1., z), h(2., 2., z)],
551 knots(&[0., 0., 1., 1.]),
552 knots(&[0., 0., 1., 1.]),
553 )
554 .unwrap()
555 }
556
557 #[test]
562 fn a_dome_through_a_plane_turns_twice() {
563 let found = turning_points(&dome(2.4), &level(0.3), 20_000).unwrap();
564 assert_eq!(found.candidates.len(), 2, "{found:?}");
565 let found = turning_points(&dome(2.4), &level(0.9), 20_000).unwrap();
566 assert!(found.candidates.is_empty(), "{found:?}");
567 }
568
569 #[test]
571 fn crossing_planes_turn_nowhere() {
572 let tilted = NurbSurface::try_new(
573 1,
574 1,
575 vec![h(0., 0., -1.), h(0., 1., -1.), h(1., 0., 1.), h(1., 1., 1.)],
576 knots(&[0., 0., 1., 1.]),
577 knots(&[0., 0., 1., 1.]),
578 )
579 .unwrap();
580 let found = turning_points(&tilted, &level(0.0), 20_000).unwrap();
581 assert!(found.candidates.is_empty(), "{found:?}");
582 assert!(found.tangent.is_empty(), "{found:?}");
583 }
584
585 #[test]
587 fn a_planes_normal_is_sharp() {
588 let piece = Piece::new(level(0.0)).unwrap();
589 assert!(
590 (0..3).all(|k| piece.normal[k].width().to_f64() < 1e-12)
591 && piece.normal[2].definitely_greater(Scalar::ZERO),
592 "{:?}",
593 piece.normal
594 );
595 }
596
597 #[test]
603 fn crossing_cylinders_have_a_node() {
604 use crate::shape::Axis;
605 let v = |p: [f64; 3]| Vector3::from_array(p.map(Scalar::from_f64));
606 let pi = std::f64::consts::PI;
607 let around_z = NurbSurface3D::revolve(
608 &crate::test_fixtures::line([0.5, 0.0, -1.0], [0.5, 0.0, 1.0]),
609 &Axis::try_new(v([0.0, 0.0, 0.0]), v([0.0, 0.0, 1.0])).unwrap(),
610 pi / 4.0,
611 3.0 * pi / 4.0,
612 )
613 .unwrap();
614 let around_x = NurbSurface3D::revolve(
615 &crate::test_fixtures::line([-1.0, 0.5, 0.0], [1.0, 0.5, 0.0]),
616 &Axis::try_new(v([0.0, 0.0, 0.0]), v([1.0, 0.0, 0.0])).unwrap(),
617 -pi / 4.0,
618 pi / 4.0,
619 )
620 .unwrap();
621 let found = turning_points(&around_z, &around_x, 20_000).unwrap();
622 let nodes: Vec<Vector3> = found
623 .tangent
624 .iter()
625 .filter_map(|c| refine_node(&around_z, &around_x, c.seed))
626 .map(|(_, p)| p)
627 .collect();
628 assert_eq!(nodes.len(), 1, "{nodes:?} from {:?}", found.tangent);
629 assert!(nodes[0].could_be_equal(&v([0.0, 0.5, 0.0])), "{nodes:?}");
630 assert!(
631 (0..3).all(|k| nodes[0][k].width().to_f64() < 1e-9),
632 "{nodes:?}"
633 );
634 }
635}