Add polygon graph

4 files changed, 369 insertions, 1 deletions

diff --git a/src/math/line_segment.rs b/src/math/line_segment.rs
index 7dacf54..1c2c01c 100644
--- a/src/math/line_segment.rs
+++ b/src/math/line_segment.rs
@@ -126,6 +126,14 @@ impl<T: Scalar + Copy> LineSegment<T> {
             }
         }
     }
+
+    /// 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.
+    pub fn eq_ignore_dir(&self, other: &LineSegment<T>) -> bool {
+        (self.start == other.start && self.end == other.end)
+            || (self.end == other.start && self.start == other.end)
+    }
 }
 
 #[derive(PartialEq, Eq)]
diff --git a/src/math/mod.rs b/src/math/mod.rs
index bfc2ab4..afbc4a3 100644
--- a/src/math/mod.rs
+++ b/src/math/mod.rs
@@ -1,10 +1,12 @@
 pub mod line_segment;
 pub mod polygon;
+pub mod polygon_graph;
 pub mod rect;
 pub mod vec2;
 
 pub use self::line_segment::*;
 pub use self::polygon::*;
+pub use self::polygon_graph::*;
 pub use self::rect::*;
 pub use self::vec2::*;
 
diff --git a/src/math/polygon_graph.rs b/src/math/polygon_graph.rs
new file mode 100644
index 0000000..3acd46f
--- /dev/null
+++ b/src/math/polygon_graph.rs
@@ -0,0 +1,358 @@
+use super::{LineSegment, Polygon, Vec2};
+use nalgebra::Scalar;
+use std::cmp::{Ordering, PartialOrd};
+
+struct Node<T: Scalar + Copy> {
+    pub vec: Vec2<T>,
+    pub adjacent: Vec<Vec2<T>>,
+}
+
+/// 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. ß
+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]
+fn find_vec2<T: Scalar + Copy + PartialOrd>(
+    field: &[Vec2<T>],
+    lookup: &Vec2<T>,
+) -> Result<usize, usize> {
+    field.binary_search_by(|n| n.partial_cmp(lookup).unwrap_or(Ordering::Greater))
+}
+#[inline]
+fn find_node<T: Scalar + Copy + PartialOrd>(
+    field: &[Node<T>],
+    lookup: &Vec2<T>,
+) -> Result<usize, usize> {
+    field.binary_search_by(|n| n.vec.partial_cmp(lookup).unwrap_or(Ordering::Greater))
+}
+
+impl<T: Scalar + Copy + PartialOrd> PolygonGraph<T> {
+    /// Create a new, empty polygon graph.
+    pub fn new() -> Self {
+        Self { nodes: Vec::new() }
+    }
+
+    /// Count the number of edges in the graph. Internally, for each two connected points there are
+    /// two edges, but this returns the amount of polygon edges.
+    pub fn num_edges(&self) -> usize {
+        let mut num_edges = 0;
+        for node in &self.nodes {
+            for _ in &node.adjacent {
+                num_edges += 1;
+            }
+        }
+
+        assert!(num_edges % 2 == 0);
+        num_edges / 2
+    }
+
+    /// Count the number of nodes in this graph. If this graph consists of multiple polygons, this
+    /// can be different than the amount of corners, since corners with the same position are only
+    /// counted once.
+    pub fn num_nodes(&self) -> usize {
+        self.nodes.len()
+    }
+
+    /// Checks if there is an edge between the two given vectors. Is commutative in respect to the
+    /// two arguments.
+    pub fn has_edge(&self, from: &Vec2<T>, to: &Vec2<T>) -> bool {
+        // Binary search the starting and then the end node.
+        if let Ok(from) = find_node(&self.nodes, from) {
+            if let Ok(_) = find_vec2(&self.nodes[from].adjacent, to) {
+                true
+            } else {
+                false
+            }
+        } else {
+            false
+        }
+    }
+
+    // Helper function to add the edge into the internal graph representation for one side only.
+    // Since to the outside the graph should always be undirected, this must be private.
+    fn add_edge_onesided(&mut self, from: &Vec2<T>, to: &Vec2<T>) -> bool {
+        match find_node(&self.nodes, from) {
+            Ok(pos) => match find_vec2(&self.nodes[pos].adjacent, to) {
+                Ok(_) => return false,
+                Err(i) => self.nodes[pos].adjacent.insert(i, *to),
+            },
+            Err(pos) => self.nodes.insert(
+                pos,
+                Node {
+                    vec: *from,
+                    adjacent: vec![*to],
+                },
+            ),
+        }
+
+        true
+    }
+
+    /// Add an edge between the given vectors. If the edge already exists, it does nothing and
+    /// returns false, otherwise it returns true after addition.
+    pub fn add_edge(&mut self, from: &Vec2<T>, to: &Vec2<T>) -> bool {
+        if !self.add_edge_onesided(from, to) {
+            return false;
+        }
+
+        let back_edge_succ = self.add_edge_onesided(to, from);
+        assert!(back_edge_succ);
+
+        true
+    }
+
+    // Helper function to remove the edge in the internal graph representation for one side only.
+    // Since to the outside the graph should always be undirected, this must be private.
+    fn remove_edge_onesided(&mut self, from: &Vec2<T>, to: &Vec2<T>) -> bool {
+        if let Ok(from) = find_node(&self.nodes, from) {
+            if let Ok(to) = find_vec2(&self.nodes[from].adjacent, to) {
+                // Remove the edge from the vector.
+                self.nodes[from].adjacent.remove(to);
+
+                // If the node has no adjacent nodes anymore, remove it entirely.
+                if self.nodes[from].adjacent.is_empty() {
+                    self.nodes.remove(from);
+                }
+
+                true
+            } else {
+                false
+            }
+        } else {
+            false
+        }
+    }
+
+    /// Remove an edge between the given vectors. If there is no edge between them, it does nothing
+    /// and returns false, otherwise it returns true after deletion.
+    pub fn remove_edge(&mut self, from: &Vec2<T>, to: &Vec2<T>) -> bool {
+        if !self.remove_edge_onesided(from, to) {
+            return false;
+        }
+
+        let back_edge_succ = self.remove_edge_onesided(to, from);
+        assert!(back_edge_succ);
+
+        true
+    }
+
+    /// Constructs a new PolygonGraph from the provided polygon. Adds a node for every corner and
+    /// an edge to all connected corners (which should be exactly two, for regular polygons)
+    pub fn from_polygon(polygon: &Polygon<T>) -> Self {
+        let mut graph = PolygonGraph {
+            nodes: Vec::with_capacity(polygon.corners.len()),
+        };
+
+        graph.add_all(polygon);
+        graph
+    }
+
+    /// Add all edges of the provided polygon into the graph. Requires roughly double as much space
+    /// as the normal polygon.
+    pub fn add_all(&mut self, polygon: &Polygon<T>) {
+        for i in 0..polygon.corners.len() {
+            self.add_edge(
+                &polygon.corners[i],
+                &polygon.corners[(i + 1) % polygon.corners.len()],
+            );
+        }
+    }
+
+    /// 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) {
+        // 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);
+
+                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;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    /// Finds the minimal polygon that could hold this graph together, i.e. could contain the
+    /// entire graph, but with the minimal amount of space. It may however still contain extra
+    /// corner points, meaning an extra edge for three collinear points on this edge, that can be
+    /// cleaned up.
+    pub fn bounding_polygon(&self) -> Polygon<T> {
+        unimplemented!()
+    }
+}
+
+impl<T: Scalar + Copy + PartialOrd> Default for PolygonGraph<T> {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use super::*;
+
+    #[test]
+    fn from_polygon() {
+        let a = Vec2::new(0., 0.);
+        let b = Vec2::new(0., 1.);
+        let c = Vec2::new(0.5, 1.);
+
+        let triangle = Polygon::new(vec![a, b, c]);
+
+        let graph = PolygonGraph::from_polygon(&triangle);
+        assert_eq!(graph.num_edges(), 3);
+
+        assert!(graph.has_edge(&a, &b));
+        assert!(graph.has_edge(&b, &a));
+
+        assert!(graph.has_edge(&a, &c));
+        assert!(graph.has_edge(&c, &a));
+
+        assert!(graph.has_edge(&b, &c));
+        assert!(graph.has_edge(&c, &b));
+    }
+
+    #[test]
+    fn add_all() {
+        let top_left = Vec2::new(0., 0.);
+        let top_right = Vec2::new(1., 0.);
+        let bot_left = Vec2::new(0., 1.);
+        let bot_right = Vec2::new(1., 1.);
+
+        let triangle = Polygon::new(vec![top_left, bot_right, top_right]);
+
+        let square = Polygon::new(vec![bot_left, bot_right, top_right, top_left]);
+
+        let mut graph = PolygonGraph::new();
+        graph.add_all(&triangle);
+        graph.add_all(&square);
+
+        assert_eq!(graph.num_edges(), 5);
+        assert_eq!(graph.num_nodes(), 4);
+
+        assert!(graph.has_edge(&top_left, &top_right));
+        assert!(graph.has_edge(&top_right, &top_left));
+
+        assert!(graph.has_edge(&top_left, &bot_left));
+        assert!(graph.has_edge(&bot_left, &top_left));
+
+        assert!(graph.has_edge(&bot_left, &bot_right));
+        assert!(graph.has_edge(&bot_right, &bot_left));
+
+        assert!(graph.has_edge(&bot_right, &top_right));
+        assert!(graph.has_edge(&top_right, &bot_right));
+
+        assert!(graph.has_edge(&top_left, &bot_right));
+        assert!(graph.has_edge(&bot_right, &top_left));
+    }
+
+    #[test]
+    fn intersect_self() {
+        let first = Polygon::new(vec![
+            Vec2::new(0., 0.),
+            Vec2::new(0., 2.),
+            Vec2::new(2., 2.),
+            Vec2::new(3., 1.),
+            Vec2::new(2., 0.),
+        ]);
+
+        let second = Polygon::new(vec![
+            Vec2::new(2.5, -0.5),
+            Vec2::new(0., 2.),
+            Vec2::new(2., 2.),
+            Vec2::new(2., 0.5),
+            Vec2::new(2.5, 0.),
+        ]);
+
+        let mut graph = PolygonGraph::from_polygon(&first);
+        graph.add_all(&second);
+
+        graph.intersect_self();
+
+        assert_eq!(graph.num_nodes(), 9);
+        assert_eq!(graph.num_edges(), 12);
+
+        assert!(graph.has_edge(&Vec2::new(2., 0.), &Vec2::new(2.25, 0.25)));
+        assert!(graph.has_edge(&Vec2::new(3., 1.), &Vec2::new(2.25, 0.25)));
+        assert!(!graph.has_edge(&Vec2::new(2., 0.), &Vec2::new(3., 1.)));
+        assert!(graph.has_edge(&Vec2::new(0., 2.), &Vec2::new(2., 2.)));
+        assert!(graph.has_edge(&Vec2::new(0., 2.), &Vec2::new(2., 0.)));
+        assert!(!graph.has_edge(&Vec2::new(0., 2.), &Vec2::new(2.5, -0.5)));
+    }
+
+    #[test]
+    fn bounding_polygon() {
+        let first = Polygon::new(vec![
+            Vec2::new(0., 0.),
+            Vec2::new(0., 2.),
+            Vec2::new(2., 2.),
+            Vec2::new(3., 1.),
+            Vec2::new(2., 0.),
+        ]);
+
+        let second = Polygon::new(vec![
+            Vec2::new(2.5, -0.5),
+            Vec2::new(0., 2.),
+            Vec2::new(2., 2.),
+            Vec2::new(2., 0.5),
+            Vec2::new(2.5, 0.),
+        ]);
+
+        let mut graph = PolygonGraph::from_polygon(&first);
+        graph.add_all(&second);
+
+        let bounding = graph.bounding_polygon();
+
+        let num_corners = 8;
+        assert_eq!(bounding.corners.len(), num_corners);
+
+        // Run around the polygon to see if it was constructed correctly.
+        let start_i = bounding
+            .corners
+            .iter()
+            .position(|&x| x == Vec2::new(0., 0.))
+            .expect("Starting vector does not exist in polygon.");
+
+        assert_eq!(
+            bounding.corners[(start_i + 1) % num_corners],
+            Vec2::new(0., 2.)
+        );
+        assert_eq!(
+            bounding.corners[(start_i + 2) % num_corners],
+            Vec2::new(2., 2.)
+        );
+        assert_eq!(
+            bounding.corners[(start_i + 3) % num_corners],
+            Vec2::new(3., 1.)
+        );
+        assert_eq!(
+            bounding.corners[(start_i + 4) % num_corners],
+            Vec2::new(2.25, 0.25)
+        );
+        assert_eq!(
+            bounding.corners[(start_i + 5) % num_corners],
+            Vec2::new(2.5, 0.0)
+        );
+        assert_eq!(
+            bounding.corners[(start_i + 6) % num_corners],
+            Vec2::new(2.5, -0.5)
+        );
+        assert_eq!(
+            bounding.corners[(start_i + 7) % num_corners],
+            Vec2::new(2., 0.)
+        );
+    }
+}
diff --git a/src/math/vec2.rs b/src/math/vec2.rs
index 2b33f7b..cd38889 100644
--- a/src/math/vec2.rs
+++ b/src/math/vec2.rs
@@ -8,7 +8,7 @@ use std::convert::{From, Into};
 use std::ops::{Add, AddAssign, Div, Mul, MulAssign, Neg, Sub, SubAssign};
 use std::{fmt, mem};
 
-#[derive(Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Eq)]
+#[derive(Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Eq, Hash)]
 pub struct Vec2<T: Scalar + Copy> {
     pub x: T,
     pub y: T,