Fix corner case not being handled

Previously, the algorithm to check, if a line-segment is inside a
polygon did not have a special case for when the start or end of the
segment is on a polygon corner. In case this corner is reflexive,
checking against one line between this corner and an adjacent one may
not be enough.

2 files changed, 97 insertions, 2 deletions

diff --git a/src/math/polygon/mod.rs b/src/math/polygon/mod.rs
index baa1e6d..e19e097 100644
--- a/src/math/polygon/mod.rs
+++ b/src/math/polygon/mod.rs
@@ -130,11 +130,60 @@ impl<
 
         /* Both end-points are inside the polygon. */
 
+        /* In case the an endpoint of the line segment is equal to a corner of the polygon, it's
+         * not enough to merely check one edge, since if the corner is reflex, the segment may
+         * still be inside, eventhough its similar to the outwards pointing normal of one edge, but
+         * may be to the inside of the other edge.
+         */
+        let mut start_looks_inside = false;
+        let mut end_looks_inside = false;
+        /* Helper function that checks if a point p, when starting from the given corner c is in a
+         * direction so that considering both edges that are connected to c, the point is in the
+         * direction of the inside of the polygon.
+         */
+        let corner_vec_pointing_inside = |p: Vec2<T>, c: usize| {
+            let prev = (c + self.corners.len() - 1) % self.corners.len();
+            let next = (c + 1) % self.corners.len();
+
+            let last_edge_orientation =
+                math::triplet_orientation(self.corners[prev], self.corners[c], p);
+            let current_edge_orientation =
+                math::triplet_orientation(self.corners[c], self.corners[next], p);
+
+            if last_edge_orientation == TripletOrientation::Clockwise
+                && current_edge_orientation == TripletOrientation::Clockwise
+            {
+                false
+            } else {
+                true
+            }
+        };
+
+        for c in 0..self.corners.len() {
+            if line_segment.start == self.corners[c] {
+                start_looks_inside = corner_vec_pointing_inside(line_segment.end, c);
+                if !start_looks_inside {
+                    return false;
+                }
+            }
+            if line_segment.end == self.corners[c] {
+                end_looks_inside = corner_vec_pointing_inside(line_segment.start, c);
+                if !end_looks_inside {
+                    return false;
+                }
+            }
+        }
+
+        if start_looks_inside && end_looks_inside {
+            return true;
+        }
+
         /* Check the intersections of the line segment with all polygon edges and see if it is
          * piercing through any of them.
          */
         for c in 0..self.corners.len() {
             let next = (c + 1) % self.corners.len();
+
             let current_edge = LineSegment::new(self.corners[c], self.corners[next]);
 
             if LineSegment::intersect(&line_segment, &current_edge) {
@@ -152,8 +201,13 @@ impl<
                     /* If at least one of the points is on the edge, make sure, the line points
                      * inside of the polygon, not to the outside.
                      */
-                    (TripletOrientation::Collinear, o) | (o, TripletOrientation::Collinear) => {
-                        if o == TripletOrientation::Clockwise {
+                    (TripletOrientation::Collinear, o) => {
+                        if !start_looks_inside && o == TripletOrientation::Clockwise {
+                            return false;
+                        }
+                    }
+                    (o, TripletOrientation::Collinear) => {
+                        if !end_looks_inside && o == TripletOrientation::Clockwise {
                             return false;
                         }
                     }
@@ -237,6 +291,12 @@ mod test {
         // Start point and endpoint outside. Line completely outside.
         assert!(!polygon
             .contains_line_segment(&LineSegment::new(Vec2::new(7.0, 0.), Vec2::new(7.5, 1.))));
+
+        // Start point on vertex, pointing in same dir as one of the adjacent edge normals,
+        // completely inside.
+        assert!(
+            polygon.contains_line_segment(&LineSegment::new(Vec2::new(2., 0.5), Vec2::new(4., 0.)))
+        );
     }
 
     #[test]
diff --git a/src/math/triangle.rs b/src/math/triangle.rs
index 5cf16e5..35bdcec 100644
--- a/src/math/triangle.rs
+++ b/src/math/triangle.rs
@@ -4,6 +4,7 @@ use nalgebra::{RealField, Scalar};
 use num_traits::Zero;
 
 /// Represents a triangle
+#[derive(Debug)]
 pub struct Triangle<T: Scalar + Copy> {
     /// The three corners of the triangle. Internally, it is made sure that the corners are always
     /// ordered in a counterclockwise manner, to make operations like contains simpler.
@@ -80,6 +81,25 @@ impl<T: Scalar + Copy> Into<[Vec2<T>; 3]> for Triangle<T> {
     }
 }
 
+impl<T: Scalar + Copy + PartialEq> PartialEq for Triangle<T> {
+    fn eq(&self, other: &Self) -> bool {
+        // The indexes of the elements are not important, so try all shifting options.
+        for shift in 0..=2 {
+            if self
+                .corners
+                .iter()
+                .enumerate()
+                .find(|(i, &c)| c != other.corners[(i + shift) % 3])
+                .is_none()
+            {
+                return true;
+            }
+        }
+
+        false
+    }
+}
+
 #[derive(PartialEq, Eq)]
 pub(crate) enum TripletOrientation {
     Clockwise,
@@ -187,6 +207,21 @@ mod test {
     }
 
     #[test]
+    fn equality() {
+        let a = Vec2::new(0., 0.);
+        let b = Vec2::new(-1., -1.);
+        let c = Vec2::new(-2., 0.);
+        let d = Vec2::new(-3., 0.);
+
+        let cmp = Triangle::new(a, b, c);
+
+        assert_eq!(Triangle::new(a, b, c), cmp);
+        assert_eq!(Triangle::new(c, b, a), cmp);
+        assert_eq!(Triangle::new(b, a, c), cmp);
+        assert!(Triangle::new(a, b, d) != cmp);
+    }
+
+    #[test]
     fn triplet_angle() {
         assert_eq!(
             super::triplet_angle(Vec2::new(0., 0.), Vec2::new(0., -1.), Vec2::new(-1., -1.)),