1use crate::certified_bezier::{
4 distance_between_point_intervals_upper, next_up, representative_distance, Cell,
5 HomogeneousPoint,
6};
7use crate::certified_projection::{
8 CertifiedProjectionOptions, CurveDistanceCertificate2, CurveDistanceCertificate3,
9 CurvePairParameterBox, ParameterInterval,
10};
11use crate::certified_refinement::{piecewise_bezier_cells, RefinementBudget};
12use axiolid_contracts::{GeomError, GeomResult};
13use axiolid_core::{Point2, Point3, Scalar};
14use axiolid_curve::{BSplineCurve2, BSplineCurve3};
15use axiolid_evaluate::curve::{bspline_jet2, bspline_jet3};
16use std::cmp::Ordering;
17use std::collections::BinaryHeap;
18
19pub fn distance_curve2_certified(
21 first: &BSplineCurve2,
22 second: &BSplineCurve2,
23 options: CertifiedProjectionOptions,
24) -> GeomResult<CurveDistanceCertificate2> {
25 let mut refinement_budget =
26 RefinementBudget::new(options.max_nodes(), "certified curve-pair budget");
27 let first_cells = piecewise_bezier_cells(
28 first,
29 |point| [point.x, point.y, 0.0],
30 &mut refinement_budget,
31 )?;
32 let second_cells = piecewise_bezier_cells(
33 second,
34 |point| [point.x, point.y, 0.0],
35 &mut refinement_budget,
36 )?;
37 let core = distance_core(
38 first_cells,
39 second_cells,
40 2,
41 options,
42 |parameter| {
43 let point = bspline_jet2(first, parameter)?.point;
44 Ok([point.x, point.y, 0.0])
45 },
46 |parameter| {
47 let point = bspline_jet2(second, parameter)?.point;
48 Ok([point.x, point.y, 0.0])
49 },
50 )?;
51 Ok(CurveDistanceCertificate2 {
52 first_parameter: core.first_parameter,
53 second_parameter: core.second_parameter,
54 first_point: Point2::new(core.first_point[0], core.first_point[1]),
55 second_point: Point2::new(core.second_point[0], core.second_point[1]),
56 distance: core.distance,
57 distance_lower_bound: core.lower,
58 distance_upper_bound: core.upper,
59 possible_minimizer_boxes: core.boxes,
60 visited_nodes: core.visited_nodes,
61 })
62}
63
64pub fn distance_curve3_certified(
66 first: &BSplineCurve3,
67 second: &BSplineCurve3,
68 options: CertifiedProjectionOptions,
69) -> GeomResult<CurveDistanceCertificate3> {
70 let mut refinement_budget =
71 RefinementBudget::new(options.max_nodes(), "certified curve-pair budget");
72 let first_cells = piecewise_bezier_cells(
73 first,
74 |point| [point.x, point.y, point.z],
75 &mut refinement_budget,
76 )?;
77 let second_cells = piecewise_bezier_cells(
78 second,
79 |point| [point.x, point.y, point.z],
80 &mut refinement_budget,
81 )?;
82 let core = distance_core(
83 first_cells,
84 second_cells,
85 3,
86 options,
87 |parameter| {
88 let point = bspline_jet3(first, parameter)?.point;
89 Ok([point.x, point.y, point.z])
90 },
91 |parameter| {
92 let point = bspline_jet3(second, parameter)?.point;
93 Ok([point.x, point.y, point.z])
94 },
95 )?;
96 Ok(CurveDistanceCertificate3 {
97 first_parameter: core.first_parameter,
98 second_parameter: core.second_parameter,
99 first_point: Point3::new(
100 core.first_point[0],
101 core.first_point[1],
102 core.first_point[2],
103 ),
104 second_point: Point3::new(
105 core.second_point[0],
106 core.second_point[1],
107 core.second_point[2],
108 ),
109 distance: core.distance,
110 distance_lower_bound: core.lower,
111 distance_upper_bound: core.upper,
112 possible_minimizer_boxes: core.boxes,
113 visited_nodes: core.visited_nodes,
114 })
115}
116
117#[derive(Debug)]
118struct QueueCell {
119 lower: Scalar,
120 serial: u64,
121 first: Cell,
122 second: Cell,
123}
124
125impl PartialEq for QueueCell {
126 fn eq(&self, other: &Self) -> bool {
127 self.serial == other.serial
128 }
129}
130
131impl Eq for QueueCell {}
132
133impl PartialOrd for QueueCell {
134 fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
135 Some(self.cmp(other))
136 }
137}
138
139impl Ord for QueueCell {
140 fn cmp(&self, other: &Self) -> Ordering {
141 other
142 .lower
143 .total_cmp(&self.lower)
144 .then_with(|| other.serial.cmp(&self.serial))
145 }
146}
147
148#[derive(Debug)]
149struct Candidate {
150 first_parameter: Scalar,
151 second_parameter: Scalar,
152 first_point: [Scalar; 3],
153 second_point: [Scalar; 3],
154 distance: Scalar,
155 upper: Scalar,
156}
157
158#[derive(Debug)]
159struct DistanceCore {
160 first_parameter: Scalar,
161 second_parameter: Scalar,
162 first_point: [Scalar; 3],
163 second_point: [Scalar; 3],
164 distance: Scalar,
165 lower: Scalar,
166 upper: Scalar,
167 boxes: Vec<CurvePairParameterBox>,
168 visited_nodes: u32,
169}
170
171fn distance_core(
172 first_cells: Vec<Cell>,
173 second_cells: Vec<Cell>,
174 dimensions: usize,
175 options: CertifiedProjectionOptions,
176 evaluate_first: impl Fn(Scalar) -> GeomResult<[Scalar; 3]>,
177 evaluate_second: impl Fn(Scalar) -> GeomResult<[Scalar; 3]>,
178) -> GeomResult<DistanceCore> {
179 let initial_count =
180 first_cells
181 .len()
182 .checked_mul(second_cells.len())
183 .ok_or(GeomError::BudgetExceeded {
184 resource: "certified curve-pair budget",
185 })?;
186 if initial_count > options.max_nodes() as usize {
187 return Err(GeomError::BudgetExceeded {
188 resource: "certified curve-pair budget",
189 });
190 }
191
192 let mut heap = BinaryHeap::with_capacity(initial_count);
193 let mut best = None;
194 let mut serial = 0_u64;
195 for first in first_cells {
196 for second in &second_cells {
197 consider_cell_samples(
198 &mut best,
199 &first,
200 second,
201 dimensions,
202 &evaluate_first,
203 &evaluate_second,
204 )?;
205 let lower = first.gap(second, dimensions)?;
206 heap.push(QueueCell {
207 lower,
208 serial,
209 first: first.clone(),
210 second: second.clone(),
211 });
212 serial = serial.checked_add(1).ok_or(GeomError::BudgetExceeded {
213 resource: "certified curve-pair budget",
214 })?;
215 }
216 }
217 let mut visited_nodes =
218 u32::try_from(initial_count).map_err(|_| GeomError::BudgetExceeded {
219 resource: "certified curve-pair budget",
220 })?;
221 let mut best = best.ok_or_else(|| GeomError::Degenerate("no pair candidate".to_owned()))?;
222
223 loop {
224 while heap.peek().is_some_and(|cell| cell.lower > best.upper) {
225 heap.pop();
226 }
227 let global_lower = heap
228 .peek()
229 .map_or(best.upper, |cell| cell.lower.min(best.upper));
230 if best.upper - global_lower <= options.tolerance().linear() {
231 let mut boxes = heap
232 .iter()
233 .filter(|cell| cell.lower <= best.upper)
234 .map(|cell| CurvePairParameterBox {
235 first: ParameterInterval {
236 start: cell.first.start,
237 end: cell.first.end,
238 },
239 second: ParameterInterval {
240 start: cell.second.start,
241 end: cell.second.end,
242 },
243 })
244 .collect::<Vec<_>>();
245 boxes.push(CurvePairParameterBox {
246 first: ParameterInterval {
247 start: best.first_parameter,
248 end: best.first_parameter,
249 },
250 second: ParameterInterval {
251 start: best.second_parameter,
252 end: best.second_parameter,
253 },
254 });
255 boxes.sort_by(|left, right| {
256 left.first
257 .start
258 .total_cmp(&right.first.start)
259 .then_with(|| left.second.start.total_cmp(&right.second.start))
260 });
261 return Ok(DistanceCore {
262 first_parameter: best.first_parameter,
263 second_parameter: best.second_parameter,
264 first_point: best.first_point,
265 second_point: best.second_point,
266 distance: best.distance,
267 lower: global_lower,
268 upper: best.upper,
269 boxes,
270 visited_nodes,
271 });
272 }
273
274 let cell = heap
275 .pop()
276 .ok_or_else(|| GeomError::Degenerate("curve-pair queue exhausted".to_owned()))?;
277 let first_span = (cell.first.end - cell.first.start).abs();
278 let second_span = (cell.second.end - cell.second.start).abs();
279 let split_first = cell.first.depth < options.max_depth()
280 && (cell.second.depth >= options.max_depth() || first_span >= second_span);
281 if !split_first && cell.second.depth >= options.max_depth() {
282 return Err(GeomError::BudgetExceeded {
283 resource: "certified curve-pair budget",
284 });
285 }
286 let requested = visited_nodes
287 .checked_add(2)
288 .ok_or(GeomError::BudgetExceeded {
289 resource: "certified curve-pair budget",
290 })?;
291 if requested > options.max_nodes() {
292 return Err(GeomError::BudgetExceeded {
293 resource: "certified curve-pair budget",
294 });
295 }
296
297 let children = if split_first {
298 let (left, right) = cell.first.split()?;
299 [(left, cell.second.clone()), (right, cell.second)]
300 } else {
301 let (left, right) = cell.second.split()?;
302 [(cell.first.clone(), left), (cell.first, right)]
303 };
304 visited_nodes = requested;
305 for (first, second) in children {
306 consider_midpoint_pair(
307 &mut best,
308 &first,
309 &second,
310 dimensions,
311 &evaluate_first,
312 &evaluate_second,
313 )?;
314 let lower = first.gap(&second, dimensions)?;
315 if lower <= best.upper {
316 heap.push(QueueCell {
317 lower,
318 serial,
319 first,
320 second,
321 });
322 serial = serial.checked_add(1).ok_or(GeomError::BudgetExceeded {
323 resource: "certified curve-pair budget",
324 })?;
325 }
326 }
327 }
328}
329
330fn consider_cell_samples(
331 best: &mut impl CandidateSlot,
332 first: &Cell,
333 second: &Cell,
334 dimensions: usize,
335 evaluate_first: &impl Fn(Scalar) -> GeomResult<[Scalar; 3]>,
336 evaluate_second: &impl Fn(Scalar) -> GeomResult<[Scalar; 3]>,
337) -> GeomResult<()> {
338 let first_samples = cell_samples(first)?;
339 let second_samples = cell_samples(second)?;
340 for (first_parameter, first_enclosure) in &first_samples {
341 for (second_parameter, second_enclosure) in &second_samples {
342 consider_candidate(
343 best,
344 *first_parameter,
345 *second_parameter,
346 first_enclosure,
347 second_enclosure,
348 dimensions,
349 evaluate_first,
350 evaluate_second,
351 )?;
352 }
353 }
354 Ok(())
355}
356
357fn consider_midpoint_pair(
358 best: &mut Candidate,
359 first: &Cell,
360 second: &Cell,
361 dimensions: usize,
362 evaluate_first: &impl Fn(Scalar) -> GeomResult<[Scalar; 3]>,
363 evaluate_second: &impl Fn(Scalar) -> GeomResult<[Scalar; 3]>,
364) -> GeomResult<()> {
365 let first_parameter = first.start * 0.5 + first.end * 0.5;
366 let second_parameter = second.start * 0.5 + second.end * 0.5;
367 let first_enclosure = first.midpoint_point()?;
368 let second_enclosure = second.midpoint_point()?;
369 consider_candidate(
370 best,
371 first_parameter,
372 second_parameter,
373 &first_enclosure,
374 &second_enclosure,
375 dimensions,
376 evaluate_first,
377 evaluate_second,
378 )
379}
380
381fn cell_samples(cell: &Cell) -> GeomResult<Vec<(Scalar, HomogeneousPoint)>> {
382 Ok(vec![
383 (cell.start, cell.controls[0].clone()),
384 (cell.start * 0.5 + cell.end * 0.5, cell.midpoint_point()?),
385 (cell.end, cell.controls[cell.controls.len() - 1].clone()),
386 ])
387}
388
389#[allow(clippy::too_many_arguments)]
390fn consider_candidate(
391 best: &mut impl CandidateSlot,
392 first_parameter: Scalar,
393 second_parameter: Scalar,
394 first_enclosure: &HomogeneousPoint,
395 second_enclosure: &HomogeneousPoint,
396 dimensions: usize,
397 evaluate_first: &impl Fn(Scalar) -> GeomResult<[Scalar; 3]>,
398 evaluate_second: &impl Fn(Scalar) -> GeomResult<[Scalar; 3]>,
399) -> GeomResult<()> {
400 let first_point = evaluate_first(first_parameter)?;
401 let second_point = evaluate_second(second_parameter)?;
402 let distance = representative_distance(first_point, second_point, dimensions)?;
403 let upper = distance_between_point_intervals_upper(
404 first_enclosure.euclidean()?,
405 second_enclosure.euclidean()?,
406 dimensions,
407 )?
408 .max(next_up(distance));
409 let candidate = Candidate {
410 first_parameter,
411 second_parameter,
412 first_point,
413 second_point,
414 distance,
415 upper,
416 };
417 best.consider(candidate);
418 Ok(())
419}
420
421trait CandidateSlot {
422 fn consider(&mut self, candidate: Candidate);
423}
424
425impl CandidateSlot for Option<Candidate> {
426 fn consider(&mut self, candidate: Candidate) {
427 match self {
428 Some(current)
429 if candidate.upper > current.upper
430 || (candidate.upper == current.upper
431 && (candidate.first_parameter, candidate.second_parameter)
432 >= (current.first_parameter, current.second_parameter)) => {}
433 slot => *slot = Some(candidate),
434 }
435 }
436}
437
438impl CandidateSlot for Candidate {
439 fn consider(&mut self, candidate: Candidate) {
440 if candidate.upper < self.upper
441 || (candidate.upper == self.upper
442 && (candidate.first_parameter, candidate.second_parameter)
443 < (self.first_parameter, self.second_parameter))
444 {
445 *self = candidate;
446 }
447 }
448}