1use crate::curve::interior_knot_count;
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
59
60fn fit_parameter(value: f64, minimum: f64, maximum: f64, closed: bool) -> f64 {
61 if closed {
62 (value - minimum).rem_euclid(maximum - minimum) + minimum
63 } else {
64 value.clamp(minimum, maximum)
65 }
66}
67
68fn solve_three(matrix: [[f64; 3]; 3], rhs: Vec3) -> Option<[f64; 3]> {
69 let a = matrix;
70 let determinant = a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
71 - a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
72 + a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]);
73 let scale: f64 = a.iter().flatten().map(|value| value.abs()).sum();
74 if determinant.abs() <= 1e-14 * scale.max(1.0).powi(3) {
75 return None;
76 }
77 let x = (rhs.x * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
78 - a[0][1] * (rhs.y * a[2][2] - a[1][2] * rhs.z)
79 + a[0][2] * (rhs.y * a[2][1] - a[1][1] * rhs.z))
80 / determinant;
81 let y = (a[0][0] * (rhs.y * a[2][2] - a[1][2] * rhs.z)
82 - rhs.x * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
83 + a[0][2] * (a[1][0] * rhs.z - rhs.y * a[2][0]))
84 / determinant;
85 let z = (a[0][0] * (a[1][1] * rhs.z - rhs.y * a[2][1])
86 - a[0][1] * (a[1][0] * rhs.z - rhs.y * a[2][0])
87 + rhs.x * (a[1][0] * a[2][1] - a[1][1] * a[2][0]))
88 / determinant;
89 Some([x, y, z])
90}
91
92fn newton_curve_surface(
93 curve: &NurbsCurve,
94 surface: &NurbsSurface,
95 seed: f64,
96) -> Result<[f64; 3], String> {
97 let [t0, t1] = curve.domain()?;
98 let [u0, u1] = surface.domain_u()?;
99 let [v0, v1] = surface.domain_v()?;
100 let (closed_u, closed_v) = surface.closed_directions()?;
101 let mut t = seed;
102 let projection = project_point_to_surface(surface, curve.evaluate(t)?)?;
103 let mut u = projection.u;
104 let mut v = projection.v;
105 for _ in 0..60 {
106 let (curve_point, tangent) = curve.deriv1(t)?;
107 let (surface_point, du, dv) = surface.deriv1(u, v)?;
108 let residual = curve_point.sub(surface_point);
109 if residual.length() <= EPSILON {
110 return Ok([t, u, v]);
111 }
112 let Some([mut step_t, mut step_u, mut step_v]) = solve_three(
113 [
114 [tangent.x, -du.x, -dv.x],
115 [tangent.y, -du.y, -dv.y],
116 [tangent.z, -du.z, -dv.z],
117 ],
118 residual.scale(-1.0),
119 ) else {
120 if tangent.length_squared() <= EPSILON {
121 return Ok([t, u, v]);
122 }
123 let next_t = (t - residual.dot(tangent) / tangent.length_squared()).clamp(t0, t1);
124 if (next_t - t).abs() <= 1e-15 * (t1 - t0) {
125 return Ok([t, u, v]);
126 }
127 t = next_t;
128 let projection = project_point_to_surface(surface, curve.evaluate(t)?)?;
129 u = projection.u;
130 v = projection.v;
131 continue;
132 };
133 step_t = step_t.clamp(-(t1 - t0) / 4.0, (t1 - t0) / 4.0);
134 step_u = step_u.clamp(-(u1 - u0) / 4.0, (u1 - u0) / 4.0);
135 step_v = step_v.clamp(-(v1 - v0) / 4.0, (v1 - v0) / 4.0);
136 let next_t = (t + step_t).clamp(t0, t1);
137 let next_u = fit_parameter(u + step_u, u0, u1, closed_u);
138 let next_v = fit_parameter(v + step_v, v0, v1, closed_v);
139 let stalled = (next_t - t).abs() <= 1e-15 * (t1 - t0)
140 && (next_u - u).abs() <= 1e-15 * (u1 - u0)
141 && (next_v - v).abs() <= 1e-15 * (v1 - v0);
142 t = next_t;
143 u = next_u;
144 v = next_v;
145 if stalled {
146 return Ok([t, u, v]);
147 }
148 }
149 Ok([t, u, v])
150}
151
152#[derive(Clone, Copy, Debug, Serialize)]
153pub struct CurveSurfaceIntersection {
154 pub t: f64,
155 pub u: f64,
156 pub v: f64,
157 pub point: Vec3,
158 pub gap: f64,
159 pub tangential: bool,
160}
161
162pub fn intersect_curve_surface(
163 curve: &NurbsCurve,
164 surface: &NurbsSurface,
165 tolerance: f64,
166) -> Result<Vec<CurveSurfaceIntersection>, String> {
167 let [t0, t1] = curve.domain()?;
168 let surface_box = surface_bounds(surface)?.expanded(tolerance * 10.0);
169 let sample_count =
170 32usize.max((interior_knot_count(&curve.knots, curve.degree) + 1) * (curve.degree + 1) * 4);
171 let mut samples = Vec::with_capacity(sample_count + 1);
172 for index in 0..=sample_count {
173 let t = t0 + (t1 - t0) * index as f64 / sample_count as f64;
174 samples.push((t, curve.evaluate(t)?));
175 }
176 let mut seeds = Vec::new();
177 for pair in samples.windows(2) {
178 let segment_box = Bounds::from_points([pair[0].1, pair[1].1]).expanded(tolerance * 10.0);
179 if segment_box.intersects(surface_box) {
180 seeds.push((pair[0].0 + pair[1].0) * 0.5);
181 }
182 }
183 let mut results: Vec<CurveSurfaceIntersection> = Vec::new();
184 for seed in seeds {
185 let [t, u, v] = newton_curve_surface(curve, surface, seed)?;
186 let on_curve = curve.evaluate(t)?;
187 let on_surface = surface.evaluate(u, v)?;
188 let gap = on_curve.sub(on_surface).length();
189 if gap > tolerance {
190 continue;
191 }
192 let tangent = curve.deriv1(t)?.1;
193 let parameter_tolerance =
194 ((tolerance / tangent.length().max(EPSILON)) * 10.0).max((t1 - t0) * 1e-9);
195 if results.iter().any(|result| {
196 (result.t - t).abs() <= parameter_tolerance
197 || result.point.sub(on_curve).length() <= tolerance * 10.0
198 }) {
199 continue;
200 }
201 let normal = surface.normal(u, v).ok();
202 let tangential = normal
203 .and_then(|normal| {
204 tangent
205 .normalized()
206 .ok()
207 .map(|unit| normal.dot(unit).abs() <= 1e-3)
208 })
209 .unwrap_or(false);
210 results.push(CurveSurfaceIntersection {
211 t,
212 u,
213 v,
214 point: on_curve.add(on_surface).scale(0.5),
215 gap,
216 tangential,
217 });
218 }
219 results.sort_by(|a, b| a.t.total_cmp(&b.t));
220 Ok(results)
221}
222
223