3 files changed, 138 insertions, 21 deletions

diff --git a/src/math/line_segment.rs b/src/math/line_segment.rs
index 2b67783..e6ca70f 100644
--- a/src/math/line_segment.rs
+++ b/src/math/line_segment.rs
@@ -192,15 +192,57 @@ where
      * they are the same it follows that the three points are collinear.
      */
     match (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y) {
-        q if q > T::zero() => TripletOrientation::Clockwise,
-        q if q < T::zero() => TripletOrientation::Counterclockwise,
+        q if q > T::zero() => TripletOrientation::Counterclockwise,
+        q if q < T::zero() => TripletOrientation::Clockwise,
         _ => TripletOrientation::Collinear,
     }
 }
 
+/// Calculate the angle between three points. Guaranteed to have 0 <= angle < 2Pi. The angle is
+/// calculated as the angle between a line from a to b and then from b to c, increasing
+/// counterclockwise.
+///
+/// # Panics
+/// If the length from a to b or the length from b to c is zero.
+pub(crate) fn triplet_angle<T>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> T
+where
+    T: Scalar + Copy + ClosedSub + RealField + Zero,
+{
+    assert!(a != b);
+    assert!(b != c);
+
+    // Handle the extreme 0 and 180 degree cases
+    let orientation = triplet_orientation(a, b, c);
+    if orientation == TripletOrientation::Collinear {
+        if LineSegment::new(a, b).contains_collinear(c)
+            || LineSegment::new(b, c).contains_collinear(a)
+        {
+            return T::zero();
+        } else {
+            return T::pi();
+        }
+    }
+
+    // Calculate the vectors out of the points
+    let ba = a - b;
+    let bc = c - b;
+
+    // Calculate the angle between 0 and Pi.
+    let angle = ((ba * bc) / (ba.length() * bc.length())).acos();
+
+    // Make angle into a full circle angle by looking at the orientation of the triplet.
+    let angle = match orientation {
+        TripletOrientation::Counterclockwise => T::pi() + (T::pi() - angle),
+        _ => angle,
+    };
+
+    angle
+}
+
 #[cfg(test)]
 mod test {
     use super::*;
+    use std::f64::consts::PI;
 
     #[test]
     fn contains_collinear() {
@@ -214,4 +256,32 @@ mod test {
         assert!(!segment.contains_collinear(Vec2::new(3., 3.)));
         assert!(!segment.contains_collinear(Vec2::new(-1., 0.)));
     }
+
+    #[test]
+    fn triplet_angle() {
+        assert_eq!(
+            super::triplet_angle(Vec2::new(0., 0.), Vec2::new(0., -1.), Vec2::new(-1., -1.)),
+            1.5 * PI
+        );
+        assert_eq!(
+            super::triplet_angle(Vec2::new(2., 2.), Vec2::new(0., 0.), Vec2::new(-1., -1.)),
+            PI
+        );
+        assert_eq!(
+            super::triplet_angle(Vec2::new(3., 1.), Vec2::new(0., 0.), Vec2::new(6., 2.)),
+            0.
+        );
+        assert_eq!(
+            super::triplet_angle(Vec2::new(3., 1.), Vec2::new(0., 0.), Vec2::new(-6., -2.)),
+            PI
+        );
+        assert_eq!(
+            super::triplet_angle(Vec2::new(0., 1.), Vec2::new(0., 2.), Vec2::new(-3., 2.)),
+            0.5 * PI
+        );
+        assert_eq!(
+            super::triplet_angle(Vec2::new(3., 1.), Vec2::new(2., 2.), Vec2::new(0., 2.)),
+            0.75 * PI
+        );
+    }
 }
diff --git a/src/math/polygon.rs b/src/math/polygon.rs
index e761136..5711049 100644
--- a/src/math/polygon.rs
+++ b/src/math/polygon.rs
@@ -1,9 +1,10 @@
-use super::Vec2;
-use nalgebra::{ClosedDiv, ClosedMul, ClosedSub, Scalar};
+use super::{PolygonGraph, Vec2};
+use nalgebra::{ClosedDiv, ClosedMul, ClosedSub, RealField, Scalar};
 use num_traits::Zero;
 use std::ops::Neg;
 
 #[derive(Debug)]
+// TODO: Support polygons with holes
 pub struct Polygon<T: Scalar + Copy> {
     pub corners: Vec<Vec2<T>>,
 }
@@ -96,10 +97,15 @@ impl<T: Scalar + Copy> Polygon<T> {
     /// 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.
     /// Returns the Polygons themselves, if there is no overlap
-    // TODO: At the moment, counts holes in the middle as a different polygon. These should be
-    // attached to the polygon they are inside of.
-    pub fn unite(self, _other: Polygon<T>) -> Vec<Polygon<T>> {
-        unimplemented!()
+    pub fn unite(self, other: Polygon<T>) -> Vec<Polygon<T>>
+    where
+        T: RealField,
+    {
+        let mut graph = PolygonGraph::from_polygon(&self);
+        graph.add_all(&other);
+
+        // TODO: Make bounding box support multiple polygons
+        vec![graph.bounding_polygon()]
     }
 }
 
@@ -153,7 +159,7 @@ mod test {
 
         println!("Union of the two polygons: {:?}", union);
 
-        assert_eq!(union.corners.len(), 10);
+        assert_eq!(union.corners.len(), 11);
         assert!(union
             .corners
             .iter()
diff --git a/src/math/polygon_graph.rs b/src/math/polygon_graph.rs
index 43c3bc4..7721cbf 100644
--- a/src/math/polygon_graph.rs
+++ b/src/math/polygon_graph.rs
@@ -220,10 +220,6 @@ impl<T: Scalar + Copy + PartialOrd> PolygonGraph<T> {
                 }
 
                 if let Some(intersection) = LineSegment::intersection(&segment, &compare_segment) {
-                    println!(
-                        "Found intersection between {:?} and {:?}: {:?}",
-                        &segment, &compare_segment, &intersection
-                    );
                     intersections.push(intersection);
                 }
             }
@@ -253,8 +249,52 @@ impl<T: Scalar + Copy + PartialOrd> PolygonGraph<T> {
     /// 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!()
+    pub fn bounding_polygon(mut self) -> Polygon<T>
+    where
+        T: RealField,
+    {
+        assert!(self.num_nodes() >= 3);
+        self.intersect_self();
+
+        /* Start with the top-left node. Since the nodes are always sorted by y over x from top to
+         * bottom and left to right, this is the very first element in the vector. This is also a
+         * corner, because for such a node to be enveloped, there would have to be a node further
+         * to the top, in which case that node would have been selected.
+         */
+        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]);
+        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
+             * in a clockwise direction, this yields the hull around the polygon.
+             * NOTE: Going left is just as viable, but we would have to handle the case where the
+             * algorithm would try to go back because the edge back has 0 degrees, which would be
+             * always preferred. Going right makes going back the absolute worst option.
+             */
+            let next_corner = current_node
+                .adjacent
+                .iter()
+                .max_by(|&a, &b| {
+                    super::triplet_angle(last_vec, current_node.vec, *a)
+                        .partial_cmp(&super::triplet_angle(last_vec, current_node.vec, *b))
+                        .unwrap_or(Ordering::Equal)
+                })
+                .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] {
+                break;
+            }
+
+            bounding_polygon.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
     }
 }
 
