brep_kernel/intersect/
curve_curve_intersection.rs

1use crate::curve::KNOT_IDENTITY_TOL as KNOT_TOLERANCE;
2use crate::{NurbsCurve, 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_curve(curve: &NurbsCurve) -> Result<Self, String> {
15        let points = curve
16            .control_points
17            .iter()
18            .map(|point| point.point())
19            .collect::<Result<Vec<_>, _>>()?;
20        let mut minimum = Vec3::new(f64::INFINITY, f64::INFINITY, f64::INFINITY);
21        let mut maximum = Vec3::new(f64::NEG_INFINITY, f64::NEG_INFINITY, f64::NEG_INFINITY);
22        for point in points {
23            minimum.x = minimum.x.min(point.x);
24            minimum.y = minimum.y.min(point.y);
25            minimum.z = minimum.z.min(point.z);
26            maximum.x = maximum.x.max(point.x);
27            maximum.y = maximum.y.max(point.y);
28            maximum.z = maximum.z.max(point.z);
29        }
30        Ok(Self { minimum, maximum })
31    }
32
33    fn intersects(self, other: Self, tolerance: f64) -> bool {
34        self.minimum.x <= other.maximum.x + tolerance
35            && self.maximum.x + tolerance >= other.minimum.x
36            && self.minimum.y <= other.maximum.y + tolerance
37            && self.maximum.y + tolerance >= other.minimum.y
38            && self.minimum.z <= other.maximum.z + tolerance
39            && self.maximum.z + tolerance >= other.minimum.z
40    }
41}
42
43#[derive(Clone, Copy)]
44struct Segment {
45    start: f64,
46    end: f64,
47    bounds: Bounds,
48}
49
50fn interior_knots(curve: &NurbsCurve) -> Vec<f64> {
51    let start = curve.knots[curve.degree];
52    let end = curve.knots[curve.knots.len() - 1 - curve.degree];
53    let mut result = Vec::new();
54    for &knot in &curve.knots {
55        if knot <= start + KNOT_TOLERANCE || knot >= end - KNOT_TOLERANCE {
56            continue;
57        }
58        if result
59            .last()
60            .is_none_or(|previous: &f64| (*previous - knot).abs() > KNOT_TOLERANCE)
61        {
62            result.push(knot);
63        }
64    }
65    result
66}
67
68fn segment_boxes(curve: &NurbsCurve, count: usize) -> Result<Vec<Segment>, String> {
69    let [start, end] = curve.domain()?;
70    let mut parameters: Vec<f64> = (0..=count)
71        .map(|index| start + (end - start) * index as f64 / count as f64)
72        .collect();
73    parameters.extend(interior_knots(curve));
74    parameters.sort_by(f64::total_cmp);
75    parameters.dedup_by(|a, b| (*a - *b).abs() <= KNOT_TOLERANCE);
76
77    let mut segments = Vec::new();
78    let mut rest = curve.clone();
79    let mut rest_start = start;
80    for (index, &parameter) in parameters.iter().enumerate().skip(1) {
81        if parameter - rest_start <= KNOT_TOLERANCE {
82            continue;
83        }
84        if index == parameters.len() - 1 {
85            segments.push(Segment {
86                start: rest_start,
87                end,
88                bounds: Bounds::from_curve(&rest)?,
89            });
90        } else {
91            let (piece, remainder) = rest.split(parameter)?;
92            segments.push(Segment {
93                start: rest_start,
94                end: parameter,
95                bounds: Bounds::from_curve(&piece)?,
96            });
97            rest = remainder;
98            rest_start = parameter;
99        }
100    }
101    Ok(segments)
102}
103
104fn newton_closest_pair(
105    first: &NurbsCurve,
106    second: &NurbsCurve,
107    seed_s: f64,
108    seed_t: f64,
109) -> Result<[f64; 2], String> {
110    let [s0, s1] = first.domain()?;
111    let [t0, t1] = second.domain()?;
112    let mut s = seed_s;
113    let mut t = seed_t;
114    for _ in 0..40 {
115        let first_derivatives = first.derivatives_small(s, 2)?;
116        let second_derivatives = second.derivatives_small(t, 2)?;
117        let residual = first_derivatives[0].sub(second_derivatives[0]);
118        let f = first_derivatives[1].dot(residual);
119        let g = -second_derivatives[1].dot(residual);
120        let j00 = first_derivatives[2].dot(residual) + first_derivatives[1].length_squared();
121        let j01 = -first_derivatives[1].dot(second_derivatives[1]);
122        let j10 = -second_derivatives[1].dot(first_derivatives[1]);
123        let j11 = -second_derivatives[2].dot(residual) + second_derivatives[1].length_squared();
124        let residual_length = residual.length();
125        let converged_first =
126            f.abs() <= EPSILON + 1e-10 * first_derivatives[1].length() * residual_length.max(1e-30);
127        let converged_second = g.abs()
128            <= EPSILON + 1e-10 * second_derivatives[1].length() * residual_length.max(1e-30);
129        if (converged_first && converged_second) || residual_length <= EPSILON {
130            return Ok([s, t]);
131        }
132        let determinant = j00 * j11 - j01 * j10;
133        if determinant.abs() <= EPSILON {
134            return Ok([s, t]);
135        }
136        let mut ds = (-f * j11 + g * j01) / determinant;
137        let mut dt = (-g * j00 + f * j10) / determinant;
138        ds = ds.clamp(-(s1 - s0) / 4.0, (s1 - s0) / 4.0);
139        dt = dt.clamp(-(t1 - t0) / 4.0, (t1 - t0) / 4.0);
140        let next_s = (s + ds).clamp(s0, s1);
141        let next_t = (t + dt).clamp(t0, t1);
142        if (next_s - s).abs() <= 1e-15 * (s1 - s0) && (next_t - t).abs() <= 1e-15 * (t1 - t0) {
143            return Ok([next_s, next_t]);
144        }
145        s = next_s;
146        t = next_t;
147    }
148    Ok([s, t])
149}
150
151fn parameter_tolerance(curve: &NurbsCurve, parameter: f64, tolerance: f64) -> Result<f64, String> {
152    let speed = curve.deriv1(parameter)?.1.length();
153    let [start, end] = curve.domain()?;
154    if speed <= EPSILON {
155        Ok((end - start) * 1e-3)
156    } else {
157        Ok(((tolerance / speed) * 10.0).max((end - start) * 1e-9))
158    }
159}
160
161fn parameter_distance(a: f64, b: f64, start: f64, end: f64, closed: bool) -> f64 {
162    let distance = (a - b).abs();
163    if closed {
164        distance.min(end - start - distance)
165    } else {
166        distance
167    }
168}
169
170#[derive(Clone, Copy, Debug, Serialize)]
171pub struct CurveCurveIntersection {
172    pub s: f64,
173    pub t: f64,
174    pub point: Vec3,
175    pub gap: f64,
176    pub tangential: bool,
177}
178
179pub fn intersect_curves(
180    first: &NurbsCurve,
181    second: &NurbsCurve,
182    tolerance: f64,
183) -> Result<Vec<CurveCurveIntersection>, String> {
184    let first_segment_count = 8usize.max((interior_knots(first).len() + 1) * (first.degree + 1));
185    let second_segment_count = 8usize.max((interior_knots(second).len() + 1) * (second.degree + 1));
186    let first_segments = segment_boxes(first, first_segment_count)?;
187    let second_segments = segment_boxes(second, second_segment_count)?;
188    let mut seeds = Vec::new();
189    for first_segment in &first_segments {
190        for second_segment in &second_segments {
191            if first_segment
192                .bounds
193                .intersects(second_segment.bounds, tolerance)
194            {
195                seeds.push([
196                    (first_segment.start + first_segment.end) * 0.5,
197                    (second_segment.start + second_segment.end) * 0.5,
198                ]);
199            }
200        }
201    }
202    let [s0, s1] = first.domain()?;
203    let [t0, t1] = second.domain()?;
204    let closed_first = first.evaluate(s0)?.sub(first.evaluate(s1)?).length() <= tolerance;
205    let closed_second = second.evaluate(t0)?.sub(second.evaluate(t1)?).length() <= tolerance;
206    let mut results: Vec<CurveCurveIntersection> = Vec::new();
207    for [seed_s, seed_t] in seeds {
208        let [s, t] = newton_closest_pair(first, second, seed_s, seed_t)?;
209        let first_point = first.evaluate(s)?;
210        let second_point = second.evaluate(t)?;
211        let gap = first_point.sub(second_point).length();
212        if gap > tolerance {
213            continue;
214        }
215        let first_tangent = first.deriv1(s)?.1;
216        let second_tangent = second.deriv1(t)?.1;
217        let tangential = match (first_tangent.normalized(), second_tangent.normalized()) {
218            (Ok(a), Ok(b)) => a.cross(b).length() <= 1e-3_f64.sin(),
219            _ => false,
220        };
221        let point = first_point.add(second_point).scale(0.5);
222        let scale = 1.0 + point.length();
223        let spatial_tolerance = if tangential {
224            tolerance.sqrt() * scale
225        } else {
226            tolerance * 10.0
227        };
228        let mut duplicate = false;
229        for result in &results {
230            if (parameter_distance(result.s, s, s0, s1, closed_first)
231                <= parameter_tolerance(first, result.s, tolerance)?
232                && parameter_distance(result.t, t, t0, t1, closed_second)
233                    <= parameter_tolerance(second, result.t, tolerance)?)
234                || result.point.sub(point).length()
235                    <= if result.tangential || tangential {
236                        spatial_tolerance
237                    } else {
238                        tolerance * 10.0
239                    }
240            {
241                duplicate = true;
242                break;
243            }
244        }
245        if duplicate {
246            continue;
247        }
248        results.push(CurveCurveIntersection {
249            s: s.clamp(s0, s1),
250            t: t.clamp(t0, t1),
251            point,
252            gap,
253            tangential,
254        });
255    }
256    results.sort_by(|a, b| a.s.total_cmp(&b.s));
257    Ok(results)
258}
259
260// BREP private tests: 53828ae9ea2f3766
brep_kernel/intersect/curve_curve_intersection.rs

brep_kernel/intersect/
curve_curve_intersection.rs
