1use crate::sketch::{CurveId, CurveKind, PointId, Positions, Sketch};
21use geop_core_geometry::{
22 intersection::curve_curve_intersect,
23 nurb_curve::{NurbCurve, NurbCurve2D},
24};
25use geop_core_math::{
26 geop_error::{GeopError, GeopResult, WithContext},
27 scalars::{Field, Ring, Scalar, scal_in_f64::ScalInF64},
28 vector::Vector3,
29 with_context,
30};
31use std::collections::BTreeMap;
32use std::f64::consts::{FRAC_PI_2, SQRT_2};
33
34type F = ScalInF64;
40
41const MAX_CROSSINGS: usize = 16;
45const MAX_NODES: usize = 4000;
46const MIN_SUBDIVISION: f64 = 1e-6;
47const MAX_RAY_ATTEMPTS: usize = 32;
49
50#[derive(Clone, Copy, Debug, PartialEq, Eq)]
52pub struct ProfileEdge {
53 pub curve: CurveId,
54 pub reversed: bool,
55}
56
57#[derive(Clone, Debug, PartialEq, Eq)]
59pub struct ProfileLoop {
60 pub edges: Vec<ProfileEdge>,
61}
62
63#[derive(Clone, Debug, PartialEq, Eq)]
66pub struct Region {
67 pub outer: ProfileLoop,
68 pub holes: Vec<ProfileLoop>,
69}
70
71const SAMPLES: usize = 16;
73
74impl Sketch {
75 pub fn regions(&self) -> GeopResult<Vec<Region>> {
77 self.validate()?;
78 let loops = self.loops()?;
79 if loops.is_empty() {
80 return Err(GeopError::new(
81 "sketch has no closed profile: its curves do not form a loop",
82 ));
83 }
84 let positions = self.positions();
85 let curves: Vec<Vec<NurbCurve2D<F>>> = loops
86 .iter()
87 .map(|l| {
88 Ok(l.to_nurbs::<F>(self, &positions)?
89 .into_iter()
90 .map(|p| p.curve)
91 .collect())
92 })
93 .collect::<GeopResult<_>>()?;
94 let extent = extent_of(&curves);
95 let counter_clockwise: Vec<bool> = curves
96 .iter()
97 .map(|c| turns_counter_clockwise(c, extent))
98 .collect::<GeopResult<_>>()?;
99
100 let mut containers: Vec<Vec<usize>> = Vec::with_capacity(loops.len());
104 for i in 0..loops.len() {
105 let probe = midpoint(&curves[i][0])?;
106 let mut inside = Vec::new();
107 for (j, other) in curves.iter().enumerate() {
108 if j != i && loop_contains(other, probe, extent)? {
109 inside.push(j);
110 }
111 }
112 containers.push(inside);
113 }
114
115 let mut regions: Vec<(usize, Region)> = Vec::new();
116 for i in (0..loops.len()).filter(|&i| containers[i].len().is_multiple_of(2)) {
117 let outer = if counter_clockwise[i] {
118 loops[i].clone()
119 } else {
120 loops[i].reversed()
121 };
122 regions.push((
123 i,
124 Region {
125 outer,
126 holes: Vec::new(),
127 },
128 ));
129 }
130 for i in (0..loops.len()).filter(|&i| !containers[i].len().is_multiple_of(2)) {
131 let depth = containers[i].len();
132 let parent = *containers[i]
133 .iter()
134 .find(|&&j| containers[j].len() == depth - 1)
135 .expect("an odd-depth loop lies directly inside an even-depth one");
136 let hole = if counter_clockwise[i] {
137 loops[i].reversed()
138 } else {
139 loops[i].clone()
140 };
141 regions
142 .iter_mut()
143 .find(|(j, _)| *j == parent)
144 .unwrap()
145 .1
146 .holes
147 .push(hole);
148 }
149 Ok(regions.into_iter().map(|(_, r)| r).collect())
150 }
151
152 fn loops(&self) -> GeopResult<Vec<ProfileLoop>> {
154 let class = self.point_classes();
155 let mut loops = Vec::new();
156 let mut edges: Vec<(CurveId, PointId, PointId)> = Vec::new();
159 for (&id, curve) in &self.curves {
160 if curve.construction {
161 continue;
162 }
163 match curve.endpoints() {
164 None => loops.push(ProfileLoop {
165 edges: vec![ProfileEdge {
166 curve: id,
167 reversed: false,
168 }],
169 }),
170 Some((s, e)) if class[&s] == class[&e] => {
171 if !matches!(curve.kind, CurveKind::Spline { .. }) {
172 return Err(GeopError::new(format!(
173 "curve {id} starts and ends at the same point"
174 )));
175 }
176 loops.push(ProfileLoop {
177 edges: vec![ProfileEdge {
178 curve: id,
179 reversed: false,
180 }],
181 });
182 }
183 Some((s, e)) => edges.push((id, class[&s], class[&e])),
184 }
185 }
186
187 let mut alive = vec![true; edges.len()];
189 let mut degree: BTreeMap<PointId, usize> = BTreeMap::new();
190 for &(_, a, b) in &edges {
191 *degree.entry(a).or_default() += 1;
192 *degree.entry(b).or_default() += 1;
193 }
194 loop {
195 let mut changed = false;
196 for (k, &(_, a, b)) in edges.iter().enumerate() {
197 if alive[k] && (degree[&a] == 1 || degree[&b] == 1) {
198 alive[k] = false;
199 *degree.get_mut(&a).unwrap() -= 1;
200 *degree.get_mut(&b).unwrap() -= 1;
201 changed = true;
202 }
203 }
204 if !changed {
205 break;
206 }
207 }
208 if let Some((p, d)) = degree.iter().find(|(_, d)| **d > 2) {
209 return Err(GeopError::new(format!(
210 "profile curves branch at point {p}: {d} curves meet there"
211 )));
212 }
213
214 let mut used = vec![false; edges.len()];
216 for start in 0..edges.len() {
217 if !alive[start] || used[start] {
218 continue;
219 }
220 let mut lp = Vec::new();
221 let (mut k, mut at_end) = (start, false);
222 loop {
223 used[k] = true;
224 let (id, a, b) = edges[k];
225 lp.push(ProfileEdge {
226 curve: id,
227 reversed: at_end,
228 });
229 let next_point = if at_end { a } else { b };
230 let Some(next) = (0..edges.len()).find(|&j| {
231 alive[j] && !used[j] && (edges[j].1 == next_point || edges[j].2 == next_point)
232 }) else {
233 break;
234 };
235 at_end = edges[next].2 == next_point;
236 k = next;
237 }
238 loops.push(ProfileLoop { edges: lp });
239 }
240 Ok(loops)
241 }
242}
243
244#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, PartialOrd, Ord)]
255pub enum ProfileJoint {
256 Point(PointId),
257 Split { curve: CurveId, index: usize },
258}
259
260impl std::fmt::Display for ProfileJoint {
262 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
263 match self {
264 ProfileJoint::Point(p) => write!(f, "{p}"),
265 ProfileJoint::Split { curve, index } => write!(f, "{curve}@{index}"),
266 }
267 }
268}
269
270#[derive(Clone, Debug)]
275pub struct ProfilePiece<S: Scalar> {
276 pub curve: NurbCurve2D<S>,
277 pub source: CurveId,
278 pub index: usize,
279 pub start: ProfileJoint,
280 pub end: ProfileJoint,
281}
282
283impl<S: Scalar> ProfilePiece<S> {
284 pub fn name(&self) -> String {
287 if self.index == 0 {
288 format!("{}", self.source)
289 } else {
290 format!("{}#{}", self.source, self.index)
291 }
292 }
293}
294
295impl ProfileLoop {
296 pub fn reversed(&self) -> ProfileLoop {
298 ProfileLoop {
299 edges: self
300 .edges
301 .iter()
302 .rev()
303 .map(|e| ProfileEdge {
304 curve: e.curve,
305 reversed: !e.reversed,
306 })
307 .collect(),
308 }
309 }
310
311 pub fn to_nurbs<S: Scalar>(
326 &self,
327 sketch: &Sketch,
328 positions: &Positions,
329 ) -> GeopResult<Vec<ProfilePiece<S>>> {
330 let mut out = Vec::new();
331 for edge in &self.edges {
332 let curve = edge.curve;
333 let ctx = with_context!("converting sketch curve {curve} to NURBS");
334 let pieces = edge_pieces(sketch, positions, curve).with_context(ctx)?;
335 let (first, last) = curve_joints(sketch, curve).with_context(ctx)?;
336 let n = pieces.len();
337 let joint = |j: usize| match j {
340 0 => first,
341 j if j == n => last,
342 index => ProfileJoint::Split { curve, index },
343 };
344 let pieces = pieces
345 .into_iter()
346 .enumerate()
347 .map(|(index, c)| ProfilePiece {
348 curve: c,
349 source: curve,
350 index,
351 start: joint(index),
352 end: joint(index + 1),
353 });
354 if edge.reversed {
355 out.extend(pieces.rev().map(|p| ProfilePiece {
356 curve: p.curve.reverse(),
357 start: p.end,
358 end: p.start,
359 ..p
360 }));
361 } else {
362 out.extend(pieces);
363 }
364 }
365 if let [only] = &out[..] {
366 let (a, b) = only.curve.split(S::from_f64(0.5))?;
369 let middle = ProfileJoint::Split {
370 curve: only.source,
371 index: 1,
372 };
373 let (first, second) = if self.edges[0].reversed {
376 (1, 0)
377 } else {
378 (0, 1)
379 };
380 out = vec![
381 ProfilePiece {
382 curve: rescale_to_unit(a)?,
383 source: only.source,
384 index: first,
385 start: only.start,
386 end: middle,
387 },
388 ProfilePiece {
389 curve: rescale_to_unit(b)?,
390 source: only.source,
391 index: second,
392 start: middle,
393 end: only.end,
394 },
395 ];
396 }
397 Ok(out)
398 }
399
400 pub fn polyline(&self, sketch: &Sketch, positions: &Positions) -> Vec<[f64; 2]> {
402 let mut out = Vec::new();
403 for edge in &self.edges {
404 let mut pts = curve_polyline(sketch, positions, edge.curve);
405 if edge.reversed {
406 pts.reverse();
407 }
408 pts.pop();
410 out.extend(pts);
411 }
412 out
413 }
414}
415
416struct PlainArc {
424 s: [f64; 2],
425 e: [f64; 2],
426 half: f64,
427}
428
429impl PlainArc {
430 fn chord(&self) -> [f64; 2] {
431 [self.e[0] - self.s[0], self.e[1] - self.s[1]]
432 }
433 fn chord_length(&self) -> f64 {
434 let c = self.chord();
435 c[0].hypot(c[1])
436 }
437 fn chord_mid(&self) -> [f64; 2] {
438 [(self.s[0] + self.e[0]) * 0.5, (self.s[1] + self.e[1]) * 0.5]
439 }
440 fn left(&self) -> [f64; 2] {
442 let c = self.chord();
443 let n = self.chord_length();
444 [-c[1] / n, c[0] / n]
445 }
446 fn center(&self) -> [f64; 2] {
447 let d = self.chord_length() * 0.5 * self.half.cos() / self.half.sin();
448 let (m, l) = (self.chord_mid(), self.left());
449 [m[0] + l[0] * d, m[1] + l[1] * d]
450 }
451 fn radius(&self) -> f64 {
453 self.chord_length() / (2.0 * self.half.sin().abs())
454 }
455}
456
457fn pos(positions: &Positions, p: PointId) -> [f64; 2] {
458 positions[&p]
459}
460
461fn curve_joints(sketch: &Sketch, curve: CurveId) -> GeopResult<(ProfileJoint, ProfileJoint)> {
464 Ok(match sketch.curve(curve)?.endpoints() {
465 Some((s, e)) => (ProfileJoint::Point(s), ProfileJoint::Point(e)),
466 None => {
467 let seam = ProfileJoint::Split { curve, index: 0 };
468 (seam, seam)
469 }
470 })
471}
472
473fn arc_of(positions: &Positions, start: PointId, end: PointId, sweep: f64) -> PlainArc {
474 PlainArc {
475 s: pos(positions, start),
476 e: pos(positions, end),
477 half: sweep / 2.0,
478 }
479}
480
481pub fn curve_polyline(sketch: &Sketch, positions: &Positions, curve: CurveId) -> Vec<[f64; 2]> {
484 match &sketch.curves[&curve].kind {
485 CurveKind::Line { start, end } => vec![positions[start], positions[end]],
486 CurveKind::Arc { start, end, sweep } => {
487 let arc = arc_of(positions, *start, *end, *sweep);
488 if *sweep == 0.0 {
489 return vec![positions[start], positions[end]];
490 }
491 let c = arc.center();
492 let r = arc.radius();
493 let a0 = (arc.s[1] - c[1]).atan2(arc.s[0] - c[0]);
494 let n = SAMPLES * (1 + (sweep.abs() / FRAC_PI_2) as usize);
495 let mut pts: Vec<[f64; 2]> = (0..=n)
496 .map(|i| {
497 let a = a0 + sweep * i as f64 / n as f64;
498 [c[0] + r * a.cos(), c[1] + r * a.sin()]
499 })
500 .collect();
501 pts[0] = positions[start];
502 pts[n] = positions[end];
503 pts
504 }
505 CurveKind::Circle { center, radius } => {
506 let c = positions[center];
507 let n = 4 * SAMPLES;
508 (0..=n)
509 .map(|i| {
510 let a = std::f64::consts::TAU * i as f64 / n as f64;
511 [c[0] + radius * a.cos(), c[1] + radius * a.sin()]
512 })
513 .collect()
514 }
515 CurveKind::Spline { control_points } => {
516 let cps: Vec<[f64; 2]> = control_points.iter().map(|p| positions[p]).collect();
517 let n = 2 * SAMPLES * cps.len();
518 (0..=n)
519 .map(|i| bspline_point(&cps, i as f64 / n as f64))
520 .collect()
521 }
522 }
523}
524
525fn spline_degree(n: usize) -> usize {
527 3.min(n - 1)
528}
529
530fn spline_knots(n: usize, degree: usize) -> Vec<f64> {
532 let spans = n - degree;
533 let mut knots = vec![0.0; degree + 1];
534 knots.extend((1..spans).map(|i| i as f64 / spans as f64));
535 knots.extend(std::iter::repeat_n(1.0, degree + 1));
536 knots
537}
538
539fn bspline_point(cps: &[[f64; 2]], t: f64) -> [f64; 2] {
541 let p = spline_degree(cps.len());
542 let knots = spline_knots(cps.len(), p);
543 let k = (p..cps.len()).rev().find(|&k| knots[k] <= t).unwrap_or(p);
545 let mut d: Vec<[f64; 2]> = (0..=p).map(|j| cps[j + k - p]).collect();
546 for r in 1..=p {
547 for j in (r..=p).rev() {
548 let i = j + k - p;
549 let denom = knots[i + p + 1 - r] - knots[i];
550 let alpha = if denom == 0.0 {
551 0.0
552 } else {
553 (t - knots[i]) / denom
554 };
555 d[j] = [
556 (1.0 - alpha) * d[j - 1][0] + alpha * d[j][0],
557 (1.0 - alpha) * d[j - 1][1] + alpha * d[j][1],
558 ];
559 }
560 }
561 d[p]
562}
563
564fn hom<S: Scalar>(p: [f64; 2], w: f64) -> Vector3<S> {
566 Vector3::from_array([S::from_f64(p[0] * w), S::from_f64(p[1] * w), S::from_f64(w)])
567}
568
569fn unit_knots<S: Scalar>(knots: &[f64]) -> Vec<S> {
570 knots.iter().map(|&k| S::from_f64(k)).collect()
571}
572
573fn conic<S: Scalar>(p0: [f64; 2], m: [f64; 2], p2: [f64; 2], w: f64) -> GeopResult<NurbCurve2D<S>> {
576 NurbCurve::try_new(
577 2,
578 vec![hom(p0, 1.0), hom(m, w), hom(p2, 1.0)],
579 unit_knots(&[0.0, 0.0, 0.0, 1.0, 1.0, 1.0]),
580 )
581}
582
583fn line<S: Scalar>(p0: [f64; 2], p1: [f64; 2]) -> GeopResult<NurbCurve2D<S>> {
584 NurbCurve::try_new(
585 1,
586 vec![hom(p0, 1.0), hom(p1, 1.0)],
587 unit_knots(&[0.0, 0.0, 1.0, 1.0]),
588 )
589}
590
591fn edge_pieces<S: Scalar>(
593 sketch: &Sketch,
594 positions: &Positions,
595 curve: CurveId,
596) -> GeopResult<Vec<NurbCurve2D<S>>> {
597 match &sketch.curve(curve)?.kind {
598 CurveKind::Line { start, end } => Ok(vec![line(positions[start], positions[end])?]),
599 CurveKind::Arc { start, end, sweep } => {
600 let (s, e) = (positions[start], positions[end]);
601 if *sweep == 0.0 {
602 return Ok(vec![line(s, e)?]);
603 }
604 let arc = arc_of(positions, *start, *end, *sweep);
605 let pieces = (sweep.abs() / FRAC_PI_2).ceil().max(1.0) as usize;
606 let delta = sweep / pieces as f64;
607 let mut ends = vec![s];
611 if pieces > 1 {
612 let c = arc.center();
613 let r = arc.radius();
614 let a0 = (s[1] - c[1]).atan2(s[0] - c[0]);
615 ends.extend((1..pieces).map(|j| {
616 let a = a0 + delta * j as f64;
617 [c[0] + r * a.cos(), c[1] + r * a.sin()]
618 }));
619 }
620 ends.push(e);
621 ends.windows(2)
622 .map(|w| {
623 let piece = PlainArc {
628 s: w[0],
629 e: w[1],
630 half: delta / 2.0,
631 };
632 let bulge = piece.chord_length() * 0.5 * (delta / 2.0).tan();
633 let (cm, l) = (piece.chord_mid(), piece.left());
634 let m = [cm[0] - l[0] * bulge, cm[1] - l[1] * bulge];
635 conic(w[0], m, w[1], (delta / 2.0).cos())
636 })
637 .collect()
638 }
639 CurveKind::Circle { center, radius } => {
640 let [cx, cy] = positions[center];
641 let r = *radius;
642 let q = [[cx + r, cy], [cx, cy + r], [cx - r, cy], [cx, cy - r]];
643 let corners = [
644 [cx + r, cy + r],
645 [cx - r, cy + r],
646 [cx - r, cy - r],
647 [cx + r, cy - r],
648 ];
649 (0..4)
650 .map(|j| conic(q[j], corners[j], q[(j + 1) % 4], SQRT_2 / 2.0))
651 .collect()
652 }
653 CurveKind::Spline { control_points } => {
654 let n = control_points.len();
655 let degree = spline_degree(n);
656 Ok(vec![NurbCurve::try_new(
657 degree,
658 control_points
659 .iter()
660 .map(|p| hom(positions[p], 1.0))
661 .collect(),
662 unit_knots(&spline_knots(n, degree)),
663 )?])
664 }
665 }
666}
667
668fn rescale_to_unit<S: Scalar>(curve: NurbCurve2D<S>) -> GeopResult<NurbCurve2D<S>> {
670 let (t0, t1) = curve.domain();
671 let span = t1.sub(t0);
672 let knots = curve
673 .knot_vector
674 .iter()
675 .map(|&k| k.sub(t0).div(span))
676 .collect::<GeopResult<Vec<S>>>()?;
677 NurbCurve::try_new(curve.degree, curve.control_points, knots)
678}
679
680fn extent_of(loops: &[Vec<NurbCurve2D<F>>]) -> f64 {
686 let points: Vec<[f64; 2]> = loops
687 .iter()
688 .flatten()
689 .flat_map(|c| {
690 c.control_points.iter().map(|cp| {
691 let w = cp[2].to_f64();
692 [cp[0].to_f64() / w, cp[1].to_f64() / w]
693 })
694 })
695 .collect();
696 let span = |k: usize| {
697 let (lo, hi) = points
698 .iter()
699 .fold((f64::INFINITY, f64::NEG_INFINITY), |(lo, hi), p| {
700 (lo.min(p[k]), hi.max(p[k]))
701 });
702 hi - lo
703 };
704 span(0).hypot(span(1)).max(1e-9)
705}
706
707fn midpoint(curve: &NurbCurve2D<F>) -> GeopResult<[f64; 2]> {
709 let (t0, t1) = curve.domain();
710 let p = curve.evaluate(t0.add(t1).div(F::TWO)?)?;
711 Ok([p[0].to_f64(), p[1].to_f64()])
712}
713
714fn ray(from: [f64; 2], dir: [f64; 2], extent: f64) -> GeopResult<NurbCurve2D<F>> {
717 let length = 3.0 * extent;
718 let to = [from[0] + dir[0] * length, from[1] + dir[1] * length];
719 NurbCurve::try_new(
720 1,
721 vec![hom::<F>(from, 1.0), hom::<F>(to, 1.0)],
722 unit_knots::<F>(&[0.0, 0.0, 1.0, 1.0]),
723 )
724}
725
726fn loop_contains(curves: &[NurbCurve2D<F>], probe: [f64; 2], extent: f64) -> GeopResult<bool> {
737 let min_subdivision = F::from_f64(MIN_SUBDIVISION);
738 let mut last_rejection = String::new();
739 'attempt: for k in 0..MAX_RAY_ATTEMPTS {
740 let angle = k as f64 * 2.399_963_229_728_653;
741 let ray = ray(probe, [angle.cos(), angle.sin()], extent)?;
742 let mut crossings = 0usize;
743 for curve in curves {
744 let hits =
745 match curve_curve_intersect(&ray, curve, MAX_CROSSINGS, MAX_NODES, min_subdivision)
746 {
747 Ok(hits) if !hits.is_coincident() => hits.into_vec(),
748 Ok(_) => {
749 last_rejection = format!("the ray runs along {curve:?}");
750 continue 'attempt;
751 }
752 Err(e) => {
753 last_rejection =
754 format!("the ray against {curve:?} did not converge: {e:?}");
755 continue 'attempt;
756 }
757 };
758 let (t0, t1) = curve.domain();
759 for (along_ray, along_curve) in hits {
760 if !along_ray.definitely_greater(F::ZERO) {
761 continue;
765 }
766 let from_start = along_curve.sub(t0).abs();
767 let from_end = along_curve.sub(t1).abs();
768 if !from_start.definitely_greater(min_subdivision)
769 || !from_end.definitely_greater(min_subdivision)
770 {
771 last_rejection =
772 format!("the ray grazes an end of {curve:?} at {along_curve:?}");
773 continue 'attempt;
774 }
775 crossings += 1;
776 }
777 }
778 return Ok(!crossings.is_multiple_of(2));
779 }
780 Err(GeopError::new(format!(
781 "could not classify {probe:?} against a loop: every ray direction was ambiguous, \
782 the last because {last_rejection}"
783 )))
784}
785
786fn turns_counter_clockwise(curves: &[NurbCurve2D<F>], extent: f64) -> GeopResult<bool> {
795 let curve = &curves[0];
796 let (t0, t1) = curve.domain();
797 let mid = t0.add(t1).div(F::TWO)?;
798 let point = curve.evaluate(mid)?;
799 let tangent = curve.tangent(mid)?;
800 let left = [F::ZERO.sub(tangent[1]).to_f64(), tangent[0].to_f64()];
801 let mut step = extent / 64.0;
802 for _ in 0..24 {
803 let at = |sign: f64| {
804 [
805 point[0].to_f64() + left[0] * step * sign,
806 point[1].to_f64() + left[1] * step * sign,
807 ]
808 };
809 let inside_left = loop_contains(curves, at(1.0), extent)?;
810 let inside_right = loop_contains(curves, at(-1.0), extent)?;
811 if inside_left != inside_right {
812 return Ok(inside_left);
813 }
814 step /= 2.0;
815 }
816 Err(GeopError::new(format!(
817 "could not tell which way {curve:?} winds: both sides of it classify the same way"
818 )))
819}