Add self intersection for polygon graphs

2 files changed, 142 insertions, 20 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.)));
+    }
+}
diff --git a/src/math/polygon_graph.rs b/src/math/polygon_graph.rs
index 3acd46f..43c3bc4 100644
--- a/src/math/polygon_graph.rs
+++ b/src/math/polygon_graph.rs
@@ -1,20 +1,26 @@
 use super::{LineSegment, Polygon, Vec2};
-use nalgebra::Scalar;
+use nalgebra::{RealField, Scalar};
 use std::cmp::{Ordering, PartialOrd};
 
+#[derive(Debug)]
 struct Node<T: Scalar + Copy> {
     pub vec: Vec2<T>,
     pub adjacent: Vec<Vec2<T>>,
 }
 
+struct EdgeIterator<'a, T: Scalar + Copy> {
+    nodes: &'a Vec<Node<T>>,
+    pos: (usize, usize),
+}
+
 /// An undirected graph, that is optimised for polygon edge operations. Since edges of a polygon
 /// are an undirected graph, this structure also holds both directions. This makes it rather space
 /// inefficient, but makes edge operations rather swift. ß
+#[derive(Debug)]
 pub struct PolygonGraph<T: Scalar + Copy + PartialOrd> {
     /// The nodes of the graph, together with their adjacency list.
     nodes: Vec<Node<T>>,
 }
-
 // Helper functions to find nodes/vecs in sorted fields, so It doesn't always have to be written
 // out.
 #[inline]
@@ -32,6 +38,35 @@ fn find_node<T: Scalar + Copy + PartialOrd>(
     field.binary_search_by(|n| n.vec.partial_cmp(lookup).unwrap_or(Ordering::Greater))
 }
 
+impl<'a, T: Scalar + Copy> EdgeIterator<'a, T> {
+    pub fn new(nodes: &'a Vec<Node<T>>) -> Self {
+        Self { nodes, pos: (0, 0) }
+    }
+}
+
+impl<'a, T: Scalar + Copy> Iterator for EdgeIterator<'a, T> {
+    type Item = LineSegment<T>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        // Try to find the element in the nodes vector, if it exists.
+        if let Some(node) = self.nodes.get(self.pos.0) {
+            let end = node.adjacent[self.pos.1];
+
+            // Advance the iterator to the next possible element
+            if self.pos.1 + 1 < node.adjacent.len() {
+                self.pos.1 += 1;
+            } else {
+                self.pos.1 = 0;
+                self.pos.0 += 1;
+            }
+
+            Some(LineSegment::new(node.vec, end))
+        } else {
+            None
+        }
+    }
+}
+
 impl<T: Scalar + Copy + PartialOrd> PolygonGraph<T> {
     /// Create a new, empty polygon graph.
     pub fn new() -> Self {
@@ -167,21 +202,50 @@ impl<T: Scalar + Copy + PartialOrd> PolygonGraph<T> {
     /// Calculates all points where the graph edges intersect with one another. It then adds them
     /// into the adjacency list such that the intersection point lies between the nodes of the
     /// lines.
-    pub fn intersect_self(&mut self) {
+    pub fn intersect_self(&mut self)
+    where
+        T: RealField,
+    {
         // Find all intersections with all other edges.
-        for node in &self.nodes {
-            for to in &node.adjacent {
-                let current_segment = LineSegment::new(node.vec, *to);
+        let mut to_delete: Vec<LineSegment<T>> = Vec::new();
+        let mut to_add: Vec<(Vec2<T>, Vec2<T>)> = Vec::new();
+        for segment in EdgeIterator::new(&self.nodes) {
+            /* Save all intersections of this line with any other line, and the line that it's
+             * intersecting with.
+             */
+            let mut intersections: Vec<Vec2<T>> = Vec::new();
+            for compare_segment in EdgeIterator::new(&self.nodes) {
+                if segment.eq_ignore_dir(&compare_segment) {
+                    continue;
+                }
 
-                for node in &self.nodes {
-                    for to in &node.adjacent {
-                        let compare_segment = LineSegment::new(node.vec, *to);
-                        if current_segment.eq_ignore_dir(&compare_segment) {
-                            continue;
-                        }
-                    }
+                if let Some(intersection) = LineSegment::intersection(&segment, &compare_segment) {
+                    println!(
+                        "Found intersection between {:?} and {:?}: {:?}",
+                        &segment, &compare_segment, &intersection
+                    );
+                    intersections.push(intersection);
                 }
             }
+
+            if intersections.is_empty() {
+                continue;
+            }
+
+            to_delete.push(segment.clone());
+
+            // Safe, since at least the line segment itself is represented.
+            let segments = segment.segments(&intersections);
+            for i in 1..segments.len() {
+                to_add.push((segments[i - 1], segments[i]));
+            }
+        }
+
+        for segment in to_delete {
+            self.remove_edge(&segment.start, &segment.end);
+        }
+        for (start, end) in to_add {
+            self.add_edge(&start, &end);
         }
     }
 
@@ -282,6 +346,9 @@ mod test {
 
         graph.intersect_self();
 
+        println!("Intersected graph:");
+        println!("{:#?}", &graph);
+
         assert_eq!(graph.num_nodes(), 9);
         assert_eq!(graph.num_edges(), 12);