Another project
1use bone_types::{
2 Aabb3, Angle, AngleTolerance, ChordHeightTolerance, Length, Plane3, Point3, Vec3,
3};
4use core::f64::consts::PI;
5use uom::si::angle::radian;
6use uom::si::length::millimeter;
7
8use crate::angles;
9
10const MAX_SEGMENTS: u32 = 4096;
11
12#[must_use]
13pub(crate) fn point_at(plane: Plane3, radius: Length, theta: Angle) -> Point3 {
14 let radius_mm = radius.get::<millimeter>();
15 let theta = theta.get::<radian>();
16 let (cx, cy, cz) = plane.origin().coords_mm();
17 let (xx, xy, xz) = plane.x_axis().components();
18 let (yx, yy, yz) = plane.y_axis().components();
19 let (c, s) = (theta.cos(), theta.sin());
20 Point3::from_mm(
21 cx + radius_mm * (c * xx + s * yx),
22 cy + radius_mm * (c * xy + s * yy),
23 cz + radius_mm * (c * xz + s * yz),
24 )
25}
26
27#[must_use]
28pub(crate) fn tangent_vec(plane: Plane3, radius: Length, theta: Angle) -> Vec3 {
29 let radius_mm = radius.get::<millimeter>();
30 let theta = theta.get::<radian>();
31 let (xx, xy, xz) = plane.x_axis().components();
32 let (yx, yy, yz) = plane.y_axis().components();
33 let (c, s) = (theta.cos(), theta.sin());
34 Vec3::from_mm(
35 radius_mm * (-s * xx + c * yx),
36 radius_mm * (-s * xy + c * yy),
37 radius_mm * (-s * xz + c * yz),
38 )
39}
40
41#[must_use]
42pub(crate) fn local_coords(plane: Plane3, p: Point3) -> (f64, f64, f64) {
43 let (dx, dy, dz) = (p - plane.origin()).coords_mm();
44 let (xx, xy, xz) = plane.x_axis().components();
45 let (yx, yy, yz) = plane.y_axis().components();
46 let (nx, ny, nz) = plane.normal().components();
47 (
48 dx * xx + dy * xy + dz * xz,
49 dx * yx + dy * yy + dz * yz,
50 dx * nx + dy * ny + dz * nz,
51 )
52}
53
54#[must_use]
55pub(crate) fn bounding_box(plane: Plane3, radius: Length, start: Angle, sweep: Angle) -> Aabb3 {
56 let start = start.get::<radian>();
57 let sweep = sweep.get::<radian>();
58 let start_pt = point_at(plane, radius, Angle::new::<radian>(start));
59 let end_pt = point_at(plane, radius, Angle::new::<radian>(start + sweep));
60 let base = Aabb3::from_corners(start_pt, end_pt);
61 let (xx, xy, xz) = plane.x_axis().components();
62 let (yx, yy, yz) = plane.y_axis().components();
63 [(xx, yx), (xy, yy), (xz, yz)]
64 .into_iter()
65 .flat_map(|(xk, yk)| {
66 let phi = yk.atan2(xk);
67 [phi, phi + PI]
68 })
69 .filter(|&theta| angles::contains(theta, start, sweep, 0.0))
70 .map(|theta| point_at(plane, radius, Angle::new::<radian>(theta)))
71 .fold(base, |bb, p| bb.union(Aabb3::from_corners(p, p)))
72}
73
74#[must_use]
75pub(crate) fn sample_count(
76 radius: Length,
77 sweep: Angle,
78 tolerance: ChordHeightTolerance,
79 min_segments: u32,
80) -> u32 {
81 let chord_tol_mm = tolerance.millimeters();
82 if chord_tol_mm <= 0.0 {
83 return min_segments;
84 }
85 let radius_mm = radius.get::<millimeter>();
86 let sweep_abs = sweep.get::<radian>().abs();
87 (min_segments..=MAX_SEGMENTS)
88 .find(|&n| {
89 let half_step = sweep_abs / (2.0 * f64::from(n));
90 radius_mm * (1.0 - half_step.cos()) <= chord_tol_mm
91 })
92 .unwrap_or(MAX_SEGMENTS)
93}
94
95#[must_use]
96pub(crate) fn angular_sample_count(
97 sweep: Angle,
98 tolerance: AngleTolerance,
99 min_segments: u32,
100) -> u32 {
101 let sweep_abs = sweep.get::<radian>().abs();
102 let angle_tol = tolerance.radians();
103 if angle_tol <= 0.0 {
104 return min_segments;
105 }
106 (min_segments..=MAX_SEGMENTS)
107 .find(|&n| sweep_abs / f64::from(n) <= angle_tol)
108 .unwrap_or(MAX_SEGMENTS)
109}