Add intersection point algorithm

1 files changed, 111 insertions, 113 deletions

diff --git a/src/math/polygon.rs b/src/math/polygon.rs
index 2af669a..e761136 100644
--- a/src/math/polygon.rs
+++ b/src/math/polygon.rs
@@ -1,8 +1,9 @@
-use super::{LineSegment, TripletOrientation, Vec2};
-use alga::general::{ClosedMul, ClosedSub};
-use nalgebra::Scalar;
+use super::Vec2;
+use nalgebra::{ClosedDiv, ClosedMul, ClosedSub, Scalar};
 use num_traits::Zero;
+use std::ops::Neg;
 
+#[derive(Debug)]
 pub struct Polygon<T: Scalar + Copy> {
     pub corners: Vec<Vec2<T>>,
 }
@@ -14,125 +15,90 @@ impl<T: Scalar + Copy> Polygon<T> {
 
     /// Check whether a point is inside a polygon or not. If a point lies on an edge, it also
     /// counts as inside the polygon.
-    pub fn contains(&self, point: Vec2<T>) -> bool
+    pub fn contains_point(&self, p: Vec2<T>) -> bool
     where
-        T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero,
+        T: Zero + ClosedSub + ClosedMul + ClosedDiv + Neg<Output = T> + PartialOrd,
     {
-        // Must be at least a triangle to contain anything
-        if self.corners.len() < 3 {
-            return false;
-        }
-
-        // Get the maximum x for the horizontal lines for "Infinity".
-        let mut max_x = self.corners[0].x;
-        for corner in self.corners.iter() {
-            /* Handle the edge case where the point is on a polygon corner, so we don't have to
-             * worry about it later.
-             */
-            if point == *corner {
-                return true;
-            }
-
-            max_x = super::partial_max(corner.x, max_x);
-        }
-
-        // The horizontally "infinite" point of the test point.
-        let line_to_infinity = LineSegment::new(point, Vec2::new(max_x, point.y));
-
-        // The number of times the point line to infinity pierced through the polygon hull.
-        let mut num_pierced = 0;
-
-        /* Find edges that should be ignored. These are all edges, where the straight line passes
-         * right through a corner of the edge. It might still be counted as piercing the polygon,
-         * if the next corners lie below and the previous corner lies above the line or vice
-         * versa, but not if both points lie above it or below. The corners are exempted from the
-         * run through the edges. The edges that should be ignored are saved by their starting
-         * corner's index.
-         */
-        let mut ignore_edge = vec![false; self.corners.len()];
-        for i in 0..self.corners.len() {
-            if point.y == self.corners[i].y {
-                // The previous corner index
-                let prev_i = (i + self.corners.len() - 1) % self.corners.len();
-
-                // Make sure the edges containing this corner will be ignored.
-                ignore_edge[i] = true;
-                ignore_edge[prev_i] = true;
+        let n = self.corners.len();
 
-                /* Make sure we don't count a pierce double because of consecutive collinear
-                 * points. (Although the polygon really should be sanitised, but you know)
-                 */
-                if self.corners[prev_i].y == point.y {
-                    continue;
-                }
+        let a = self.corners[n - 1];
+        let mut b = self.corners[n - 2];
+        let mut ax;
+        let mut ay = a.y - p.y;
+        let mut bx = b.x - p.x;
+        let mut by = b.y - p.y;
 
-                /* Ignore coming collinear points, since we are only interested in when the
-                 * y-coordinate finally changes
-                 */
-                let mut j = (i + 1) % self.corners.len();
-                while i != j {
-                    if self.corners[j].y != point.y {
-                        if (self.corners[prev_i].y < point.y && self.corners[j].y > point.y)
-                            || (self.corners[prev_i].y > point.y && self.corners[j].y < point.y)
-                        {
-                            /* Only count as pierced when it would be right of the point, otherwise
-                             * it would be like checking to the left of the line that is drawn to
-                             * infinity, which is incorrect
-                             */
-                            if self.corners[i].x >= point.x {
-                                //println!("Pierced through corner {:?}", self.corners[i]);
-                                num_pierced += 1;
-                            }
-                        }
+        let mut lup = by > ay;
+        for i in 0..n {
+            // ax = bx;
+            ay = by;
+            b = self.corners[i];
+            bx = b.x - p.x;
+            by = b.y - p.y;
 
-                        break;
-                    }
-
-                    j = (j + 1) % self.corners.len();
-                }
+            if ay == by {
+                continue;
             }
+            lup = by > ay;
         }
 
-        // Find the points where the x-line may actually collide with a proper line.. ^^
-        for (i, edge_start) in self.corners.iter().enumerate() {
-            if ignore_edge[i] {
+        let mut depth = 0;
+        for i in 0..n {
+            ax = bx;
+            ay = by;
+            let b = &self.corners[i];
+            bx = b.x - p.x;
+            by = b.y - p.y;
+
+            if ay < T::zero() && by < T::zero() {
+                // both "up" or both "down"
+                continue;
+            }
+            if ay > T::zero() && by > T::zero() {
+                // both "up" or both "down"
+                continue;
+            }
+            if ax < T::zero() && bx < T::zero() {
+                // both points on the left
                 continue;
             }
 
-            let next = (i + 1) % self.corners.len();
-            let current_edge = LineSegment::new(*edge_start, self.corners[next]);
-
-            if LineSegment::intersect(&current_edge, &line_to_infinity) {
-                num_pierced += 1;
-                //println!("Pierced through edge: {:?}", &current_edge);
+            if ay == by && (if ax < bx { ax } else { bx }) <= T::zero() {
+                return true;
+            }
+            if ay == by {
+                continue;
+            }
 
-                /* Check for the edge case, where the point might lie right on an edge of the
-                 * polygon.
-                 */
-                if super::triplet_orientation(*edge_start, current_edge.end, point)
-                    == TripletOrientation::Collinear
-                {
-                    // The point should be right on an edge.
-                    // XXX: Should this not just be an assertion? It seems that this always has to
-                    // be the case when reaching this part.
-                    return current_edge.contains_collinear(point);
-                }
+            let lx = ax + (((bx - ax) * -ay) / (by - ay));
+            if lx == T::zero() {
+                // point on edge
+                return true;
+            }
+            if lx > T::zero() {
+                depth += 1;
+            }
+            if ay == T::zero() && lup && by > ay {
+                // hit vertex, both up
+                depth -= 1;
+            }
+            if ay == T::zero() && !lup && by < ay {
+                // hit vertex, both down
+                depth -= 1;
             }
-        }
 
-        //println!("Num pierced is: {}", num_pierced);
+            lup = by > ay;
+        }
 
-        num_pierced % 2 == 1
+        (depth & 1) == 1
     }
 
-    /// Join this polygon with another, ensuring the area of the two stays the same, minus the
-    /// overlap.
-    ///
-    /// # Panics
-    /// If the two polygons do not intersect, it is impossible to unite them into a single polygon,
-    /// in which case this function is bound to fail.
-    // TODO: Create a function that only tries to join the polygons.
-    pub fn unite(&mut self, mut _other: Polygon<T>) {
+    /// Join this polygon with another, ensuring the area of the two stays the same, but the
+    /// overlap is not doubled, but instead joined into one.
+    /// Returns the Polygons themselves, if there is no overlap
+    // TODO: At the moment, counts holes in the middle as a different polygon. These should be
+    // attached to the polygon they are inside of.
+    pub fn unite(self, _other: Polygon<T>) -> Vec<Polygon<T>> {
         unimplemented!()
     }
 }
@@ -153,13 +119,45 @@ mod test {
             Vec2::new(1., 1.),
         ]);
 
-        assert!(!polygon.contains(Vec2::new(1., -2.)));
-        assert!(!polygon.contains(Vec2::new(-1., 0.5)));
-        assert!(polygon.contains(Vec2::new(0., 0.5)));
-        assert!(polygon.contains(Vec2::new(0.5, 1.)));
-        assert!(polygon.contains(Vec2::new(0.5, 1.5)));
-        assert!(!polygon.contains(Vec2::new(-2., 1.9)));
-        assert!(!polygon.contains(Vec2::new(0., 3.)));
-        assert!(polygon.contains(Vec2::new(1., 3.)));
+        assert!(!polygon.contains_point(Vec2::new(1., -2.)));
+        assert!(!polygon.contains_point(Vec2::new(-1., 0.5)));
+        assert!(polygon.contains_point(Vec2::new(0., 0.5)));
+        assert!(polygon.contains_point(Vec2::new(0.5, 1.)));
+        assert!(polygon.contains_point(Vec2::new(0.5, 1.5)));
+        assert!(!polygon.contains_point(Vec2::new(-2., 1.9)));
+        assert!(!polygon.contains_point(Vec2::new(0., 3.)));
+        assert!(polygon.contains_point(Vec2::new(1., 3.)));
+    }
+
+    #[test]
+    fn polygon_union() {
+        let first = Polygon::new(vec![
+            Vec2::new(-2., 1.),
+            Vec2::new(-0.5, 2.5),
+            Vec2::new(2., 2.),
+            Vec2::new(0.5, 1.5),
+            Vec2::new(1., 0.),
+            Vec2::new(-0.5, 1.),
+        ]);
+
+        let second = Polygon::new(vec![
+            Vec2::new(0., 0.),
+            Vec2::new(-2., 2.),
+            Vec2::new(3., 2.),
+            Vec2::new(1.5, 0.),
+        ]);
+
+        let union = first.unite(second);
+        assert_eq!(union.len(), 1);
+        let union = &union[0];
+
+        println!("Union of the two polygons: {:?}", union);
+
+        assert_eq!(union.corners.len(), 10);
+        assert!(union
+            .corners
+            .iter()
+            .find(|&p| p.x == 0. && p.y == 0.)
+            .is_some());
     }
 }