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)]
9pub struct NearestPointOptions {
11 pub max_distance_error: f64,
14 pub max_iterations: u32,
16}
17impl NearestPointOptions {
18 pub fn for_tolerance(t: &GeometryTolerance) -> Self {
21 Self {
22 max_distance_error: t.linear,
23 max_iterations: 262_144,
24 }
25 }
26 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)]
45pub struct NearestPointResult {
48 pub point: Point2,
50 pub parameter: f64,
53 pub distance: f64,
55 pub distance_error_bound: f64,
57 pub iterations: u32,
59 pub converged: bool,
62}
63
64impl Curve {
65 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 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}