1use crate::curve::KNOT_DEDUP_EPS;
2use crate::{project_point_to_surface, NurbsCurve, NurbsSurface, Vec3};
3use serde::Serialize;
4
5const EPSILON: f64 = 1e-12;
6
7#[derive(Clone, Copy)]
8struct Bounds {
9 minimum: Vec3,
10 maximum: Vec3,
11}
12
13impl Bounds {
14 fn from_points(points: impl IntoIterator<Item = Vec3>) -> Self {
15 let mut bounds = Self {
16 minimum: Vec3::new(f64::INFINITY, f64::INFINITY, f64::INFINITY),
17 maximum: Vec3::new(f64::NEG_INFINITY, f64::NEG_INFINITY, f64::NEG_INFINITY),
18 };
19 for point in points {
20 bounds.minimum.x = bounds.minimum.x.min(point.x);
21 bounds.minimum.y = bounds.minimum.y.min(point.y);
22 bounds.minimum.z = bounds.minimum.z.min(point.z);
23 bounds.maximum.x = bounds.maximum.x.max(point.x);
24 bounds.maximum.y = bounds.maximum.y.max(point.y);
25 bounds.maximum.z = bounds.maximum.z.max(point.z);
26 }
27 bounds
28 }
29
30 fn expanded(self, amount: f64) -> Self {
31 let delta = Vec3::new(amount, amount, amount);
32 Self {
33 minimum: self.minimum.sub(delta),
34 maximum: self.maximum.add(delta),
35 }
36 }
37
38 fn intersects(self, other: Self) -> bool {
39 self.minimum.x <= other.maximum.x
40 && self.maximum.x >= other.minimum.x
41 && self.minimum.y <= other.maximum.y
42 && self.maximum.y >= other.minimum.y
43 && self.minimum.z <= other.maximum.z
44 && self.maximum.z >= other.minimum.z
45 }
46}
47
48fn surface_bounds(surface: &NurbsSurface) -> Result<Bounds, String> {
49 Ok(Bounds::from_points(
50 surface
51 .control_points
52 .iter()
53 .flatten()
54 .map(|point| point.point())
55 .collect::<Result<Vec<_>, _>>()?,
56 ))
57}
58
59fn interior_knot_count(knots: &[f64], degree: usize) -> usize {
60 let start = knots[degree];
61 let end = knots[knots.len() - 1 - degree];
62 let mut count = 0;
63 let mut previous = None;
64 for &knot in knots {
65 if knot <= start + KNOT_DEDUP_EPS || knot >= end - KNOT_DEDUP_EPS {
67 continue;
68 }
69 if previous.is_none_or(|value: f64| (value - knot).abs() > KNOT_DEDUP_EPS) {
70 count += 1;
71 previous = Some(knot);
72 }
73 }
74 count
75}
76
77fn fit_parameter(value: f64, minimum: f64, maximum: f64, closed: bool) -> f64 {
78 if closed {
79 (value - minimum).rem_euclid(maximum - minimum) + minimum
80 } else {
81 value.clamp(minimum, maximum)
82 }
83}
84
85fn solve_three(matrix: [[f64; 3]; 3], rhs: Vec3) -> Option<[f64; 3]> {
86 let a = matrix;
87 let determinant = a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
88 - a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
89 + a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]);
90 let scale: f64 = a.iter().flatten().map(|value| value.abs()).sum();
91 if determinant.abs() <= 1e-14 * scale.max(1.0).powi(3) {
92 return None;
93 }
94 let x = (rhs.x * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
95 - a[0][1] * (rhs.y * a[2][2] - a[1][2] * rhs.z)
96 + a[0][2] * (rhs.y * a[2][1] - a[1][1] * rhs.z))
97 / determinant;
98 let y = (a[0][0] * (rhs.y * a[2][2] - a[1][2] * rhs.z)
99 - rhs.x * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
100 + a[0][2] * (a[1][0] * rhs.z - rhs.y * a[2][0]))
101 / determinant;
102 let z = (a[0][0] * (a[1][1] * rhs.z - rhs.y * a[2][1])
103 - a[0][1] * (a[1][0] * rhs.z - rhs.y * a[2][0])
104 + rhs.x * (a[1][0] * a[2][1] - a[1][1] * a[2][0]))
105 / determinant;
106 Some([x, y, z])
107}
108
109fn newton_curve_surface(
110 curve: &NurbsCurve,
111 surface: &NurbsSurface,
112 seed: f64,
113) -> Result<[f64; 3], String> {
114 let [t0, t1] = curve.domain()?;
115 let [u0, u1] = surface.domain_u()?;
116 let [v0, v1] = surface.domain_v()?;
117 let (closed_u, closed_v) = surface.closed_directions()?;
118 let mut t = seed;
119 let projection = project_point_to_surface(surface, curve.evaluate(t)?)?;
120 let mut u = projection.u;
121 let mut v = projection.v;
122 for _ in 0..60 {
123 let (curve_point, tangent) = curve.deriv1(t)?;
124 let (surface_point, du, dv) = surface.deriv1(u, v)?;
125 let residual = curve_point.sub(surface_point);
126 if residual.length() <= EPSILON {
127 return Ok([t, u, v]);
128 }
129 let Some([mut step_t, mut step_u, mut step_v]) = solve_three(
130 [
131 [tangent.x, -du.x, -dv.x],
132 [tangent.y, -du.y, -dv.y],
133 [tangent.z, -du.z, -dv.z],
134 ],
135 residual.scale(-1.0),
136 ) else {
137 if tangent.length_squared() <= EPSILON {
138 return Ok([t, u, v]);
139 }
140 let next_t = (t - residual.dot(tangent) / tangent.length_squared()).clamp(t0, t1);
141 if (next_t - t).abs() <= 1e-15 * (t1 - t0) {
142 return Ok([t, u, v]);
143 }
144 t = next_t;
145 let projection = project_point_to_surface(surface, curve.evaluate(t)?)?;
146 u = projection.u;
147 v = projection.v;
148 continue;
149 };
150 step_t = step_t.clamp(-(t1 - t0) / 4.0, (t1 - t0) / 4.0);
151 step_u = step_u.clamp(-(u1 - u0) / 4.0, (u1 - u0) / 4.0);
152 step_v = step_v.clamp(-(v1 - v0) / 4.0, (v1 - v0) / 4.0);
153 let next_t = (t + step_t).clamp(t0, t1);
154 let next_u = fit_parameter(u + step_u, u0, u1, closed_u);
155 let next_v = fit_parameter(v + step_v, v0, v1, closed_v);
156 let stalled = (next_t - t).abs() <= 1e-15 * (t1 - t0)
157 && (next_u - u).abs() <= 1e-15 * (u1 - u0)
158 && (next_v - v).abs() <= 1e-15 * (v1 - v0);
159 t = next_t;
160 u = next_u;
161 v = next_v;
162 if stalled {
163 return Ok([t, u, v]);
164 }
165 }
166 Ok([t, u, v])
167}
168
169#[derive(Clone, Copy, Debug, Serialize)]
170pub struct CurveSurfaceIntersection {
171 pub t: f64,
172 pub u: f64,
173 pub v: f64,
174 pub point: Vec3,
175 pub gap: f64,
176 pub tangential: bool,
177}
178
179pub fn intersect_curve_surface(
180 curve: &NurbsCurve,
181 surface: &NurbsSurface,
182 tolerance: f64,
183) -> Result<Vec<CurveSurfaceIntersection>, String> {
184 let [t0, t1] = curve.domain()?;
185 let surface_box = surface_bounds(surface)?.expanded(tolerance * 10.0);
186 let sample_count =
187 32usize.max((interior_knot_count(&curve.knots, curve.degree) + 1) * (curve.degree + 1) * 4);
188 let mut samples = Vec::with_capacity(sample_count + 1);
189 for index in 0..=sample_count {
190 let t = t0 + (t1 - t0) * index as f64 / sample_count as f64;
191 samples.push((t, curve.evaluate(t)?));
192 }
193 let mut seeds = Vec::new();
194 for pair in samples.windows(2) {
195 let segment_box = Bounds::from_points([pair[0].1, pair[1].1]).expanded(tolerance * 10.0);
196 if segment_box.intersects(surface_box) {
197 seeds.push((pair[0].0 + pair[1].0) * 0.5);
198 }
199 }
200 let mut results: Vec<CurveSurfaceIntersection> = Vec::new();
201 for seed in seeds {
202 let [t, u, v] = newton_curve_surface(curve, surface, seed)?;
203 let on_curve = curve.evaluate(t)?;
204 let on_surface = surface.evaluate(u, v)?;
205 let gap = on_curve.sub(on_surface).length();
206 if gap > tolerance {
207 continue;
208 }
209 let tangent = curve.deriv1(t)?.1;
210 let parameter_tolerance =
211 ((tolerance / tangent.length().max(EPSILON)) * 10.0).max((t1 - t0) * 1e-9);
212 if results.iter().any(|result| {
213 (result.t - t).abs() <= parameter_tolerance
214 || result.point.sub(on_curve).length() <= tolerance * 10.0
215 }) {
216 continue;
217 }
218 let normal = surface.normal(u, v).ok();
219 let tangential = normal
220 .and_then(|normal| {
221 tangent
222 .normalized()
223 .ok()
224 .map(|unit| normal.dot(unit).abs() <= 1e-3)
225 })
226 .unwrap_or(false);
227 results.push(CurveSurfaceIntersection {
228 t,
229 u,
230 v,
231 point: on_curve.add(on_surface).scale(0.5),
232 gap,
233 tangential,
234 });
235 }
236 results.sort_by(|a, b| a.t.total_cmp(&b.t));
237 Ok(results)
238}
239
240#[cfg(test)]
241mod tests {
242 use super::*;
243 use crate::{make_line, make_plane};
244
245 #[test]
246 fn line_intersects_plane_patch_once() {
247 let line = make_line(Vec3::new(0.5, 0.5, -2.0), Vec3::new(0.5, 0.5, 2.0)).unwrap();
248 let plane = make_plane(
249 Vec3::default(),
250 Vec3::new(1.0, 0.0, 0.0),
251 Vec3::new(0.0, 1.0, 0.0),
252 1.0,
253 1.0,
254 )
255 .unwrap();
256 let intersections = intersect_curve_surface(&line, &plane, 1e-7).unwrap();
257 assert_eq!(intersections.len(), 1);
258 assert!(
259 intersections[0]
260 .point
261 .sub(Vec3::new(0.5, 0.5, 0.0))
262 .length()
263 < 1e-8
264 );
265 assert!(!intersections[0].tangential);
266 }
267}