Fix polygon corners not always running counterclockwise

2 files changed, 101 insertions, 14 deletions

diff --git a/src/math/polygon/mod.rs b/src/math/polygon/mod.rs
index cda1f2a..f84211d 100644
--- a/src/math/polygon/mod.rs
+++ b/src/math/polygon/mod.rs
@@ -16,16 +16,79 @@ use std::ops::Neg;
 #[derive(Debug, Deserialize, Serialize)]
 // TODO: Support polygons with holes
 pub struct Polygon<T: Scalar + Copy> {
-    pub corners: Vec<Vec2<T>>,
+    corners: Vec<Vec2<T>>,
 }
 
 impl<T: Scalar + Copy> Polygon<T> {
-    pub fn new(corners: Vec<Vec2<T>>) -> Self {
+    /// Create a new polygon from the provided corners. If the corners are right-turning, they will
+    /// be reversed to be left-turning.
+    ///
+    /// # Panics
+    /// If one tries to create a polygon with less than three corners, it will fail. Zero area
+    /// polygons however are allowed at the moment.
+    pub fn new(corners: Vec<Vec2<T>>) -> Self
+    where
+        T: RealField,
+    {
+        if corners.len() < 3 {
+            panic!("Cannot create polygon with less than three corners.");
+        }
+
+        let corners = if combined_angle(&corners) > T::zero() {
+            corners
+        } else {
+            corners.into_iter().rev().collect()
+        };
+
         Self { corners }
     }
 
-    /// Check whether a point is inside a polygon or not. If a point lies on an edge, it also
-    /// counts as inside the polygon.
+    /// Add a vertex as a corner between the two provided neighbours. The direction of the
+    /// neighbours is not relevant. The edge between the two will be replaced with two edges to the
+    /// new corner from each of the neighbours respectively.
+    /// If there already is a point `corner` in the polygon, this function will fail and return
+    /// `false`. It will do nothing and return `false` if the provided neighbours are not actually
+    /// neighbours in the polygon already.
+    /// Otherwise it will perform the addition and return `true`.
+    pub fn add_corner(
+        &mut self,
+        corner: Vec2<T>,
+        neighbour1: &Vec2<T>,
+        neighbour2: &Vec2<T>,
+    ) -> bool {
+        // Check that the corners do not contain the corner vector already.
+        if self.corners.iter().find(|&c| *c == corner).is_some() {
+            return false;
+        }
+
+        for i in 0..self.corners.len() {
+            let next = (i + 1) % self.corners.len();
+
+            if self.corners[i] == *neighbour1 && self.corners[next] == *neighbour2 {
+                self.corners.insert(next, corner);
+                return true;
+            }
+            if self.corners[i] == *neighbour2 && self.corners[next] == *neighbour1 {
+                self.corners.insert(next, corner);
+                return true;
+            }
+        }
+
+        // No matching neighbour pair can be found.
+        false
+    }
+
+    /// Get the corners of this polygon in left-turning direction.
+    pub fn corners(&self) -> &Vec<Vec2<T>> {
+        &self.corners
+    }
+
+    /// Get the corners mutable. Be careful, since if you constructed this polygon from a
+    /// right-turning line-strip, these are not the same as you constructed the polygon with, since
+    /// all polygons' corners are normalised to be left-turning.
+    pub fn corners_mut(&mut self) -> &mut Vec<Vec2<T>> {
+        &mut self.corners
+    }
 
     /// 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.
@@ -225,6 +288,30 @@ impl<
     }
 }
 
+/* Helper function to calculate the combined angle of a set of points when connecting them one
+ * after another until finally connecting the last point to the first point in radians. Negative,
+ * when the points in sum are right-turning, positive, when they are left-turning.
+ */
+fn combined_angle<T: Scalar + Copy + RealField>(points: &Vec<Vec2<T>>) -> T {
+    let mut combined_angle = T::zero();
+    for i in 0..points.len() {
+        let prev = (i + points.len() - 1) % points.len();
+        let next = (i + 1) % points.len();
+
+        let angle = math::triplet_angle(points[prev], points[i], points[next]);
+        if angle == T::zero() || angle == T::two_pi() {
+            continue;
+        }
+
+        // Add the change in direction.
+        combined_angle += angle - T::pi();
+    }
+
+    println!("Calculated combined angle: {} Pi", combined_angle / T::pi());
+
+    combined_angle
+}
+
 #[cfg(test)]
 mod test {
     use super::*;
diff --git a/src/math/polygon/polygon_graph.rs b/src/math/polygon/polygon_graph.rs
index 9477fbc..6fdb6cd 100644
--- a/src/math/polygon/polygon_graph.rs
+++ b/src/math/polygon/polygon_graph.rs
@@ -261,7 +261,7 @@ impl<T: Scalar + Copy + PartialOrd> PolygonGraph<T> {
         let mut current_node = &self.nodes[0];
         // Pretend we're coming from the top to start in the right direction.
         let mut last_vec = current_node.vec - Vec2::new(T::zero(), T::one());
-        let mut bounding_polygon = Polygon::new(vec![current_node.vec]);
+        let mut bounding_corners = vec![current_node.vec];
         loop {
             /* Find the next point by choosing the one with the greatest angle. This means we are
              * "bending" to the leftmost edge at each step. Since we are going around the polygon
@@ -281,17 +281,17 @@ impl<T: Scalar + Copy + PartialOrd> PolygonGraph<T> {
                 .expect("Adjacency list is empty. The polygon has an open edge (is broken)");
 
             // When we have come back to the start, the traversal is completed
-            if *next_corner == bounding_polygon.corners[0] {
+            if *next_corner == bounding_corners[0] {
                 break;
             }
 
-            bounding_polygon.corners.push(*next_corner);
+            bounding_corners.push(*next_corner);
             last_vec = current_node.vec;
             current_node = &self.nodes[find_node(&self.nodes, &next_corner)
                 .expect("Failure to find node that should be inside list.")];
         }
 
-        bounding_polygon
+        Polygon::new(bounding_corners)
     }
 }
 
@@ -433,15 +433,15 @@ mod test {
 
         assert_eq!(
             bounding.corners[(start_i + 1) % num_corners],
-            Vec2::new(2., 0.)
+            Vec2::new(0., 2.)
         );
         assert_eq!(
             bounding.corners[(start_i + 2) % num_corners],
-            Vec2::new(2.5, -0.5)
+            Vec2::new(2., 2.)
         );
         assert_eq!(
             bounding.corners[(start_i + 3) % num_corners],
-            Vec2::new(2.5, 0.0)
+            Vec2::new(3., 1.)
         );
         assert_eq!(
             bounding.corners[(start_i + 4) % num_corners],
@@ -449,15 +449,15 @@ mod test {
         );
         assert_eq!(
             bounding.corners[(start_i + 5) % num_corners],
-            Vec2::new(3., 1.)
+            Vec2::new(2.5, 0.0)
         );
         assert_eq!(
             bounding.corners[(start_i + 6) % num_corners],
-            Vec2::new(2., 2.)
+            Vec2::new(2.5, -0.5)
         );
         assert_eq!(
             bounding.corners[(start_i + 7) % num_corners],
-            Vec2::new(0., 2.)
+            Vec2::new(2., 0.)
         );
     }
 }