1 files changed, 80 insertions, 13 deletions

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);