Fix polygon corners not always running counterclockwise

1 files changed, 91 insertions, 4 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::*;