Another project
1use bone_types::{Angle, Parameter, Plane3, Point2, Point3, Tolerance, UnitVec3, Vec3};
2use uom::si::angle::radian;
3
4use crate::arc2::Arc2;
5use crate::arc3::Arc3;
6use crate::circle2::Circle2;
7use crate::circle3::Circle3;
8use crate::circular3;
9use crate::curve2::Curve2;
10use crate::curve3::{Curve3, Curve3Kind};
11use crate::intersect::{IntersectionSet, IntersectionSet2, intersect_curves};
12use crate::line2::Line2;
13use crate::line3::Line3;
14
15pub type IntersectionSet3 = IntersectionSet<Point3>;
16
17#[must_use]
18pub fn intersect_curves_3(
19 a: &Curve3Kind,
20 b: &Curve3Kind,
21 tolerance: Tolerance,
22) -> IntersectionSet3 {
23 use Curve3Kind::{Arc, Circle, Line};
24 match (a, b) {
25 (Line(l), Line(m)) => line_line(l, m, tolerance),
26 (Line(l), Circle(c)) | (Circle(c), Line(l)) => line_circle(l, c, tolerance),
27 (Line(l), Arc(a)) | (Arc(a), Line(l)) => line_arc(l, a, tolerance),
28 (Circle(c), Circle(d)) => circle_circle(c, d, tolerance),
29 (Circle(c), Arc(a)) | (Arc(a), Circle(c)) => circle_arc(c, a, tolerance),
30 (Arc(a), Arc(b)) => arc_arc(a, b, tolerance),
31 }
32}
33
34#[must_use]
35fn unit_to_vec(u: UnitVec3) -> Vec3 {
36 let (x, y, z) = u.components();
37 Vec3::from_mm(x, y, z)
38}
39
40#[must_use]
41fn project(plane: Plane3, p: Point3) -> Point2 {
42 let (px, py, _) = circular3::local_coords(plane, p);
43 Point2::from_mm(px, py)
44}
45
46#[must_use]
47fn lift(plane: Plane3, p: Point2) -> Point3 {
48 let (px, py) = p.coords_mm();
49 let (ox, oy, oz) = plane.origin().coords_mm();
50 let (xx, xy, xz) = plane.x_axis().components();
51 let (yx, yy, yz) = plane.y_axis().components();
52 Point3::from_mm(
53 ox + px * xx + py * yx,
54 oy + px * xy + py * yy,
55 oz + px * xz + py * yz,
56 )
57}
58
59#[must_use]
60fn lift_set(plane: Plane3, set: IntersectionSet2) -> IntersectionSet3 {
61 match set {
62 IntersectionSet::Empty => IntersectionSet3::Empty,
63 IntersectionSet::One(p) => IntersectionSet3::One(lift(plane, p)),
64 IntersectionSet::Two(a, b) => IntersectionSet3::Two(lift(plane, a), lift(plane, b)),
65 IntersectionSet::Coincident => IntersectionSet3::Coincident,
66 }
67}
68
69#[must_use]
70fn keep_points(set: IntersectionSet3, keep: impl Fn(Point3) -> bool) -> IntersectionSet3 {
71 match set {
72 IntersectionSet3::Empty => IntersectionSet3::Empty,
73 IntersectionSet3::Coincident => IntersectionSet3::Coincident,
74 IntersectionSet3::One(p) => {
75 if keep(p) {
76 IntersectionSet3::One(p)
77 } else {
78 IntersectionSet3::Empty
79 }
80 }
81 IntersectionSet3::Two(a, b) => match (keep(a), keep(b)) {
82 (true, true) => IntersectionSet3::Two(a, b),
83 (true, false) => IntersectionSet3::One(a),
84 (false, true) => IntersectionSet3::One(b),
85 (false, false) => IntersectionSet3::Empty,
86 },
87 }
88}
89
90fn line_line(a: &Line3, b: &Line3, tolerance: Tolerance) -> IntersectionSet3 {
91 let (pa, pb) = (a.start(), b.start());
92 let dir_a = a.end() - a.start();
93 let dir_b = b.end() - b.start();
94 let eps = tolerance.value();
95 let (la, lb) = (a.length_mm(), b.length_mm());
96 let aa = dir_a.dot_mm2(dir_a);
97 let ab = dir_a.dot_mm2(dir_b);
98 let bb = dir_b.dot_mm2(dir_b);
99 let offset = pa - pb;
100 let dot_a = dir_a.dot_mm2(offset);
101 let dot_b = dir_b.dot_mm2(offset);
102 let denom = aa * bb - ab * ab;
103
104 if dir_a.cross(dir_b).norm_mm() <= eps * la.min(lb) {
105 let between = pb - pa;
106 let along = between.dot_mm2(dir_a) / aa;
107 if (between - dir_a * along).norm_mm() > eps {
108 return IntersectionSet3::Empty;
109 }
110 let other_end = (b.end() - pa).dot_mm2(dir_a) / aa;
111 let (lo, hi) = if along <= other_end {
112 (along, other_end)
113 } else {
114 (other_end, along)
115 };
116 let overlap_lo = lo.max(0.0);
117 let overlap_hi = hi.min(1.0);
118 let eps_param = eps / la;
119 if overlap_hi < overlap_lo - eps_param {
120 return IntersectionSet3::Empty;
121 }
122 if overlap_hi <= overlap_lo + eps_param {
123 return IntersectionSet3::One(pa + dir_a * f64::midpoint(overlap_lo, overlap_hi));
124 }
125 return IntersectionSet3::Coincident;
126 }
127
128 let param_a = (ab * dot_b - bb * dot_a) / denom;
129 let param_b = (aa * dot_b - ab * dot_a) / denom;
130 let (eps_a, eps_b) = (eps / la, eps / lb);
131 if param_a < -eps_a || param_a > 1.0 + eps_a || param_b < -eps_b || param_b > 1.0 + eps_b {
132 return IntersectionSet3::Empty;
133 }
134 let on_a = pa + dir_a * param_a;
135 let on_b = pb + dir_b * param_b;
136 if (on_b - on_a).norm_mm() <= eps {
137 IntersectionSet3::One(on_a + (on_b - on_a) * 0.5)
138 } else {
139 IntersectionSet3::Empty
140 }
141}
142
143fn line_circle(line: &Line3, circle: &Circle3, tolerance: Tolerance) -> IntersectionSet3 {
144 let plane = circle.plane();
145 let normal = unit_to_vec(plane.normal());
146 let u = line.end() - line.start();
147 let eps = tolerance.value();
148 let r = circle.radius_mm();
149 let un = u.dot_mm2(normal);
150 let dist_start = (line.start() - plane.origin()).dot_mm2(normal);
151
152 if un.abs() <= eps {
153 if dist_start.abs() > eps {
154 return IntersectionSet3::Empty;
155 }
156 let (Ok(l2), Ok(c2)) = (
157 Line2::new(
158 project(plane, line.start()),
159 project(plane, line.end()),
160 tolerance,
161 ),
162 Circle2::new(Point2::origin(), circle.radius(), tolerance),
163 ) else {
164 return IntersectionSet3::Empty;
165 };
166 return lift_set(
167 plane,
168 intersect_curves(&l2.as_kind(), &c2.as_kind(), tolerance),
169 );
170 }
171
172 let s = -dist_start / un;
173 let eps_s = eps / line.length_mm();
174 if s < -eps_s || s > 1.0 + eps_s {
175 return IntersectionSet3::Empty;
176 }
177 let x = line.start() + u * s;
178 let radial = (x - plane.origin()).norm_mm();
179 if (radial - r).abs() <= eps {
180 IntersectionSet3::One(x)
181 } else {
182 IntersectionSet3::Empty
183 }
184}
185
186fn line_arc(line: &Line3, arc: &Arc3, tolerance: Tolerance) -> IntersectionSet3 {
187 let set = line_circle(line, &arc.as_full_circle(), tolerance);
188 keep_points(set, |p| arc.contains_point(p, tolerance))
189}
190
191fn circle_circle(a: &Circle3, b: &Circle3, tolerance: Tolerance) -> IntersectionSet3 {
192 let (pa, pb) = (a.plane(), b.plane());
193 let na = unit_to_vec(pa.normal());
194 let nb = unit_to_vec(pb.normal());
195 let eps = tolerance.value();
196 let cross = na.cross(nb);
197
198 if cross.norm_mm() <= eps {
199 if (pb.origin() - pa.origin()).dot_mm2(na).abs() > eps {
200 return IntersectionSet3::Empty;
201 }
202 let (Ok(a2), Ok(b2)) = (
203 Circle2::new(Point2::origin(), a.radius(), tolerance),
204 Circle2::new(project(pa, pb.origin()), b.radius(), tolerance),
205 ) else {
206 return IntersectionSet3::Empty;
207 };
208 return lift_set(
209 pa,
210 intersect_curves(&a2.as_kind(), &b2.as_kind(), tolerance),
211 );
212 }
213
214 let axis_u = unit_to_vec(pa.x_axis());
215 let axis_v = unit_to_vec(pa.y_axis());
216 let ra = a.radius_mm();
217 let coef_cos = ra * nb.dot_mm2(axis_u);
218 let coef_sin = ra * nb.dot_mm2(axis_v);
219 let rhs = nb.dot_mm2(b.center() - a.center());
220 let amp = (coef_cos * coef_cos + coef_sin * coef_sin).sqrt();
221 if rhs.abs() > amp + eps {
222 return IntersectionSet3::Empty;
223 }
224 let phi = coef_sin.atan2(coef_cos);
225 let delta = (rhs / amp).clamp(-1.0, 1.0).acos();
226 let on_b =
227 |candidate: Point3| ((candidate - b.center()).norm_mm() - b.radius_mm()).abs() <= eps;
228 let point_at = |t: f64| a.center() + axis_u * (ra * t.cos()) + axis_v * (ra * t.sin());
229 let mut hits = [phi - delta, phi + delta]
230 .into_iter()
231 .map(point_at)
232 .filter(|&candidate| on_b(candidate));
233 match (hits.next(), hits.next()) {
234 (None, _) => IntersectionSet3::Empty,
235 (Some(p), None) => IntersectionSet3::One(p),
236 (Some(p), Some(q)) => {
237 if (q - p).norm_mm() <= eps {
238 IntersectionSet3::One(p)
239 } else {
240 IntersectionSet3::Two(p, q)
241 }
242 }
243 }
244}
245
246fn circle_arc(circle: &Circle3, arc: &Arc3, tolerance: Tolerance) -> IntersectionSet3 {
247 let set = circle_circle(circle, &arc.as_full_circle(), tolerance);
248 keep_points(set, |p| arc.contains_point(p, tolerance))
249}
250
251fn arc_arc(a: &Arc3, b: &Arc3, tolerance: Tolerance) -> IntersectionSet3 {
252 match circle_circle(&a.as_full_circle(), &b.as_full_circle(), tolerance) {
253 IntersectionSet3::Coincident => coincident_arc_arc(a, b, tolerance),
254 other => keep_points(other, |p| {
255 a.contains_point(p, tolerance) && b.contains_point(p, tolerance)
256 }),
257 }
258}
259
260fn coincident_arc_arc(a: &Arc3, b: &Arc3, tolerance: Tolerance) -> IntersectionSet3 {
261 let plane = a.plane();
262 let start_b = b.evaluate(Parameter::new(0.0));
263 let (bx, by, _) = circular3::local_coords(plane, start_b);
264 let alpha0 = by.atan2(bx);
265 let winding = unit_to_vec(plane.normal()).dot_mm2(unit_to_vec(b.plane().normal()));
266 let sweep_b = b.sweep_rad() * if winding >= 0.0 { 1.0 } else { -1.0 };
267
268 let (Ok(a2), Ok(b2)) = (
269 Arc2::new(
270 Point2::origin(),
271 a.radius(),
272 a.start_angle(),
273 a.sweep_angle(),
274 tolerance,
275 ),
276 Arc2::new(
277 Point2::origin(),
278 a.radius(),
279 Angle::new::<radian>(alpha0),
280 Angle::new::<radian>(sweep_b),
281 tolerance,
282 ),
283 ) else {
284 return IntersectionSet3::Coincident;
285 };
286 lift_set(
287 plane,
288 intersect_curves(&a2.as_kind(), &b2.as_kind(), tolerance),
289 )
290}