@@ -356,6 +396,7 @@ mod test {
         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(2., 2.), &Vec2::new(0., 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)));
     }
@@ -395,15 +436,15 @@ mod test {
 
         assert_eq!(
             bounding.corners[(start_i + 1) % num_corners],
-            Vec2::new(0., 2.)
+            Vec2::new(2., 0.)
         );
         assert_eq!(
             bounding.corners[(start_i + 2) % num_corners],
-            Vec2::new(2., 2.)
+            Vec2::new(2.5, -0.5)
         );
         assert_eq!(
             bounding.corners[(start_i + 3) % num_corners],
-            Vec2::new(3., 1.)
+            Vec2::new(2.5, 0.0)
         );
         assert_eq!(
             bounding.corners[(start_i + 4) % num_corners],
@@ -411,15 +452,15 @@ mod test {
         );
         assert_eq!(
             bounding.corners[(start_i + 5) % num_corners],
-            Vec2::new(2.5, 0.0)
+            Vec2::new(3., 1.)
         );
         assert_eq!(
             bounding.corners[(start_i + 6) % num_corners],
-            Vec2::new(2.5, -0.5)
+            Vec2::new(2., 2.)
         );
         assert_eq!(
             bounding.corners[(start_i + 7) % num_corners],
-            Vec2::new(2., 0.)
+            Vec2::new(0., 2.)
         );
     }
 }