Another project
1use bone_types::{Aabb3, ChordHeightTolerance, Length, Parameter, Point3, Tolerance, Vec3};
2use uom::si::length::millimeter;
3
4use crate::KernelError;
5use crate::closest::ClosestPoint3;
6use crate::curve3::Curve3;
7
8#[derive(Clone, Debug, PartialEq)]
9pub struct Polyline3 {
10 vertices: Vec<Point3>,
11}
12
13impl Polyline3 {
14 pub fn new(vertices: Vec<Point3>, tolerance: Tolerance) -> Result<Self, KernelError> {
15 if vertices.len() < 2 {
16 return Err(KernelError::DegeneratePolyline);
17 }
18 let has_zero_segment = vertices.windows(2).any(|w| match w {
19 [a, b] => (*b - *a).norm_mm() < tolerance.value(),
20 _ => false,
21 });
22 if has_zero_segment {
23 return Err(KernelError::DegeneratePolyline);
24 }
25 Ok(Self { vertices })
26 }
27
28 #[must_use]
29 pub fn vertices(&self) -> &[Point3] {
30 &self.vertices
31 }
32
33 #[must_use]
34 fn segments(&self) -> u32 {
35 u32::try_from(self.vertices.len() - 1).unwrap_or(u32::MAX)
36 }
37}
38
39impl Curve3 for Polyline3 {
40 fn evaluate(&self, t: Parameter) -> Point3 {
41 let segments = self.segments();
42 let scaled = t.value().clamp(0.0, 1.0) * f64::from(segments);
43 self.vertices
44 .windows(2)
45 .enumerate()
46 .find_map(|(index, pair)| {
47 let &[from, to] = pair else { return None };
48 let lo = f64::from(u32::try_from(index).unwrap_or(u32::MAX));
49 let is_last = index + 1 == self.vertices.len() - 1;
50 (scaled >= lo && (scaled < lo + 1.0 || is_last))
51 .then(|| from + (to - from) * (scaled - lo))
52 })
53 .unwrap_or_else(|| unreachable!("Polyline3 holds at least two vertices"))
54 }
55
56 fn derivative(&self, t: Parameter) -> Vec3 {
57 let segments = self.segments();
58 let scaled = t.value().clamp(0.0, 1.0) * f64::from(segments);
59 self.vertices
60 .windows(2)
61 .enumerate()
62 .find_map(|(index, pair)| {
63 let &[from, to] = pair else { return None };
64 let lo = f64::from(u32::try_from(index).unwrap_or(u32::MAX));
65 let is_last = index + 1 == self.vertices.len() - 1;
66 (scaled >= lo && (scaled < lo + 1.0 || is_last))
67 .then(|| (to - from) * f64::from(segments))
68 })
69 .unwrap_or_else(|| unreachable!("Polyline3 holds at least two vertices"))
70 }
71
72 fn bounding_box(&self) -> Aabb3 {
73 Aabb3::from_points(self.vertices.iter().copied())
74 .unwrap_or_else(|| unreachable!("Polyline3 holds at least two vertices"))
75 }
76
77 fn closest_point(&self, query: Point3, _tolerance: Tolerance) -> ClosestPoint3 {
78 let segments = self.segments();
79 self.vertices
80 .windows(2)
81 .enumerate()
82 .filter_map(|(index, pair)| {
83 let &[from, to] = pair else { return None };
84 let edge = to - from;
85 let local =
86 ((query - from).dot_mm2(edge) / edge.norm_squared_mm2()).clamp(0.0, 1.0);
87 let point = from + edge * local;
88 let distance = (query - point).norm_mm();
89 let param = (f64::from(u32::try_from(index).unwrap_or(u32::MAX)) + local)
90 / f64::from(segments);
91 Some(ClosestPoint3::new(
92 Parameter::new(param),
93 point,
94 Length::new::<millimeter>(distance),
95 ))
96 })
97 .min_by(|left, right| {
98 left.distance()
99 .partial_cmp(&right.distance())
100 .unwrap_or(core::cmp::Ordering::Equal)
101 })
102 .unwrap_or_else(|| unreachable!("Polyline3 holds at least two vertices"))
103 }
104
105 fn tessellate(&self, _tolerance: ChordHeightTolerance) -> Vec<Point3> {
106 self.vertices.clone()
107 }
108}
109
110impl core::fmt::Display for Polyline3 {
111 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
112 write!(f, "polyline3{{ {} verts: ", self.vertices.len())?;
113 self.vertices.iter().enumerate().try_for_each(|(i, v)| {
114 if i > 0 {
115 write!(f, " -> ")?;
116 }
117 write!(f, "{v}")
118 })?;
119 write!(f, " }}")
120 }
121}