1use ogeom_core::{OgeomResult, ogeom_bail};
51use ogeom_math::{Point2, Vector2};
52
53use crate::fit::segments::{
54 Run, Shape, bow, chord, circle_through, count, deviation, fewest_runs, line_fits, line_through,
55 reach,
56};
57use crate::outline::{Path, Piece2, meet};
58
59#[derive(Debug, Clone, Copy, PartialEq)]
61pub struct Segments {
62 pub tolerance: f64,
64 pub corners: bool,
69 pub axis_snap: Option<f64>,
73 pub min_bow: f64,
77 pub closed: bool,
80}
81
82#[derive(Debug, Clone, Copy, PartialEq)]
84pub struct Segment2 {
85 pub piece: Piece2,
87 pub range: (usize, usize),
92 pub sigma: f64,
94}
95
96pub fn fit_segments_2d(points: &[Point2], options: &Segments) -> OgeomResult<Vec<Segment2>> {
113 let Segments {
114 tolerance,
115 corners,
116 axis_snap,
117 min_bow,
118 closed,
119 } = *options;
120 if !(tolerance > 0.0 && tolerance.is_finite()) {
121 ogeom_bail!(
122 Construction,
123 "a segment tolerance of {tolerance} is no bound"
124 );
125 }
126 if min_bow.is_nan() || min_bow < 0.0 {
127 ogeom_bail!(Construction, "a least bow of {min_bow} is not a distance");
128 }
129 if let Some(a) = axis_snap
130 && !(0.0..=core::f64::consts::FRAC_PI_4).contains(&a)
131 {
132 ogeom_bail!(
133 Construction,
134 "snapping lines within {a} rad of an axis is not an angle up to a quarter turn"
135 );
136 }
137 if let Some(p) = points
138 .iter()
139 .find(|p| !(p.x.is_finite() && p.y.is_finite()))
140 {
141 ogeom_bail!(Numeric, "the polyline point {p:?} is not finite");
142 }
143 let (distinct, index) = distinct(points, closed);
144 let n = distinct.len();
145 if n < 2 || (closed && n < 3) {
146 ogeom_bail!(
147 Construction,
148 "{n} distinct points are too few for a {} polyline",
149 if closed { "closed" } else { "open" }
150 );
151 }
152 if closed && let Some(circle) = whole_circle(&distinct, tolerance) {
153 let sigma = rms(distinct.iter().map(|p| distance_to_piece(*p, &circle)));
154 return Ok(vec![Segment2 {
155 piece: circle,
156 range: (index[0], index[0]),
157 sigma,
158 }]);
159 }
160 let ring: Vec<Point2> = if closed {
163 (0..=4 * n).map(|k| distinct[k % n]).collect()
164 } else {
165 distinct.clone()
166 };
167 let mut runs = fewest_runs(&ring, closed.then_some(n), tolerance);
168 if closed && let Some(first) = runs.first().map(|r| r.first) {
169 let to = first % n + n;
171 for run in &mut runs {
172 (run.first, run.last) = (run.first + to - first, run.last + to - first);
173 }
174 }
175 if closed && runs.len() == 1 {
176 return Ok(whole_loop(&distinct, &index, runs[0].shape));
177 }
178 runs = straighten(&ring, runs, min_bow, tolerance);
179 if let Some(angle) = axis_snap {
180 for run in &mut runs {
181 snap(&ring[run.first..=run.last], run, angle, tolerance);
182 }
183 }
184 let tangent = tangents(&ring, &mut runs, closed, tolerance);
185 let chain = Chain {
186 ring: &ring,
187 period: n,
188 runs: &runs,
189 closed,
190 tolerance,
191 };
192 let pieces = chain.pieces(&tangent, corners);
193 let original = |k: usize| index[if closed { k % n } else { k }];
194 Ok(pieces
195 .into_iter()
196 .map(|(piece, first, last)| Segment2 {
197 piece,
198 range: (original(first), original(last)),
199 sigma: rms(ring[first..=last]
200 .iter()
201 .map(|p| distance_to_piece(*p, &piece))),
202 })
203 .collect())
204}
205
206fn whole_loop(points: &[Point2], index: &[usize], shape: Shape) -> Vec<Segment2> {
209 if let Shape::Circle { centre, radius } = shape {
210 let mut closed = points.to_vec();
211 closed.push(points[0]);
212 let circle = Piece2::Circle {
213 centre,
214 radius,
215 counter_clockwise: turn_about(&closed, centre) > 0.0,
216 };
217 let sigma = rms(points.iter().map(|p| distance_to_piece(*p, &circle)));
218 return vec![Segment2 {
219 piece: circle,
220 range: (index[0], index[0]),
221 sigma,
222 }];
223 }
224 let n = points.len();
225 (0..n)
226 .map(|k| Segment2 {
227 piece: Piece2::Line {
228 from: points[k],
229 to: points[(k + 1) % n],
230 },
231 range: (index[k], index[(k + 1) % n]),
232 sigma: 0.0,
233 })
234 .collect()
235}
236
237fn distinct(points: &[Point2], closed: bool) -> (Vec<Point2>, Vec<usize>) {
240 let (mut out, mut index) = (Vec::with_capacity(points.len()), Vec::new());
241 for (k, p) in points.iter().enumerate() {
242 if out.last() != Some(p) {
243 out.push(*p);
244 index.push(k);
245 }
246 }
247 while closed && out.len() > 1 && out.first() == out.last() {
248 out.pop();
249 index.pop();
250 }
251 (out, index)
252}
253
254fn whole_circle(points: &[Point2], tolerance: f64) -> Option<Piece2> {
257 let Shape::Circle { centre, radius } = circle_through(points)? else {
258 return None;
259 };
260 let mut closed = points.to_vec();
261 closed.push(points[0]);
262 (deviation(&closed, Shape::Circle { centre, radius }) <= tolerance).then(|| Piece2::Circle {
263 centre,
264 radius,
265 counter_clockwise: turn_about(&closed, centre) > 0.0,
266 })
267}
268
269fn turn_about(points: &[Point2], centre: Point2) -> f64 {
271 points
272 .windows(2)
273 .map(|w| {
274 let (u, v) = (w[0] - centre, w[1] - centre);
275 u.cross(v).atan2(u.dot(v))
276 })
277 .sum()
278}
279
280fn straighten(ring: &[Point2], runs: Vec<Run>, min_bow: f64, tolerance: f64) -> Vec<Run> {
284 let mut out = Vec::with_capacity(runs.len());
285 for run in runs {
286 let points = &ring[run.first..=run.last];
287 if matches!(run.shape, Shape::Line { .. }) || bow(points) > min_bow {
288 out.push(run);
289 continue;
290 }
291 let mut first = run.first;
292 while first < run.last {
293 let (last, strip) = reach(first, run.last, 1, |last| {
294 line_fits(&ring[first..=last], tolerance)
295 });
296 let points = &ring[first..=last];
297 let shape = line_through(points)
298 .filter(|s| deviation(points, *s) <= tolerance)
299 .or(strip)
300 .unwrap_or_else(|| chord(ring[first], ring[last]));
301 out.push(Run { first, last, shape });
302 first = last;
303 }
304 }
305 out
306}
307
308fn snap(points: &[Point2], run: &mut Run, angle: f64, tolerance: f64) {
311 let Shape::Line { along, .. } = run.shape else {
312 return;
313 };
314 let axis = if along.x.abs() >= along.y.abs() {
315 Vector2::new(along.x.signum(), 0.0)
316 } else {
317 Vector2::new(0.0, along.y.signum())
318 };
319 if along.cross(axis).abs().asin() > angle {
320 return;
321 }
322 let sum = points
323 .iter()
324 .fold(Vector2::new(0.0, 0.0), |s, p| s + p.to_vector());
325 let centroid = Point2::from_vector(sum / count(points.len()));
326 let snapped = Shape::Line {
327 at: centroid,
328 along: axis,
329 };
330 if deviation(points, snapped) <= tolerance {
331 run.shape = snapped;
332 }
333}
334
335fn tangents(ring: &[Point2], runs: &mut [Run], closed: bool, tolerance: f64) -> Vec<bool> {
340 let m = runs.len();
341 let mut tangent = vec![false; m + 1];
342 let line = |run: &Run| match run.shape {
343 Shape::Line { at, along } => Some((at, along)),
344 Shape::Circle { .. } => None,
345 };
346 for k in 0..m {
347 let Shape::Circle { centre, radius } = runs[k].shape else {
348 continue;
349 };
350 let before = (k > 0 || closed)
352 .then(|| (k + m - 1) % m)
353 .filter(|&b| b != k);
354 let after = (k + 1 < m || closed)
355 .then(|| (k + 1) % m)
356 .filter(|&a| a != k && Some(a) != before);
357 let before = before.and_then(|b| line(&runs[b]));
358 let after = after.and_then(|a| line(&runs[a]));
359 let points = &ring[runs[k].first..=runs[k].last];
360 let holds = |s: Option<Shape>| s.filter(|s| deviation(points, *s) <= tolerance);
361 let both = before
362 .zip(after)
363 .and_then(|(b, a)| holds(tangent_to_lines(points, centre, b, a)));
364 let fitted = both
365 .map(|s| (s, true, true))
366 .or_else(|| {
367 before.and_then(|b| {
368 holds(tangent_to_line(points, centre, radius, b)).map(|s| (s, true, false))
369 })
370 })
371 .or_else(|| {
372 after.and_then(|a| {
373 holds(tangent_to_line(points, centre, radius, a)).map(|s| (s, false, true))
374 })
375 });
376 if let Some((shape, at_start, at_end)) = fitted {
377 runs[k].shape = shape;
378 tangent[k] |= at_start;
379 if at_end {
380 tangent[if closed { (k + 1) % m } else { k + 1 }] = true;
381 }
382 }
383 }
384 tangent
385}
386
387fn toward(at: Point2, along: Vector2, p: Point2) -> Option<Vector2> {
389 let normal = along.perpendicular();
390 let side = normal.dot(p - at);
391 (side != 0.0).then(|| normal * side.signum())
392}
393
394fn tangent_to_line(
398 points: &[Point2],
399 centre: Point2,
400 radius: f64,
401 (at, along): (Point2, Vector2),
402) -> Option<Shape> {
403 let normal = toward(at, along, centre)?;
404 let (mut s, mut r) = (along.dot(centre - at), radius);
405 for _ in 0..8 {
406 let c = at + along * s + normal * r;
407 let (mut m, mut v) = ([[0.0f64; 2]; 2], [0.0f64; 2]);
408 for p in points {
409 let d = *p - c;
410 let dist = d.magnitude();
411 if dist == 0.0 {
412 return None;
413 }
414 let e = d / dist;
415 let row = [-e.dot(along), -e.dot(normal) - 1.0];
416 let res = dist - r;
417 for i in 0..2 {
418 for j in 0..2 {
419 m[i][j] += row[i] * row[j];
420 }
421 v[i] -= row[i] * res;
422 }
423 }
424 let det = m[0][0].mul_add(m[1][1], -m[0][1] * m[1][0]);
425 if det.is_nan() || det == 0.0 {
426 break;
427 }
428 s += (v[0] * m[1][1] - v[1] * m[0][1]) / det;
429 r += (m[0][0] * v[1] - m[1][0] * v[0]) / det;
430 if r.is_nan() || r <= 0.0 {
431 return None;
432 }
433 }
434 let c = at + along * s + normal * r;
435 (s.is_finite() && r.is_finite()).then_some(Shape::Circle {
436 centre: c,
437 radius: r,
438 })
439}
440
441fn tangent_to_lines(
446 points: &[Point2],
447 centre: Point2,
448 (a1, u1): (Point2, Vector2),
449 (a2, u2): (Point2, Vector2),
450) -> Option<Shape> {
451 let (n1, n2) = (toward(a1, u1, centre)?, toward(a2, u2, centre)?);
452 let g = n1 - n2;
454 let g2 = g.dot(g);
455 if g2.is_nan() || g2 == 0.0 {
456 return None;
457 }
458 let k = n1.dot(a1 - centre) - n2.dot(a2 - centre);
459 let base = centre + g * (k / g2);
460 let b = g.perpendicular() / g2.sqrt();
461 let (r0, rate) = (n1.dot(base - a1), n1.dot(b));
462 let mut t = 0.0;
463 for _ in 0..8 {
464 let (c, r) = (base + b * t, rate.mul_add(t, r0));
465 if r.is_nan() || r <= 0.0 {
466 return None;
467 }
468 let (mut jj, mut jr) = (0.0, 0.0);
469 for p in points {
470 let d = *p - c;
471 let dist = d.magnitude();
472 if dist == 0.0 {
473 return None;
474 }
475 let row = -(d / dist).dot(b) - rate;
476 jj += row * row;
477 jr += row * (dist - r);
478 }
479 if jj.is_nan() || jj == 0.0 {
480 break;
481 }
482 t -= jr / jj;
483 }
484 let r = rate.mul_add(t, r0);
485 (r > 0.0 && t.is_finite()).then(|| Shape::Circle {
486 centre: base + b * t,
487 radius: r,
488 })
489}
490
491fn path(shape: Shape) -> Path {
492 match shape {
493 Shape::Line { at, along } => Path::Line { at, along },
494 Shape::Circle { centre, radius } => Path::Circle {
495 centre,
496 radius,
497 ccw: true,
498 },
499 }
500}
501
502struct Chain<'a> {
504 ring: &'a [Point2],
505 period: usize,
507 runs: &'a [Run],
508 closed: bool,
509 tolerance: f64,
510}
511
512impl Chain<'_> {
513 fn joints(&self) -> usize {
514 self.runs.len() + usize::from(!self.closed)
515 }
516
517 fn pieces(&self, tangent: &[bool], corners: bool) -> Vec<(Piece2, usize, usize)> {
527 let m = self.runs.len();
528 if m == 0 {
529 return Vec::new();
530 }
531 let joints = self.joints();
532 let choices: Vec<Vec<Point2>> = (0..joints)
533 .map(|j| self.choices(j, tangent[j], corners))
534 .collect();
535 let mut level = vec![0usize; joints];
536 let mut exact = vec![false; m];
537 loop {
538 for k in (0..m).filter(|&k| exact[k]) {
539 for j in [k, self.end(k)] {
540 level[j] = choices[j].len() - 1;
541 }
542 }
543 let built: Vec<Option<Piece2>> = (0..m)
544 .map(|k| {
545 let (from, to) = (k, self.end(k));
546 if exact[k] {
547 None
548 } else {
549 self.piece(k, choices[from][level[from]], choices[to][level[to]])
550 }
551 })
552 .collect();
553 let windows: Vec<Option<(usize, usize)>> = (0..m)
554 .map(|k| {
555 let run = self.runs[k];
556 if exact[k] {
557 Some((run.first, run.last))
558 } else {
559 built[k].and_then(|p| self.window(k, &p))
560 }
561 })
562 .collect();
563 let splits: Vec<Option<usize>> = (0..joints).map(|j| self.split(j, &windows)).collect();
564 let mut failing = (0..joints).filter(|&j| splits[j].is_none()).peekable();
565 if failing.peek().is_none() {
566 return self.emit(&built, &splits);
567 }
568 for j in failing {
569 if level[j] + 1 < choices[j].len() {
570 level[j] += 1;
571 } else {
572 for k in self.beside(j) {
573 exact[k] = true;
574 }
575 }
576 }
577 }
578 }
579
580 const fn end(&self, k: usize) -> usize {
582 if self.closed {
583 (k + 1) % self.runs.len()
584 } else {
585 k + 1
586 }
587 }
588
589 fn beside(&self, j: usize) -> Vec<usize> {
591 let m = self.runs.len();
592 if self.closed {
593 vec![(j + m - 1) % m, j]
594 } else {
595 (j.saturating_sub(1)..=j.min(m - 1)).collect()
596 }
597 }
598
599 const fn middle(&self, k: usize) -> usize {
601 let run = self.runs[k];
602 run.first + (run.last - run.first) / 2
603 }
604
605 fn window(&self, k: usize, piece: &Piece2) -> Option<(usize, usize)> {
609 let m = self.runs.len();
610 let holds = |i: usize| distance_to_piece(self.ring[i], piece) <= self.tolerance;
611 let mid = self.middle(k);
612 if !holds(mid) {
613 return None;
614 }
615 let n = self.period;
619 let lower = if self.closed && k == 0 {
620 self.middle(m - 1) - n
621 } else if k > 0 {
622 self.middle(k - 1)
623 } else {
624 self.runs[0].first
625 };
626 let upper = if self.closed && k + 1 == m {
627 self.middle(0) + n
628 } else if k + 1 < m {
629 self.middle(k + 1)
630 } else {
631 self.runs[m - 1].last
632 };
633 let (mut lo, mut hi) = (mid, mid);
634 while lo > lower && holds(lo - 1) {
635 lo -= 1;
636 }
637 while hi < upper && holds(hi + 1) {
638 hi += 1;
639 }
640 Some((lo, hi))
641 }
642
643 fn split(&self, j: usize, windows: &[Option<(usize, usize)>]) -> Option<usize> {
647 let m = self.runs.len();
648 if !self.closed && j == 0 {
649 let first = self.runs[0].first;
650 return windows[0].filter(|w| w.0 <= first).map(|_| first);
651 }
652 if !self.closed && j == m {
653 let last = self.runs[m - 1].last;
654 return windows[m - 1].filter(|w| w.1 >= last).map(|_| last);
655 }
656 let (a, b) = ((j + m - 1) % m, j % m);
657 let (wa, wb) = (windows[a]?, windows[b]?);
658 let shift = if self.closed && j == 0 {
660 self.period
661 } else {
662 0
663 };
664 let (lo, hi) = (
665 wa.0.saturating_sub(shift).max(wb.0),
666 (wa.1 - shift).min(wb.1),
667 );
668 let shared = self.runs[b].first;
669 (lo <= hi).then(|| shared.clamp(lo, hi))
670 }
671
672 fn emit(
675 &self,
676 built: &[Option<Piece2>],
677 splits: &[Option<usize>],
678 ) -> Vec<(Piece2, usize, usize)> {
679 let m = self.runs.len();
680 let n = self.period;
681 let mut out = Vec::with_capacity(m);
682 for (k, piece) in built.iter().enumerate() {
683 let run = self.runs[k];
684 let first = splits[k].unwrap_or(run.first);
685 let last = splits[self.end(k)].map_or(run.last, |s| {
686 if self.closed && k == m - 1 { s + n } else { s }
687 });
688 match piece {
689 Some(p) => out.push((*p, first, last)),
690 None => out.extend((run.first..run.last).map(|i| {
693 (
694 Piece2::Line {
695 from: self.ring[i],
696 to: self.ring[i + 1],
697 },
698 i,
699 i + 1,
700 )
701 })),
702 }
703 }
704 out
705 }
706
707 fn choices(&self, j: usize, tangent: bool, corners: bool) -> Vec<Point2> {
710 let m = self.runs.len();
711 if !self.closed && (j == 0 || j == m) {
712 let (run, p) = if j == 0 {
713 (self.runs[0], self.ring[self.runs[0].first])
714 } else {
715 (self.runs[m - 1], self.ring[self.runs[m - 1].last])
716 };
717 return path(run.shape).foot(p).into_iter().chain([p]).collect();
718 }
719 let (a, b) = (self.runs[(j + m - 1) % m], self.runs[j % m]);
720 let q = self.ring[b.first];
721 let (pa, pb) = (path(a.shape), path(b.shape));
722 let near = pa.foot(q).zip(pb.foot(q)).map(|(x, y)| x.lerp(y, 0.5));
723 let mut out = Vec::with_capacity(3);
724 if tangent {
725 let touch = match (a.shape, b.shape) {
727 (Shape::Circle { centre, .. }, Shape::Line { .. }) => pb.foot(centre),
728 (Shape::Line { .. }, Shape::Circle { centre, .. }) => pa.foot(centre),
729 _ => None,
730 };
731 out.extend(touch);
732 } else if corners {
733 let crossing = meet(pa, pb)
734 .into_iter()
735 .filter(|x| {
736 let past_a = self.past_end(&a, *x);
737 let before_b = self.past_end(&reversed(b), *x);
738 past_a.zip(before_b).is_some_and(|(s, t)| {
739 (-0.5..=0.5).contains(&s) && (-0.5..=0.5).contains(&t)
740 })
741 })
742 .min_by(|x, y| x.square_distance(q).total_cmp(&y.square_distance(q)));
743 out.extend(crossing);
744 }
745 out.extend(near);
746 out.push(q);
747 out
748 }
749
750 fn past_end(&self, run: &Run, x: Point2) -> Option<f64> {
753 let (from, to) = (self.ring[run.first], self.ring[run.last]);
754 match run.shape {
755 Shape::Line { along, .. } => {
756 let span = (to - from).dot(along);
757 (span != 0.0).then(|| (x - to).dot(along) / span)
758 }
759 Shape::Circle { centre, .. } => {
760 let turn = self.turn(run, centre);
761 let (u, v) = (to - centre, x - centre);
762 (turn != 0.0).then(|| u.cross(v).atan2(u.dot(v)) / turn)
763 }
764 }
765 }
766
767 fn turn(&self, run: &Run, centre: Point2) -> f64 {
770 let (lo, hi) = (run.first.min(run.last), run.first.max(run.last));
771 let turn = turn_about(&self.ring[lo..=hi], centre);
772 if run.first <= run.last { turn } else { -turn }
773 }
774
775 fn piece(&self, k: usize, from: Point2, to: Point2) -> Option<Piece2> {
777 let run = self.runs[k];
778 match run.shape {
779 Shape::Line { along, .. } => {
780 ((to - from).dot(along) > 0.0).then_some(Piece2::Line { from, to })
781 }
782 Shape::Circle { centre, .. } => arc_through(centre, from, to, self.turn(&run, centre)),
783 }
784 }
785}
786
787const fn reversed(run: Run) -> Run {
789 Run {
790 first: run.last,
791 last: run.first,
792 shape: run.shape,
793 }
794}
795
796fn arc_through(centre: Point2, from: Point2, to: Point2, turn: f64) -> Option<Piece2> {
799 let d = to - from;
800 let len = d.magnitude();
801 if len == 0.0 || turn == 0.0 {
802 return None;
803 }
804 let middle = from.lerp(to, 0.5);
805 let w = d.perpendicular() / len;
806 let c = middle + w * w.dot(centre - middle);
807 let (u, v) = (from - c, to - c);
808 let mut sweep = u.cross(v).atan2(u.dot(v));
809 let tau = core::f64::consts::TAU;
810 if turn > 0.0 && sweep <= 0.0 {
811 sweep += tau;
812 } else if turn < 0.0 && sweep >= 0.0 {
813 sweep -= tau;
814 }
815 (sweep.abs() < tau).then(|| Piece2::Arc {
816 centre: c,
817 radius: u.magnitude(),
818 start: u.y.atan2(u.x),
819 sweep,
820 })
821}
822
823fn distance_to_piece(p: Point2, piece: &Piece2) -> f64 {
826 match *piece {
827 Piece2::Line { from, to } => {
828 let d = to - from;
829 let len2 = d.dot(d);
830 let t = if len2 > 0.0 {
831 ((p - from).dot(d) / len2).clamp(0.0, 1.0)
832 } else {
833 0.0
834 };
835 from.lerp(to, t).distance(p)
836 }
837 Piece2::Arc {
838 centre,
839 radius,
840 start,
841 sweep,
842 } => {
843 let d = p - centre;
844 let tau = core::f64::consts::TAU;
845 let along = ((d.y.atan2(d.x) - start) * sweep.signum()).rem_euclid(tau);
846 if along <= sweep.abs() {
847 (d.magnitude() - radius).abs()
848 } else {
849 p.distance(piece.start()).min(p.distance(piece.end()))
850 }
851 }
852 Piece2::Circle { centre, radius, .. } => ((p - centre).magnitude() - radius).abs(),
853 }
854}
855
856fn rms(distances: impl Iterator<Item = f64>) -> f64 {
858 let (sum, count) = distances.fold((0.0, 0.0), |(s, c), d| (d.mul_add(d, s), c + 1.0));
859 if count > 0.0 {
860 (sum / count).sqrt()
861 } else {
862 0.0
863 }
864}