1use crate::{HasPosition, LineSideInfo, Point2, SpadeNum};
2use num_traits::{zero, Float};
3
4#[derive(Copy, Clone, PartialOrd, Ord, PartialEq, Eq, Debug, Hash)]
10pub struct PointProjection<S> {
11 factor: S,
12 length_2: S,
13}
14
15#[derive(Copy, Clone, PartialOrd, Ord, PartialEq, Eq, Debug, Hash)]
20pub enum InsertionError {
21 TooSmall,
26
27 TooLarge,
32
33 NAN,
35}
36
37impl core::fmt::Display for InsertionError {
38 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
39 <Self as core::fmt::Debug>::fmt(self, f)
40 }
41}
42
43#[cfg(feature = "std")]
44impl std::error::Error for InsertionError {}
45
46pub const MIN_ALLOWED_VALUE: f64 = 1.793662034335766e-43; pub const MAX_ALLOWED_VALUE: f64 = 3.2138760885179806e60; pub fn validate_coordinate<S: SpadeNum>(value: S) -> Result<(), InsertionError> {
98 let as_f64: f64 = value.into();
99 if as_f64.is_nan() {
100 Err(InsertionError::NAN)
101 } else if as_f64.abs() < MIN_ALLOWED_VALUE && as_f64 != 0.0 {
102 Err(InsertionError::TooSmall)
103 } else if as_f64.abs() > MAX_ALLOWED_VALUE {
104 Err(InsertionError::TooLarge)
105 } else {
106 Ok(())
107 }
108}
109
110pub fn validate_vertex<V: HasPosition>(vertex: &V) -> Result<(), InsertionError> {
117 let position = vertex.position();
118 validate_coordinate(position.x)?;
119 validate_coordinate(position.y)?;
120 Ok(())
121}
122
123pub fn mitigate_underflow(position: Point2<f64>) -> Point2<f64> {
157 Point2::new(
158 mitigate_underflow_for_coordinate(position.x),
159 mitigate_underflow_for_coordinate(position.y),
160 )
161}
162
163fn mitigate_underflow_for_coordinate<S: SpadeNum>(coordinate: S) -> S {
164 if coordinate != S::zero() && coordinate.abs().into() < MIN_ALLOWED_VALUE {
165 S::zero()
166 } else {
167 coordinate
168 }
169}
170
171impl<S: SpadeNum> PointProjection<S> {
172 fn new(factor: S, length_2: S) -> Self {
173 Self { factor, length_2 }
174 }
175
176 pub fn is_before_edge(&self) -> bool {
180 self.factor < S::zero()
181 }
182
183 pub fn is_behind_edge(&self) -> bool {
187 self.factor > self.length_2
188 }
189
190 pub fn is_on_edge(&self) -> bool {
194 !self.is_before_edge() && !self.is_behind_edge()
195 }
196
197 pub fn reversed(&self) -> Self {
204 Self {
205 factor: self.length_2 - self.factor,
206 length_2: self.length_2,
207 }
208 }
209}
210
211impl<S: SpadeNum + Float> PointProjection<S> {
212 pub fn relative_position(&self) -> S {
221 if self.length_2 >= zero() {
222 self.factor / self.length_2
223 } else {
224 let l = -self.length_2;
225 let f = -self.factor;
226 (l - f) / l
227 }
228 }
229}
230
231pub fn nearest_point<S>(p1: Point2<S>, p2: Point2<S>, query_point: Point2<S>) -> Point2<S>
232where
233 S: SpadeNum + Float,
234{
235 let dir = p2.sub(p1);
236 let s = project_point(p1, p2, query_point);
237 if s.is_on_edge() {
238 let relative_position = s.relative_position();
239 p1.add(dir.mul(relative_position))
240 } else if s.is_before_edge() {
241 p1
242 } else {
243 p2
244 }
245}
246
247pub fn project_point<S>(p1: Point2<S>, p2: Point2<S>, query_point: Point2<S>) -> PointProjection<S>
248where
249 S: SpadeNum,
250{
251 let dir = p2.sub(p1);
252 PointProjection::new(query_point.sub(p1).dot(dir), dir.length2())
253}
254
255pub fn distance_2<S>(p1: Point2<S>, p2: Point2<S>, query_point: Point2<S>) -> S
256where
257 S: SpadeNum + Float,
258{
259 let nn = nearest_point(p1, p2, query_point);
260 query_point.sub(nn).length2()
261}
262
263fn to_robust_coord<S: SpadeNum>(point: Point2<S>) -> robust::Coord<S> {
264 robust::Coord {
265 x: point.x,
266 y: point.y,
267 }
268}
269
270pub fn contained_in_circumference<S>(
271 v1: Point2<S>,
272 v2: Point2<S>,
273 v3: Point2<S>,
274 p: Point2<S>,
275) -> bool
276where
277 S: SpadeNum,
278{
279 let v1 = to_robust_coord(v1);
280 let v2 = to_robust_coord(v2);
281 let v3 = to_robust_coord(v3);
282 let p = to_robust_coord(p);
283
284 robust::incircle(v3, v2, v1, p) < 0.0
288}
289
290pub fn is_ordered_ccw<S>(p1: Point2<S>, p2: Point2<S>, query_point: Point2<S>) -> bool
291where
292 S: SpadeNum,
293{
294 let query = side_query(p1, p2, query_point);
295 query.is_on_left_side_or_on_line()
296}
297
298pub fn side_query<S>(p1: Point2<S>, p2: Point2<S>, query_point: Point2<S>) -> LineSideInfo
299where
300 S: SpadeNum,
301{
302 let p1 = to_robust_coord(p1);
303 let p2 = to_robust_coord(p2);
304 let query_point = to_robust_coord(query_point);
305
306 let result = robust::orient2d(p1, p2, query_point);
307 LineSideInfo::from_determinant(result)
308}
309
310fn side_query_inaccurate<S>(from: Point2<S>, to: Point2<S>, query_point: Point2<S>) -> LineSideInfo
311where
312 S: SpadeNum,
313{
314 let q = query_point;
315 let determinant = (to.x - from.x) * (q.y - from.y) - (to.y - from.y) * (q.x - from.x);
316 LineSideInfo::from_determinant(determinant.into())
317}
318
319pub(crate) fn intersects_edge_non_collinear<S>(
320 from0: Point2<S>,
321 to0: Point2<S>,
322 from1: Point2<S>,
323 to1: Point2<S>,
324) -> bool
325where
326 S: SpadeNum,
327{
328 let other_from = side_query(from0, to0, from1);
329 let other_to = side_query(from0, to0, to1);
330 let self_from = side_query(from1, to1, from0);
331 let self_to = side_query(from1, to1, to0);
332
333 assert!(
334 ![&other_from, &other_to, &self_from, &self_to]
335 .iter()
336 .all(|q| q.is_on_line()),
337 "intersects_edge_non_collinear: Given edge is collinear."
338 );
339
340 other_from != other_to && self_from != self_to
341}
342
343pub fn distance_2_triangle<S>(vertices: [Point2<S>; 3], query_point: Point2<S>) -> S
344where
345 S: SpadeNum + Float,
346{
347 for i in 0..3 {
348 let from = vertices[i];
349 let to = vertices[(i + 1) % 3];
350 if side_query_inaccurate(from, to, query_point).is_on_right_side() {
351 return distance_2(from, to, query_point);
352 }
353 }
354 S::zero()
356}
357
358pub fn circumcenter<S>(positions: [Point2<S>; 3]) -> (Point2<S>, S)
359where
360 S: SpadeNum + Float,
361{
362 let [v0, v1, v2] = positions;
363 let b = v1.sub(v0);
364 let c = v2.sub(v0);
365
366 let one = S::one();
367 let two = one + one;
368 let d = two * (b.x * c.y - c.x * b.y);
369 let len_b = b.dot(b);
370 let len_c = c.dot(c);
371 let d_inv: S = one / d;
372
373 let x = (len_b * c.y - len_c * b.y) * d_inv;
374 let y = (-len_b * c.x + len_c * b.x) * d_inv;
375 let result = Point2::new(x, y);
376 (result.add(v0), x * x + y * y)
377}
378
379pub fn triangle_area<S>(positions: [Point2<S>; 3]) -> S
380where
381 S: SpadeNum,
382{
383 let [v0, v1, v2] = positions;
384 let b = v1.sub(v0);
385 let c = v2.sub(v0);
386 (b.x * c.y - b.y * c.x).abs() * 0.5.into()
387}
388pub(crate) fn is_behind_edge(e1: Point2<f64>, e2: Point2<f64>, query_point: Point2<f64>) -> bool {
392 let projection = project_point(e1, e2, query_point);
393 projection.factor > projection.length_2 + f64::EPSILON
399}
400
401#[cfg(test)]
402mod test {
403 use super::{mitigate_underflow_for_coordinate, validate_coordinate};
404 use crate::{InsertionError, Point2};
405 use approx::assert_relative_eq;
406
407 #[test]
408 fn test_point_projection() {
409 use super::project_point;
410
411 let from = Point2::new(1.0f64, 1.0);
412 let to = Point2::new(4.0, 5.0);
413 let normal = Point2::new(4.0, -3.0);
414
415 let projection = project_point(from, to, from);
416 let reversed = projection.reversed();
417
418 assert!(!projection.is_before_edge());
419 assert!(!projection.is_behind_edge());
420 assert!(!reversed.is_before_edge());
421 assert!(!reversed.is_behind_edge());
422
423 assert!(projection.is_on_edge());
424 assert!(reversed.is_on_edge());
425
426 assert_eq!(projection.relative_position(), 0.0);
427 assert_eq!(reversed.relative_position(), 1.0);
428
429 assert_eq!(projection, reversed.reversed());
430
431 let mid_point = Point2::new(2.5 + normal.x, 3.0 + normal.y);
433
434 let projection = project_point(from, to, mid_point);
435 assert!(projection.is_on_edge());
436 assert_eq!(projection.relative_position(), 0.5);
437 assert_eq!(projection.reversed().relative_position(), 0.5);
438
439 let fifth = Point2::new(0.8 * from.x + 0.2 * to.x, 0.8 * from.y + 0.2 * to.y);
441 let fifth = Point2::new(fifth.x + normal.x, fifth.y + normal.y);
442 let projection = project_point(from, to, fifth);
443 assert!(projection.is_on_edge());
444 assert_relative_eq!(projection.relative_position(), 0.2);
445 assert_relative_eq!(projection.reversed().relative_position(), 0.8);
446
447 let behind_point = Point2::new(0.0, 0.0);
449 let projection = project_point(from, to, behind_point);
450 let reversed = projection.reversed();
451
452 assert!(projection.is_before_edge());
453 assert!(reversed.is_behind_edge());
454
455 assert!(!projection.is_on_edge());
456 assert!(!reversed.is_on_edge());
457 }
458
459 #[test]
460 fn test_validate_coordinate() {
461 use super::{validate_coordinate, InsertionError::*};
462 assert_eq!(validate_coordinate(f64::NAN), Err(NAN));
463 let max_value = super::MAX_ALLOWED_VALUE;
464
465 assert_eq!(validate_coordinate(f64::INFINITY), Err(TooLarge));
466 assert_eq!(validate_coordinate(f64::NEG_INFINITY), Err(TooLarge));
467 assert_eq!(validate_coordinate(max_value * 2.0), Err(TooLarge));
468
469 let min_value = super::MIN_ALLOWED_VALUE;
470 assert_eq!(validate_coordinate(min_value / 2.0), Err(TooSmall));
471
472 let tiny_float = f32::MIN_POSITIVE;
473 assert_eq!(validate_coordinate(tiny_float), Ok(()));
474
475 let big_float = f32::MAX;
476 assert_eq!(validate_coordinate(big_float), Ok(()));
477
478 assert_eq!(validate_coordinate(min_value), Ok(()));
479 assert_eq!(validate_coordinate(0.0), Ok(()));
480 }
481
482 #[test]
483 fn test_mitigate_underflow() {
484 use float_next_after::NextAfter;
485
486 for number_under_test in [
487 0.0.next_after(f64::NEG_INFINITY),
488 0.0.next_after(f64::INFINITY),
489 super::MIN_ALLOWED_VALUE.next_after(f64::NEG_INFINITY),
490 (-super::MIN_ALLOWED_VALUE).next_after(f64::INFINITY),
491 ] {
492 assert!(validate_coordinate(number_under_test).is_err());
493 let mitigated = mitigate_underflow_for_coordinate(number_under_test);
494 assert_ne!(mitigated, number_under_test);
495 assert_eq!(mitigated, 0.0);
496 }
497
498 assert_eq!(
499 validate_coordinate(mitigate_underflow_for_coordinate(f64::NAN)),
500 Err(InsertionError::NAN),
501 );
502
503 assert_eq!(
504 validate_coordinate(mitigate_underflow_for_coordinate(f64::INFINITY)),
505 Err(InsertionError::TooLarge),
506 );
507 }
508
509 #[test]
510 fn check_min_value() {
511 let mut expected = 1.0f64;
512 for _ in 0..142 {
513 expected *= 0.5;
514 }
515
516 assert_eq!(super::MIN_ALLOWED_VALUE, expected);
517 }
518
519 #[test]
520 fn check_max_value() {
521 let mut expected = 1.0f64;
522 for _ in 0..201 {
523 expected *= 2.0;
524 }
525
526 assert_eq!(super::MAX_ALLOWED_VALUE, expected);
527 }
528
529 #[test]
530 fn test_edge_distance() {
531 use super::distance_2;
532 let p1 = Point2::new(0.0, 0.0);
533 let p2 = Point2::new(1.0, 1.0);
534 assert_relative_eq!(distance_2(p1, p2, Point2::new(1.0, 0.0)), 0.5);
535 assert_relative_eq!(distance_2(p1, p2, Point2::new(0.0, 1.)), 0.5);
536 assert_relative_eq!(distance_2(p1, p2, Point2::new(-1.0, -1.0)), 2.0);
537 assert_relative_eq!(distance_2(p1, p2, Point2::new(2.0, 2.0)), 2.0);
538 }
539
540 #[test]
541 fn test_edge_side() {
542 use super::side_query;
543
544 let p1 = Point2::new(0.0, 0.0);
545 let p2 = Point2::new(1.0, 1.0);
546
547 assert!(side_query(p1, p2, Point2::new(1.0, 0.0)).is_on_right_side());
548 assert!(side_query(p1, p2, Point2::new(0.0, 1.0)).is_on_left_side());
549 assert!(side_query(p1, p2, Point2::new(0.5, 0.5)).is_on_line());
550 }
551
552 #[test]
553 fn test_intersects_middle() {
554 use super::intersects_edge_non_collinear;
555
556 let (f0, t0) = (Point2::new(0., 0.), Point2::new(5., 5.0));
557 let (f1, t1) = (Point2::new(-1.5, 1.), Point2::new(1.0, -1.5));
558 let (f2, t2) = (Point2::new(0.5, 4.), Point2::new(0.5, -4.));
559
560 assert!(!intersects_edge_non_collinear(f0, t0, f1, t1));
561 assert!(!intersects_edge_non_collinear(f1, t1, f0, t0));
562 assert!(intersects_edge_non_collinear(f0, t0, f2, t2));
563 assert!(intersects_edge_non_collinear(f2, t2, f0, t0));
564 assert!(intersects_edge_non_collinear(f1, t1, f2, t2));
565 assert!(intersects_edge_non_collinear(f2, t2, f1, t1));
566 }
567
568 #[test]
569 fn test_intersects_end_points() {
570 use super::intersects_edge_non_collinear;
571
572 let (f1, t1) = (Point2::new(0.33f64, 0.33f64), Point2::new(1.0, 0.0));
574 let (f2, t2) = (Point2::new(0.33, -1.0), Point2::new(0.33, 1.0));
575 assert!(intersects_edge_non_collinear(f1, t1, f2, t2));
576 assert!(intersects_edge_non_collinear(f2, t2, f1, t1));
577 let (f3, t3) = (Point2::new(0.0, -1.0), Point2::new(2.0, 1.0));
578 assert!(intersects_edge_non_collinear(f1, t1, f3, t3));
579 assert!(intersects_edge_non_collinear(f3, t3, f1, t1));
580
581 let (f4, t4) = (Point2::new(0.33, 0.33), Point2::new(0.0, 2.0));
583 assert!(intersects_edge_non_collinear(f1, t1, f4, t4));
584 assert!(intersects_edge_non_collinear(f4, t4, f1, t1));
585 }
586
587 #[test]
588 #[should_panic]
589 fn test_collinear_fail() {
590 use super::intersects_edge_non_collinear;
591
592 let (f1, t1) = (Point2::new(1.0, 2.0), Point2::new(3.0, 3.0));
593 let (f2, t2) = (Point2::new(-1.0, 1.0), Point2::new(-3.0, 0.0));
594 intersects_edge_non_collinear(f1, t1, f2, t2);
595 }
596
597 #[test]
598 fn test_triangle_distance() {
599 use super::distance_2_triangle;
600
601 let v1 = Point2::new(0., 0.);
602 let v2 = Point2::new(1., 0.);
603 let v3 = Point2::new(0., 1.);
604 let t = [v1, v2, v3];
605
606 assert_eq!(distance_2_triangle(t, Point2::new(0.25, 0.25)), 0.);
607 assert_relative_eq!(distance_2_triangle(t, Point2::new(-1., -1.)), 2.);
608 assert_relative_eq!(distance_2_triangle(t, Point2::new(0., -1.)), 1.);
609 assert_relative_eq!(distance_2_triangle(t, Point2::new(-1., 0.)), 1.);
610 assert_relative_eq!(distance_2_triangle(t, Point2::new(1., 1.)), 0.5);
611 assert_relative_eq!(distance_2_triangle(t, Point2::new(0.5, 0.5)), 0.0);
612 assert!(distance_2_triangle(t, Point2::new(0.6, 0.6)) > 0.001);
613 }
614
615 #[test]
616 fn test_contained_in_circumference() {
617 use super::contained_in_circumference;
618
619 let (a1, a2, a3) = (3f64, 2f64, 1f64);
620 let offset = Point2::new(0.5, 0.7);
621 let v1 = Point2::new(a1.sin(), a1.cos()).mul(2.).add(offset);
622 let v2 = Point2::new(a2.sin(), a2.cos()).mul(2.).add(offset);
623 let v3 = Point2::new(a3.sin(), a3.cos()).mul(2.).add(offset);
624 assert!(super::side_query(v1, v2, v3).is_on_left_side());
625 assert!(contained_in_circumference(v1, v2, v3, offset));
626 let shrunk = v1.sub(offset).mul(0.9).add(offset);
627 assert!(contained_in_circumference(v1, v2, v3, shrunk));
628 let expanded = v1.sub(offset).mul(1.1).add(offset);
629 assert!(!contained_in_circumference(v1, v2, v3, expanded));
630 assert!(!contained_in_circumference(
631 v1,
632 v2,
633 v3,
634 Point2::new(2.0 + offset.x, 2.0 + offset.y)
635 ));
636 assert!(contained_in_circumference(
637 Point2::new(0f64, 0f64),
638 Point2::new(0f64, -1f64),
639 Point2::new(1f64, 0f64),
640 Point2::new(0f64, -0.5f64)
641 ));
642 }
643}