Add self intersection for polygon graphs

1 files changed, 62 insertions, 7 deletions

diff --git a/src/math/line_segment.rs b/src/math/line_segment.rs
index 1c2c01c..2b67783 100644
--- a/src/math/line_segment.rs
+++ b/src/math/line_segment.rs
@@ -2,8 +2,9 @@ use super::{Rect, Vec2};
 use alga::general::{ClosedDiv, ClosedMul, ClosedSub};
 use nalgebra::{RealField, Scalar};
 use num_traits::Zero;
+use std::cmp::Ordering;
 
-#[derive(Debug)]
+#[derive(Debug, Clone)]
 pub struct LineSegment<T: Scalar + Copy> {
     pub start: Vec2<T>,
     pub end: Vec2<T>,
@@ -23,8 +24,8 @@ impl<T: Scalar + Copy> LineSegment<T> {
         T: PartialOrd,
     {
         point.x <= super::partial_max(self.start.x, self.end.x)
-            && point.x >= super::partial_max(self.start.x, self.end.x)
-            && point.y <= super::partial_min(self.start.y, self.end.y)
+            && point.x >= super::partial_min(self.start.x, self.end.x)
+            && point.y <= super::partial_max(self.start.y, self.end.y)
             && point.y >= super::partial_min(self.start.y, self.end.y)
     }
 
@@ -96,10 +97,10 @@ impl<T: Scalar + Copy> LineSegment<T> {
     {
         // Calculate the differences of each line value from starting point to end point
         // coordinate.
-        let dax = line_a.end.x - line_a.start.x;
-        let dbx = line_b.end.x - line_b.start.x;
-        let day = line_a.end.y - line_a.start.y;
-        let dby = line_b.end.y - line_b.start.y;
+        let dax = line_a.start.x - line_a.end.x;
+        let dbx = line_b.start.x - line_b.end.x;
+        let day = line_a.start.y - line_a.end.y;
+        let dby = line_b.start.y - line_b.end.y;
 
         // Calculate determinant to see, if the lines are parallel or not
         // TODO: Collinearity check?
@@ -127,6 +128,42 @@ impl<T: Scalar + Copy> LineSegment<T> {
         }
     }
 
+    /// Find all segments, into which this LineSegment would be splitted, when the points provided
+    /// would cut the segment. The points must be on the line, otherwise this does not make sense.
+    /// Also, no segments of length zero (start point = end point) will be created.
+    pub fn segments(&self, split_points: &[Vec2<T>]) -> Vec<Vec2<T>>
+    where
+        T: ClosedSub + ClosedMul + PartialOrd + Zero,
+    {
+        // Make sure all segments are collinear with the segment and actually on it
+        assert_eq!(
+            split_points
+                .iter()
+                .find(|&p| triplet_orientation(self.start, self.end, *p)
+                    != TripletOrientation::Collinear),
+            None
+        );
+        assert_eq!(
+            split_points.iter().find(|&p| !self.contains_collinear(*p)),
+            None
+        );
+
+        let mut segments = Vec::with_capacity(split_points.len() + 2);
+        segments.push(self.start);
+        segments.push(self.end);
+        for point in split_points {
+            segments.push(*point);
+        }
+
+        segments.sort_unstable_by(|a, b| a.partial_cmp(b).unwrap_or(Ordering::Greater));
+        segments.dedup();
+        if self.start > self.end {
+            segments.reverse();
+        }
+
+        segments
+    }
+
     /// Checks if this line segment and the other line segment are the same, ignoring the direction
     /// in which the lines are going, in other words, which of the vectors the line starts at and which
     /// vector the line ends at is irrelevant.
@@ -160,3 +197,21 @@ where
         _ => TripletOrientation::Collinear,
     }
 }
+
+#[cfg(test)]
+mod test {
+    use super::*;
+
+    #[test]
+    fn contains_collinear() {
+        let segment = LineSegment::new(Vec2::new(0., 0.), Vec2::new(2., 2.));
+
+        assert!(segment.contains_collinear(Vec2::new(0., 0.)));
+        assert!(segment.contains_collinear(Vec2::new(1., 1.)));
+        assert!(segment.contains_collinear(Vec2::new(2., 2.)));
+
+        assert!(!segment.contains_collinear(Vec2::new(-1., -1.)));
+        assert!(!segment.contains_collinear(Vec2::new(3., 3.)));
+        assert!(!segment.contains_collinear(Vec2::new(-1., 0.)));
+    }
+}