Another project
1use bone_types::{Angle, AngleTolerance, Length, Parameter, Point2, Tolerance, UnitVec2, Vec2};
2use core::f64::consts::{PI, TAU};
3use uom::si::angle::radian;
4use uom::si::length::millimeter;
5
6use crate::KernelError;
7use crate::aabb::Aabb2;
8use crate::angles;
9use crate::circle2::Circle2;
10use crate::closest::ClosestPoint2;
11use crate::curvature::Curvature;
12use crate::curve2::{Curve2, Curve2Kind};
13
14#[derive(Copy, Clone, Debug, PartialEq)]
15pub struct Arc2 {
16 center: Point2,
17 radius: Length,
18 start_angle: Angle,
19 sweep_angle: Angle,
20}
21
22impl Arc2 {
23 pub fn new(
24 center: Point2,
25 radius: Length,
26 start_angle: Angle,
27 sweep_angle: Angle,
28 tolerance: Tolerance,
29 ) -> Result<Self, KernelError> {
30 if radius.get::<millimeter>() < tolerance.value() {
31 return Err(KernelError::DegenerateArc);
32 }
33 let sweep = sweep_angle.get::<radian>();
34 let angle_eps = AngleTolerance::from_arc_length(tolerance, radius).radians();
35 if sweep.abs() < angle_eps {
36 return Err(KernelError::DegenerateArc);
37 }
38 if sweep.abs() > TAU + angle_eps {
39 return Err(KernelError::DegenerateArc);
40 }
41 Ok(Self {
42 center,
43 radius,
44 start_angle,
45 sweep_angle,
46 })
47 }
48
49 pub fn from_center_start_end(
50 center: Point2,
51 start: Point2,
52 end: Point2,
53 tolerance: Tolerance,
54 ) -> Result<Self, KernelError> {
55 let (cx, cy) = center.coords_mm();
56 let (sx, sy) = start.coords_mm();
57 let (ex, ey) = end.coords_mm();
58 let radius = (sx - cx).hypot(sy - cy);
59 let start_angle = (sy - cy).atan2(sx - cx);
60 let sweep = ((ey - cy).atan2(ex - cx) - start_angle).rem_euclid(TAU);
61 if !(radius.is_finite() && sweep.is_finite()) {
62 return Err(KernelError::DegenerateArc);
63 }
64 Self::new(
65 center,
66 Length::new::<millimeter>(radius),
67 Angle::new::<radian>(start_angle),
68 Angle::new::<radian>(sweep),
69 tolerance,
70 )
71 }
72
73 #[must_use]
74 pub const fn center(self) -> Point2 {
75 self.center
76 }
77
78 #[must_use]
79 pub const fn radius(self) -> Length {
80 self.radius
81 }
82
83 #[must_use]
84 pub const fn start_angle(self) -> Angle {
85 self.start_angle
86 }
87
88 #[must_use]
89 pub const fn sweep_angle(self) -> Angle {
90 self.sweep_angle
91 }
92
93 #[must_use]
94 pub fn radius_mm(self) -> f64 {
95 self.radius.get::<millimeter>()
96 }
97
98 #[must_use]
99 pub fn start_rad(self) -> f64 {
100 self.start_angle.get::<radian>()
101 }
102
103 #[must_use]
104 pub fn sweep_rad(self) -> f64 {
105 self.sweep_angle.get::<radian>()
106 }
107
108 #[must_use]
109 pub fn as_full_circle(self) -> Circle2 {
110 Circle2::from_raw(self.center, self.radius)
111 }
112
113 #[must_use]
114 pub fn contains_point(self, p: Point2, tolerance: Tolerance) -> bool {
115 let (cx, cy) = self.center.coords_mm();
116 let (px, py) = p.coords_mm();
117 let dx = px - cx;
118 let dy = py - cy;
119 let r = self.radius_mm();
120 let radial = (dx * dx + dy * dy).sqrt();
121 if (radial - r).abs() > tolerance.value() {
122 return false;
123 }
124 let angle_tol = AngleTolerance::from_arc_length(tolerance, self.radius);
125 angles::contains(
126 dy.atan2(dx),
127 self.start_rad(),
128 self.sweep_rad(),
129 angle_tol.radians(),
130 )
131 }
132
133 fn angle_at(self, t: Parameter) -> f64 {
134 self.start_rad() + self.sweep_rad() * t.value()
135 }
136}
137
138impl Curve2 for Arc2 {
139 fn evaluate(&self, t: Parameter) -> Point2 {
140 let (cx, cy) = self.center.coords_mm();
141 let r = self.radius_mm();
142 let theta = self.angle_at(t);
143 Point2::from_mm(cx + r * theta.cos(), cy + r * theta.sin())
144 }
145
146 fn derivative(&self, t: Parameter) -> Vec2 {
147 let r = self.radius_mm();
148 let sweep = self.sweep_rad();
149 let theta = self.angle_at(t);
150 Vec2::from_mm(-r * sweep * theta.sin(), r * sweep * theta.cos())
151 }
152
153 fn tangent(&self, t: Parameter) -> UnitVec2 {
154 let theta = self.angle_at(t);
155 let sign = self.sweep_rad().signum();
156 UnitVec2::new_unchecked(-sign * theta.sin(), sign * theta.cos())
157 }
158
159 fn curvature(&self, _t: Parameter) -> Curvature {
160 Curvature::from_radius(self.radius).with_sign(self.sweep_rad())
161 }
162
163 fn bounding_box(&self) -> Aabb2 {
164 arc_bounding_box(self.center, self.radius, self.start_angle, self.sweep_angle)
165 }
166
167 fn closest_point(&self, p: Point2, tolerance: Tolerance) -> ClosestPoint2 {
168 let (cx, cy) = self.center.coords_mm();
169 let (px, py) = p.coords_mm();
170 let dx = px - cx;
171 let dy = py - cy;
172 let dist = (dx * dx + dy * dy).sqrt();
173 let r = self.radius_mm();
174 let angle_tol = AngleTolerance::from_arc_length(tolerance, self.radius);
175
176 let theta_unclamped = if dist < tolerance.value() {
177 self.start_rad()
178 } else {
179 dy.atan2(dx)
180 };
181
182 let theta = angles::clamp(
183 theta_unclamped,
184 self.start_rad(),
185 self.sweep_rad(),
186 angle_tol.radians(),
187 );
188 let sweep = self.sweep_rad();
189 let delta = if sweep >= 0.0 {
190 (theta - self.start_rad()).rem_euclid(TAU)
191 } else {
192 (self.start_rad() - theta).rem_euclid(TAU)
193 };
194 let t = (delta / sweep.abs()).clamp(0.0, 1.0);
195 let proj_x = cx + r * theta.cos();
196 let proj_y = cy + r * theta.sin();
197 let projected = Point2::from_mm(proj_x, proj_y);
198 let distance =
199 Length::new::<millimeter>(((px - proj_x).powi(2) + (py - proj_y).powi(2)).sqrt());
200 ClosestPoint2::new(Parameter::new(t), projected, distance)
201 }
202
203 fn as_kind(&self) -> Curve2Kind {
204 Curve2Kind::Arc(*self)
205 }
206}
207
208impl core::fmt::Display for Arc2 {
209 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
210 write!(
211 f,
212 "arc2{{ c={}, r={} mm, start={} rad, sweep={} rad }}",
213 self.center,
214 self.radius.get::<millimeter>(),
215 self.start_rad(),
216 self.sweep_rad(),
217 )
218 }
219}
220
221#[must_use]
222pub fn arc_bounding_box(
223 center: Point2,
224 radius: Length,
225 start_angle: Angle,
226 sweep_angle: Angle,
227) -> Aabb2 {
228 let (cx, cy) = center.coords_mm();
229 let r = radius.get::<millimeter>();
230 let start = start_angle.get::<radian>();
231 let sweep = sweep_angle.get::<radian>();
232 let end = start + sweep;
233 let start_pt = Point2::from_mm(cx + r * start.cos(), cy + r * start.sin());
234 let end_pt = Point2::from_mm(cx + r * end.cos(), cy + r * end.sin());
235 let base = Aabb2::from_corners(start_pt, end_pt);
236 let cardinals: [(f64, Point2); 4] = [
237 (0.0, Point2::from_mm(cx + r, cy)),
238 (PI * 0.5, Point2::from_mm(cx, cy + r)),
239 (PI, Point2::from_mm(cx - r, cy)),
240 (-PI * 0.5, Point2::from_mm(cx, cy - r)),
241 ];
242 cardinals.into_iter().fold(base, |bbox, (theta, extreme)| {
243 if angles::contains(theta, start, sweep, 0.0) {
244 bbox.extend_point(extreme)
245 } else {
246 bbox
247 }
248 })
249}
250
251#[cfg(test)]
252mod tests {
253 use super::{Arc2, arc_bounding_box};
254 use bone_types::{Angle, Length, Point2, Tolerance};
255 use core::f64::consts::{FRAC_PI_2, PI, TAU};
256 use uom::si::angle::radian;
257 use uom::si::length::millimeter;
258
259 fn approx(a: f64, b: f64) -> bool {
260 (a - b).abs() < 1e-9
261 }
262
263 #[test]
264 fn center_start_end_sweep_is_ccw_and_order_picks_minor_or_major() {
265 let tol = Tolerance::new(1e-9);
266 let center = Point2::from_mm(0.0, 0.0);
267 let east = Point2::from_mm(5.0, 0.0);
268 let north = Point2::from_mm(0.0, 5.0);
269
270 let Ok(minor) = Arc2::from_center_start_end(center, east, north, tol) else {
271 panic!("east->north is a buildable quarter arc");
272 };
273 assert!(approx(minor.radius_mm(), 5.0));
274 assert!(approx(minor.start_rad(), 0.0));
275 assert!(approx(minor.sweep_rad(), FRAC_PI_2));
276
277 let Ok(major) = Arc2::from_center_start_end(center, north, east, tol) else {
278 panic!("north->east is the complementary major arc");
279 };
280 assert!(approx(major.start_rad(), FRAC_PI_2));
281 assert!(approx(major.sweep_rad(), 3.0 * FRAC_PI_2));
282 }
283
284 #[test]
285 fn center_start_end_rejects_non_finite_and_degenerate() {
286 let tol = Tolerance::new(1e-9);
287 let center = Point2::from_mm(0.0, 0.0);
288 let coincident = Point2::from_mm(0.0, 0.0);
289 let any = Point2::from_mm(5.0, 0.0);
290 assert!(Arc2::from_center_start_end(center, coincident, any, tol).is_err());
291 let nan = Point2::from_mm(f64::NAN, 0.0);
292 assert!(Arc2::from_center_start_end(center, nan, any, tol).is_err());
293 }
294
295 fn arc(cx: f64, cy: f64, r_mm: f64, start_rad: f64, sweep_rad: f64) -> (Point2, Point2) {
296 let bbox = arc_bounding_box(
297 Point2::from_mm(cx, cy),
298 Length::new::<millimeter>(r_mm),
299 Angle::new::<radian>(start_rad),
300 Angle::new::<radian>(sweep_rad),
301 );
302 (bbox.min(), bbox.max())
303 }
304
305 #[test]
306 fn full_circle_sweep_spans_all_cardinals() {
307 let (mn, mx) = arc(0.0, 0.0, 2.0, 0.0, TAU);
308 let (mnx, mny) = mn.coords_mm();
309 let (mxx, mxy) = mx.coords_mm();
310 assert!(approx(mnx, -2.0) && approx(mny, -2.0));
311 assert!(approx(mxx, 2.0) && approx(mxy, 2.0));
312 }
313
314 #[test]
315 fn quarter_arc_hugs_its_quadrant() {
316 let (mn, mx) = arc(0.0, 0.0, 1.0, 0.0, FRAC_PI_2);
317 let (mnx, mny) = mn.coords_mm();
318 let (mxx, mxy) = mx.coords_mm();
319 assert!(approx(mnx, 0.0) && approx(mny, 0.0));
320 assert!(approx(mxx, 1.0) && approx(mxy, 1.0));
321 }
322
323 #[test]
324 fn negative_sweep_mirrors_positive() {
325 let (fwd_min, fwd_max) = arc(0.0, 0.0, 1.0, 0.0, FRAC_PI_2);
326 let (rev_min, rev_max) = arc(0.0, 0.0, 1.0, FRAC_PI_2, -FRAC_PI_2);
327 assert_eq!(fwd_min, rev_min);
328 assert_eq!(fwd_max, rev_max);
329 }
330
331 #[test]
332 fn small_sweep_hugs_endpoints_only() {
333 let start = FRAC_PI_2 * 0.5;
334 let sweep = 0.1;
335 let (mn, mx) = arc(0.0, 0.0, 1.0, start, sweep);
336 let (mnx, mny) = mn.coords_mm();
337 let (mxx, mxy) = mx.coords_mm();
338 let end = start + sweep;
339 assert!(approx(mnx, end.cos()) && approx(mny, start.sin()));
340 assert!(approx(mxx, start.cos()) && approx(mxy, end.sin()));
341 }
342
343 #[test]
344 fn wide_sweep_crosses_three_axes() {
345 let start = FRAC_PI_2 * 0.5;
346 let sweep = FRAC_PI_2 * 2.5;
347 let (mn, mx) = arc(0.0, 0.0, 1.0, start, sweep);
348 let (mnx, mny) = mn.coords_mm();
349 let (mxx, mxy) = mx.coords_mm();
350 assert!(approx(mnx, -1.0) && approx(mny, -1.0));
351 assert!(approx(mxx, start.cos()) && approx(mxy, 1.0));
352 }
353
354 #[test]
355 fn translated_center_offsets_the_box() {
356 let (mn, mx) = arc(10.0, -5.0, 3.0, 0.0, PI);
357 let (mnx, mny) = mn.coords_mm();
358 let (mxx, mxy) = mx.coords_mm();
359 assert!(approx(mnx, 7.0) && approx(mny, -5.0));
360 assert!(approx(mxx, 13.0) && approx(mxy, -2.0));
361 }
362}