Skip to main content

cadkit_geom/
nearest.rs

1use crate::*;
2use std::cmp::Ordering;
3use std::collections::BinaryHeap;
4use std::f64::consts::{PI, TAU};
5
6#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
7#[cfg_attr(feature = "schemars", derive(schemars::JsonSchema))]
8#[serde(rename_all = "camelCase", deny_unknown_fields)]
9/// Distance certification budget in caller units and an explicit dimensionless iteration limit.
10pub struct NearestPointOptions {
11    /// Maximum error of the returned distance relative to the global minimum, in coordinate
12    /// units; not a point-position bound.
13    pub max_distance_error: f64,
14    /// Dimensionless iteration cap, from 1 through 1000000.
15    pub max_iterations: u32,
16}
17impl NearestPointOptions {
18    /// Use tolerance.linear as the coordinate-unit distance budget and the default iteration cap;
19    /// validation occurs when an operation runs.
20    pub fn for_tolerance(t: &GeometryTolerance) -> Self {
21        Self {
22            max_distance_error: t.linear,
23            max_iterations: 262_144,
24        }
25    }
26    /// Validate finite values and the supported profile. Where supplied, tolerances use caller
27    /// units, radians and squared units; no repair or unit conversion is performed.
28    pub fn validate(self) -> GeometryResult<()> {
29        positive(self.max_distance_error, "Nearest-distance error budget")
30            .map_err(|e| e.at("maxDistanceError", "nearest_point"))?;
31        if self.max_iterations == 0 || self.max_iterations > 1_000_000 {
32            return Err(GeometryError::invalid(
33                GeometryReason::InvalidParameterRange,
34                "maxIterations",
35                "nearest_point",
36                "Iteration limit must be between 1 and 1000000",
37            ));
38        }
39        Ok(())
40    }
41}
42#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
43#[cfg_attr(feature = "schemars", derive(schemars::JsonSchema))]
44#[serde(rename_all = "camelCase", deny_unknown_fields)]
45/// Nearest-point result in caller coordinates. The error bound certifies distance, not uniqueness
46/// or position accuracy.
47pub struct NearestPointResult {
48    /// Result point in the input Cartesian coordinate system.
49    pub point: Point2,
50    /// Dimensionless source parameter: normalized 0-1; circles use counterclockwise turns from +x,
51    /// polylines normalize segment index plus local fraction.
52    pub parameter: f64,
53    /// Euclidean distance to the query in caller coordinate units.
54    pub distance: f64,
55    /// Certified distance error in coordinate units; does not bound the returned point's position.
56    pub distance_error_bound: f64,
57    /// Number of search subdivisions performed; dimensionless.
58    pub iterations: u32,
59    /// True only when global distance bounds converge within the requested coordinate-unit
60    /// budget.
61    pub converged: bool,
62}
63
64impl Curve {
65    /// Search with an explicit coordinate-unit distance-error budget and iteration cap. The
66    /// supplied tolerance validates geometry; success certifies distance, not point uniqueness.
67    pub fn nearest_point_with_options(
68        &self,
69        query: Point2,
70        options: NearestPointOptions,
71        t: &GeometryTolerance,
72    ) -> GeometryResult<NearestPointResult> {
73        if let Curve::Nurbs(curve) = self {
74            return curve.nearest_point_with_options(query, options, t);
75        }
76        self.validate(t)?;
77        query.validate()?;
78        options.validate()?;
79        let bounds = self.bounds()?;
80        let scale = [
81            bounds.min.x,
82            bounds.min.y,
83            bounds.max.x,
84            bounds.max.y,
85            query.x,
86            query.y,
87        ]
88        .into_iter()
89        .map(f64::abs)
90        .fold(0., f64::max);
91        let roundoff = 64. * f64::EPSILON * scale;
92        if options.max_distance_error <= 2. * roundoff {
93            return Err(GeometryError::invalid(
94                GeometryReason::NumericalErrorBudgetExceeded,
95                "maxDistanceError",
96                "nearest_point",
97                "Nearest-distance budget is below coordinate resolution",
98            ));
99        }
100        if !matches!(self, Curve::Ellipse(_) | Curve::Bezier(_)) {
101            let point = self.nearest_point(query, t)?;
102            let distance = point.distance(query);
103            finite(distance)?;
104            return Ok(NearestPointResult {
105                point,
106                parameter: analytic_parameter(self, point, t)?,
107                distance,
108                distance_error_bound: 2. * roundoff,
109                iterations: 0,
110                converged: true,
111            });
112        }
113        if let Curve::Ellipse(ellipse) = self {
114            let a = ellipse.axis_u.length();
115            let b = ellipse.axis_v.length();
116            if (a - b).abs() <= t.linear
117                && ellipse
118                    .axis_u
119                    .scaled(1. / a)
120                    .dot(ellipse.axis_v.scaled(1. / b))
121                    .abs()
122                    <= t.angular
123                && query.distance(ellipse.center) <= t.linear
124            {
125                return Err(GeometryError::invalid(
126                    GeometryReason::AmbiguousNearestPoint,
127                    "query",
128                    "ellipse.nearest_point",
129                    "The nearest point is not unique at the center of a circular ellipse",
130                ));
131            }
132        }
133        let root = match self {
134            Curve::Bezier(curve) => NodeShape::Bezier(curve.control_points.clone()),
135            Curve::Ellipse(ellipse) => NodeShape::Ellipse(*ellipse),
136            _ => return Err(missing_geometry()),
137        };
138        let mut best = Candidate {
139            point: point_at(self, 0.)?,
140            parameter: 0.,
141            distance: f64::INFINITY,
142            start: 0.,
143            end: 1.,
144        };
145        let node = build_node(self, root, 0., 1., query, &mut best)?;
146        let mut queue = BinaryHeap::from([node]);
147        let mut iterations = 0;
148        loop {
149            let lower = queue.peek().map_or(best.distance, |node| node.lower);
150            let error = (best.distance - lower + 2. * roundoff).max(2. * roundoff);
151            if error <= options.max_distance_error {
152                refine(self, query, &mut best)?;
153                return Ok(NearestPointResult {
154                    point: best.point,
155                    parameter: best.parameter,
156                    distance: best.distance,
157                    distance_error_bound: (best.distance - lower + 2. * roundoff)
158                        .max(2. * roundoff),
159                    iterations,
160                    converged: true,
161                });
162            }
163            if iterations >= options.max_iterations {
164                return Err(GeometryError::unsupported(
165                    GeometryReason::ApproximationLimit,
166                    "maxIterations",
167                    "nearest_point",
168                    "Nearest-distance search did not converge within the iteration budget",
169                ));
170            }
171            let node = queue.pop().ok_or_else(|| {
172                GeometryError::invalid(
173                    GeometryReason::InsufficientPoints,
174                    "candidates",
175                    "nearest_point",
176                    "No nearest-point search interval remains",
177                )
178            })?;
179            iterations += 1;
180            if node.lower > best.distance {
181                continue;
182            }
183            let middle = (node.start + node.end) / 2.;
184            if middle == node.start || middle == node.end {
185                return Err(GeometryError::invalid(
186                    GeometryReason::NumericalErrorBudgetExceeded,
187                    "parameter",
188                    "nearest_point",
189                    "The parameter interval cannot be subdivided further",
190                ));
191            }
192            let (left, right) = node.shape.split()?;
193            for (shape, start, end) in [(left, node.start, middle), (right, middle, node.end)] {
194                let next = build_node(self, shape, start, end, query, &mut best)?;
195                if next.lower <= best.distance {
196                    queue.push(next);
197                }
198            }
199        }
200    }
201}
202struct Candidate {
203    point: Point2,
204    parameter: f64,
205    distance: f64,
206    start: f64,
207    end: f64,
208}
209#[derive(Clone)]
210enum NodeShape {
211    Bezier(Vec<Point2>),
212    Ellipse(Ellipse),
213}
214impl NodeShape {
215    fn split(self) -> GeometryResult<(Self, Self)> {
216        Ok(match self {
217            Self::Ellipse(value) => (Self::Ellipse(value), Self::Ellipse(value)),
218            Self::Bezier(points) => {
219                let mut work = points.clone();
220                let mut left = vec![*at(&points, 0)?];
221                let mut right = vec![*last(&points)?];
222                for n in (1..points.len()).rev() {
223                    for i in 0..n {
224                        let value = at(&work, i)?.lerp(*at(&work, i + 1)?, 0.5);
225                        *at_mut(&mut work, i)? = value;
226                    }
227                    left.push(*at(&work, 0)?);
228                    right.push(*at(&work, n - 1)?);
229                }
230                right.reverse();
231                (Self::Bezier(left), Self::Bezier(right))
232            }
233        })
234    }
235}
236struct SearchNode {
237    lower: f64,
238    start: f64,
239    end: f64,
240    shape: NodeShape,
241}
242impl PartialEq for SearchNode {
243    fn eq(&self, other: &Self) -> bool {
244        self.lower == other.lower && self.start == other.start && self.end == other.end
245    }
246}
247impl Eq for SearchNode {}
248impl PartialOrd for SearchNode {
249    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
250        Some(self.cmp(other))
251    }
252}
253impl Ord for SearchNode {
254    fn cmp(&self, other: &Self) -> Ordering {
255        other
256            .lower
257            .total_cmp(&self.lower)
258            .then_with(|| other.start.total_cmp(&self.start))
259            .then_with(|| other.end.total_cmp(&self.end))
260    }
261}
262fn build_node(
263    curve: &Curve,
264    shape: NodeShape,
265    start: f64,
266    end: f64,
267    query: Point2,
268    best: &mut Candidate,
269) -> GeometryResult<SearchNode> {
270    let first = point_at(curve, start)?;
271    let last = point_at(curve, end)?;
272    let (_, fraction) = segment_distance(first, last, query);
273    for parameter in [
274        start,
275        (start + end) / 2.,
276        end,
277        start + (end - start) * fraction,
278    ] {
279        consider(curve, query, parameter, start, end, best)?;
280    }
281    let (bounds, flatness) = match &shape {
282        NodeShape::Bezier(points) => {
283            let bounds = Bounds2::from_points(points)?;
284            let flat = points
285                .iter()
286                .map(|p| segment_distance(first, last, *p).0)
287                .fold(0., f64::max);
288            (bounds, flat)
289        }
290        NodeShape::Ellipse(ellipse) => {
291            let mut points = vec![first, last];
292            let lo = start * TAU;
293            let hi = end * TAU;
294            for angle in [
295                ellipse.axis_v.x.atan2(ellipse.axis_u.x),
296                ellipse.axis_v.y.atan2(ellipse.axis_u.y),
297            ] {
298                for angle in [angle.rem_euclid(TAU), (angle + PI).rem_euclid(TAU)] {
299                    if angle >= lo && angle <= hi {
300                        points.push(ellipse.point_at(angle));
301                    }
302                }
303            }
304            let flat = (ellipse.axis_u.length() + ellipse.axis_v.length()) * (hi - lo).powi(2) / 8.;
305            (Bounds2::from_points(&points)?, flat)
306        }
307    };
308    let box_distance = (query.x - bounds.max.x)
309        .max(bounds.min.x - query.x)
310        .max(0.)
311        .hypot((query.y - bounds.max.y).max(bounds.min.y - query.y).max(0.));
312    let lower = box_distance.max((segment_distance(first, last, query).0 - flatness).max(0.));
313    finite(lower)?;
314    Ok(SearchNode {
315        lower,
316        start,
317        end,
318        shape,
319    })
320}
321fn point_at(curve: &Curve, parameter: f64) -> GeometryResult<Point2> {
322    let point = match curve {
323        Curve::Bezier(curve) => curve.point_at(parameter)?,
324        Curve::Ellipse(ellipse) => ellipse.point_at(parameter * TAU),
325        _ => return Err(missing_geometry()),
326    };
327    point.validate()?;
328    Ok(point)
329}
330fn segment_distance(start: Point2, end: Point2, query: Point2) -> (f64, f64) {
331    let length = start.distance(end);
332    if length == 0. {
333        return (start.distance(query), 0.);
334    }
335    let direction = start.vector_to(end).scaled(1. / length);
336    let along = start.vector_to(query).dot(direction).clamp(0., length);
337    let point = start.translated(direction.scaled(along));
338    (query.distance(point), along / length)
339}
340fn consider(
341    curve: &Curve,
342    query: Point2,
343    parameter: f64,
344    start: f64,
345    end: f64,
346    best: &mut Candidate,
347) -> GeometryResult<()> {
348    let point = point_at(curve, parameter)?;
349    let distance = point.distance(query);
350    finite(distance)?;
351    if distance < best.distance {
352        *best = Candidate {
353            point,
354            parameter,
355            distance,
356            start,
357            end,
358        };
359    }
360    Ok(())
361}
362fn refine(curve: &Curve, query: Point2, best: &mut Candidate) -> GeometryResult<()> {
363    // Local refinement only lowers the upper bound; global acceptance uses the lower bound.
364    let (mut left, mut right) = (best.start, best.end);
365    let ratio = (5_f64.sqrt() - 1.) / 2.;
366    let mut x = right - ratio * (right - left);
367    let mut y = left + ratio * (right - left);
368    let mut fx = point_at(curve, x)?.distance(query);
369    let mut fy = point_at(curve, y)?.distance(query);
370    for _ in 0..64 {
371        if fx < fy {
372            right = y;
373            y = x;
374            fy = fx;
375            x = right - ratio * (right - left);
376            fx = point_at(curve, x)?.distance(query);
377        } else {
378            left = x;
379            x = y;
380            fx = fy;
381            y = left + ratio * (right - left);
382            fy = point_at(curve, y)?.distance(query);
383        }
384    }
385    consider(curve, query, x, left, right, best)?;
386    consider(curve, query, y, left, right, best)
387}
388fn analytic_parameter(curve: &Curve, point: Point2, t: &GeometryTolerance) -> GeometryResult<f64> {
389    Ok(match curve {
390        Curve::Line(line) => line.start.distance(point) / line.length(),
391        Curve::Circle(circle) => {
392            (point.y - circle.center.y)
393                .atan2(point.x - circle.center.x)
394                .rem_euclid(TAU)
395                / TAU
396        }
397        Curve::Arc(arc) => {
398            let (s, c) = arc.start_angle.sin_cos();
399            let start = s.atan2(c);
400            let angle = (point.y - arc.center.y).atan2(point.x - arc.center.x);
401            if point.distance(arc.point_at(0.)) <= t.linear {
402                0.
403            } else if point.distance(arc.point_at(1.)) <= t.linear {
404                1.
405            } else {
406                let delta = if arc.sweep_angle > 0. {
407                    (angle - start).rem_euclid(TAU)
408                } else {
409                    (start - angle).rem_euclid(TAU)
410                };
411                delta / arc.sweep_angle.abs()
412            }
413        }
414        Curve::Polyline(polyline) => {
415            let segments = polyline.segments();
416            let mut result = None;
417            for (index, segment) in segments.iter().enumerate() {
418                if segment.nearest_point(point, t)?.distance(point) <= t.linear {
419                    result = Some(
420                        (index as f64 + segment.start.distance(point) / segment.length())
421                            / segments.len() as f64,
422                    );
423                    break;
424                }
425            }
426            result.ok_or_else(|| {
427                GeometryError::invalid(
428                    GeometryReason::NumericalErrorBudgetExceeded,
429                    "parameter",
430                    "nearest_point",
431                    "Polyline parameter exceeds the error budget",
432                )
433            })?
434        }
435        _ => return Err(missing_geometry()),
436    })
437}
438
439#[cfg(test)]
440mod checked_search_tests {
441    use super::*;
442
443    #[test]
444    fn malformed_search_state_returns_errors_instead_of_panicking() {
445        assert!(NodeShape::Bezier(vec![]).split().is_err());
446        let curve = Curve::Line(LineSegment {
447            start: Point2::new(0., 0.),
448            end: Point2::new(1., 0.),
449        });
450        assert!(point_at(&curve, 0.5).is_err());
451        let ellipse = Curve::Ellipse(Ellipse {
452            center: Point2::new(0., 0.),
453            axis_u: Vector2 { x: 2., y: 0. },
454            axis_v: Vector2 { x: 0., y: 1. },
455        });
456        assert!(
457            analytic_parameter(&ellipse, Point2::new(2., 0.), &GeometryTolerance::default())
458                .is_err()
459        );
460    }
461}