Merge branch 'triangulation' into polygon-rooms

8 files changed, 825 insertions, 340 deletions

diff --git a/src/math/line_segment.rs b/src/math/line_segment.rs
index e6ca70f..94f58b2 100644
--- a/src/math/line_segment.rs
+++ b/src/math/line_segment.rs
@@ -1,4 +1,4 @@
-use super::{Rect, Vec2};
+use super::{Rect, Surface, TripletOrientation, Vec2};
 use alga::general::{ClosedDiv, ClosedMul, ClosedSub};
 use nalgebra::{RealField, Scalar};
 use num_traits::Zero;
@@ -50,10 +50,10 @@ impl<T: Scalar + Copy> LineSegment<T> {
          */
 
         // Cache the necessary orientations.
-        let ls1_ls2start_orientation = triplet_orientation(ls1.start, ls1.end, ls2.start);
-        let ls1_ls2end_orientation = triplet_orientation(ls1.start, ls1.end, ls2.end);
-        let ls2_ls1start_orientation = triplet_orientation(ls2.start, ls2.end, ls1.start);
-        let ls2_ls1end_orientation = triplet_orientation(ls2.start, ls2.end, ls1.end);
+        let ls1_ls2start_orientation = super::triplet_orientation(ls1.start, ls1.end, ls2.start);
+        let ls1_ls2end_orientation = super::triplet_orientation(ls1.start, ls1.end, ls2.end);
+        let ls2_ls1start_orientation = super::triplet_orientation(ls2.start, ls2.end, ls1.start);
+        let ls2_ls1end_orientation = super::triplet_orientation(ls2.start, ls2.end, ls1.end);
 
         // Check for the first case that wase described (general case).
         if ls1_ls2start_orientation != ls1_ls2end_orientation
@@ -118,8 +118,8 @@ impl<T: Scalar + Copy> LineSegment<T> {
              * the segments. We know it's on the lines, so checking with the lines bounding box is
              * faster than checking where on the line exactly it would be.
              */
-            if Rect::bounding_rect(line_a.start, line_a.end).contains(out)
-                && Rect::bounding_rect(line_b.start, line_b.end).contains(out)
+            if Rect::bounding_rect(line_a.start, line_a.end).contains_point(&out)
+                && Rect::bounding_rect(line_b.start, line_b.end).contains_point(&out)
             {
                 Some(out)
             } else {
@@ -139,7 +139,7 @@ impl<T: Scalar + Copy> LineSegment<T> {
         assert_eq!(
             split_points
                 .iter()
-                .find(|&p| triplet_orientation(self.start, self.end, *p)
+                .find(|&p| super::triplet_orientation(self.start, self.end, *p)
                     != TripletOrientation::Collinear),
             None
         );
@@ -173,76 +173,9 @@ impl<T: Scalar + Copy> LineSegment<T> {
     }
 }
 
-#[derive(PartialEq, Eq)]
-pub(crate) enum TripletOrientation {
-    Clockwise,
-    Counterclockwise,
-    Collinear,
-}
-
-/// Helper function to determine which direction one would turn to traverse from the first point
-/// over the second to the third point. The third option is collinear, in which case the three points
-/// are on the same line.
-pub(crate) fn triplet_orientation<T>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> TripletOrientation
-where
-    T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero,
-{
-    /* Check the slopes of the vector from a to b and b to c. If the slope of ab is greater than
-     * that of bc, the rotation is clockwise. If ab is smaller than bc it's counterclockwise. If
-     * 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::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() {
@@ -256,32 +189,4 @@ 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/mod.rs b/src/math/mod.rs
index 0b591d7..6f83c98 100644
--- a/src/math/mod.rs
+++ b/src/math/mod.rs
@@ -1,17 +1,27 @@
 pub mod line_segment;
 pub mod polygon;
-pub mod polygon_graph;
 pub mod rect;
+pub mod triangle;
 pub mod vec2;
 
 pub use self::line_segment::*;
 pub use self::polygon::*;
-pub use self::polygon_graph::*;
 pub use self::rect::*;
+pub use self::triangle::*;
 pub use self::vec2::*;
 
+use nalgebra::Scalar;
 use std::cmp::Ordering;
 
+/// Trait that describes an area in the vector space on the field of T
+pub trait Surface<T: Scalar + Copy> {
+    /// Checks if a point lies on this surface.
+    fn contains_point(&self, point: &Vec2<T>) -> bool;
+
+    /// Checks if a line segment is entirely contained by this surface.
+    fn contains_line_segment(&self, line_segment: &LineSegment<T>) -> bool;
+}
+
 /// Round a floating point number to the nearest step given by the step argument. For instance, if
 /// the step is 0.5, then all numbers from 0.0 to 0.24999... will be 0., while all numbers from
 /// 0.25 to 0.74999... will be 0.5 and so on.
diff --git a/src/math/polygon.rs b/src/math/polygon.rs
deleted file mode 100644
index 5cf8104..0000000
--- a/src/math/polygon.rs
+++ /dev/null
@@ -1,170 +0,0 @@
-use super::{PolygonGraph, Vec2};
-use nalgebra::{ClosedDiv, ClosedMul, ClosedSub, RealField, Scalar};
-use num_traits::Zero;
-use serde::{Deserialize, Serialize};
-use std::ops::Neg;
-
-#[derive(Debug, Deserialize, Serialize, PartialEq)]
-// TODO: Support polygons with holes
-pub struct Polygon<T: Scalar + Copy> {
-    pub corners: Vec<Vec2<T>>,
-}
-
-impl<T: Scalar + Copy> Polygon<T> {
-    pub fn new(corners: Vec<Vec2<T>>) -> Self {
-        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.
-    pub fn contains_point(&self, p: Vec2<T>) -> bool
-    where
-        T: Zero + ClosedSub + ClosedMul + ClosedDiv + Neg<Output = T> + PartialOrd,
-    {
-        let n = self.corners.len();
-
-        let a = self.corners[n - 1];
-        let mut b = self.corners[n - 2];
-        let mut ax;
-        let mut ay = a.y - p.y;
-        let mut bx = b.x - p.x;
-        let mut by = b.y - p.y;
-
-        let mut lup = by > ay;
-        for i in 0..n {
-            // ax = bx;
-            ay = by;
-            b = self.corners[i];
-            bx = b.x - p.x;
-            by = b.y - p.y;
-
-            if ay == by {
-                continue;
-            }
-            lup = by > ay;
-        }
-
-        let mut depth = 0;
-        for i in 0..n {
-            ax = bx;
-            ay = by;
-            let b = &self.corners[i];
-            bx = b.x - p.x;
-            by = b.y - p.y;
-
-            if ay < T::zero() && by < T::zero() {
-                // both "up" or both "down"
-                continue;
-            }
-            if ay > T::zero() && by > T::zero() {
-                // both "up" or both "down"
-                continue;
-            }
-            if ax < T::zero() && bx < T::zero() {
-                // both points on the left
-                continue;
-            }
-
-            if ay == by && (if ax < bx { ax } else { bx }) <= T::zero() {
-                return true;
-            }
-            if ay == by {
-                continue;
-            }
-
-            let lx = ax + (((bx - ax) * -ay) / (by - ay));
-            if lx == T::zero() {
-                // point on edge
-                return true;
-            }
-            if lx > T::zero() {
-                depth += 1;
-            }
-            if ay == T::zero() && lup && by > ay {
-                // hit vertex, both up
-                depth -= 1;
-            }
-            if ay == T::zero() && !lup && by < ay {
-                // hit vertex, both down
-                depth -= 1;
-            }
-
-            lup = by > ay;
-        }
-
-        (depth & 1) == 1
-    }
-
-    /// 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
-    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()]
-    }
-}
-
-#[cfg(test)]
-mod test {
-    use super::*;
-
-    #[test]
-    fn polygon_contains() {
-        let polygon = Polygon::new(vec![
-            Vec2::new(0., 0.),
-            Vec2::new(-1., 1.),
-            Vec2::new(0., 2.),
-            Vec2::new(1., 3.),
-            Vec2::new(3., 1.5),
-            Vec2::new(2., 0.),
-            Vec2::new(1., 1.),
-        ]);
-
-        assert!(!polygon.contains_point(Vec2::new(1., -2.)));
-        assert!(!polygon.contains_point(Vec2::new(-1., 0.5)));
-        assert!(polygon.contains_point(Vec2::new(0., 0.5)));
-        assert!(polygon.contains_point(Vec2::new(0.5, 1.)));
-        assert!(polygon.contains_point(Vec2::new(0.5, 1.5)));
-        assert!(!polygon.contains_point(Vec2::new(-2., 1.9)));
-        assert!(!polygon.contains_point(Vec2::new(0., 3.)));
-        assert!(polygon.contains_point(Vec2::new(1., 3.)));
-    }
-
-    #[test]
-    fn polygon_union() {
-        let first = Polygon::new(vec![
-            Vec2::new(-2., 1.),
-            Vec2::new(-0.5, 2.5),
-            Vec2::new(2., 2.),
-            Vec2::new(0.5, 1.5),
-            Vec2::new(1., 0.),
-            Vec2::new(-0.5, 1.),
-        ]);
-
-        let second = Polygon::new(vec![
-            Vec2::new(0., 0.),
-            Vec2::new(-2., 2.),
-            Vec2::new(3., 2.),
-            Vec2::new(1.5, 0.),
-        ]);
-
-        let union = first.unite(second);
-        assert_eq!(union.len(), 1);
-        let union = &union[0];
-
-        println!("Union of the two polygons: {:?}", union);
-
-        assert_eq!(union.corners.len(), 11);
-        assert!(union
-            .corners
-            .iter()
-            .find(|&p| p.x == 0. && p.y == 0.)
-            .is_some());
-    }
-}
diff --git a/src/math/polygon/mod.rs b/src/math/polygon/mod.rs
new file mode 100644
index 0000000..cda1f2a
--- /dev/null
+++ b/src/math/polygon/mod.rs
@@ -0,0 +1,341 @@
+//! Contains functions and structures to help with operations on polygons.
+
+pub mod polygon_graph;
+pub mod triangulate;
+
+pub use polygon_graph::*;
+pub use triangulate::*;
+
+use super::{LineSegment, Surface, TripletOrientation, Vec2};
+use crate::math;
+use nalgebra::{ClosedDiv, ClosedMul, ClosedSub, RealField, Scalar};
+use num_traits::Zero;
+use serde::{Deserialize, Serialize};
+use std::ops::Neg;
+
+#[derive(Debug, Deserialize, Serialize)]
+// TODO: Support polygons with holes
+pub struct Polygon<T: Scalar + Copy> {
+    pub corners: Vec<Vec2<T>>,
+}
+
+impl<T: Scalar + Copy> Polygon<T> {
+    pub fn new(corners: Vec<Vec2<T>>) -> Self {
+        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.
+
+    /// 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
+    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()]
+    }
+}
+
+impl<
+        T: Scalar
+            + Copy
+            + ClosedSub
+            + ClosedMul
+            + ClosedDiv
+            + Neg<Output = T>
+            + PartialOrd
+            + RealField
+            + Zero,
+    > Surface<T> for Polygon<T>
+{
+    fn contains_point(&self, p: &Vec2<T>) -> bool {
+        let n = self.corners.len();
+
+        let a = self.corners[n - 1];
+        let mut b = self.corners[n - 2];
+        let mut ax;
+        let mut ay = a.y - p.y;
+        let mut bx = b.x - p.x;
+        let mut by = b.y - p.y;
+
+        let mut lup = by > ay;
+        for i in 0..n {
+            // ax = bx;
+            ay = by;
+            b = self.corners[i];
+            bx = b.x - p.x;
+            by = b.y - p.y;
+
+            if ay == by {
+                continue;
+            }
+            lup = by > ay;
+        }
+
+        let mut depth = 0;
+        for i in 0..n {
+            ax = bx;
+            ay = by;
+            let b = &self.corners[i];
+            bx = b.x - p.x;
+            by = b.y - p.y;
+
+            if ay < T::zero() && by < T::zero() {
+                // both "up" or both "down"
+                continue;
+            }
+            if ay > T::zero() && by > T::zero() {
+                // both "up" or both "down"
+                continue;
+            }
+            if ax < T::zero() && bx < T::zero() {
+                // both points on the left
+                continue;
+            }
+
+            if ay == by && (if ax < bx { ax } else { bx }) <= T::zero() {
+                return true;
+            }
+            if ay == by {
+                continue;
+            }
+
+            let lx = ax + (((bx - ax) * -ay) / (by - ay));
+            if lx == T::zero() {
+                // point on edge
+                return true;
+            }
+            if lx > T::zero() {
+                depth += 1;
+            }
+            if ay == T::zero() && lup && by > ay {
+                // hit vertex, both up
+                depth -= 1;
+            }
+            if ay == T::zero() && !lup && by < ay {
+                // hit vertex, both down
+                depth -= 1;
+            }
+
+            lup = by > ay;
+        }
+
+        (depth & 1) == 1
+    }
+
+    fn contains_line_segment(&self, line_segment: &LineSegment<T>) -> bool {
+        /* In case at least one of the points is not contained by the polygon, the line cannot lie
+         * inside of the polygon in its entirety.
+         */
+        if !self.contains_point(&line_segment.start) || !self.contains_point(&line_segment.end) {
+            return false;
+        }
+
+        /* Both end-points are inside the polygon. */
+
+        /* In case the an endpoint of the line segment is equal to a corner of the polygon, it's
+         * not enough to merely check one edge, since if the corner is reflex, the segment may
+         * still be inside, eventhough its similar to the outwards pointing normal of one edge, but
+         * may be to the inside of the other edge.
+         */
+        let mut start_looks_inside = false;
+        let mut end_looks_inside = false;
+        /* Helper function that checks if a point p, when starting from the given corner c is in a
+         * direction so that considering both edges that are connected to c, the point is in the
+         * direction of the inside of the polygon.
+         */
+        let corner_vec_pointing_inside = |p: Vec2<T>, c: usize| {
+            let prev = (c + self.corners.len() - 1) % self.corners.len();
+            let next = (c + 1) % self.corners.len();
+
+            let edge_angle =
+                math::triplet_angle(self.corners[prev], self.corners[c], self.corners[next]);
+            let vec_angle = math::triplet_angle(self.corners[prev], self.corners[c], p);
+
+            vec_angle == T::zero() || vec_angle >= edge_angle
+        };
+
+        for c in 0..self.corners.len() {
+            if line_segment.start == self.corners[c] {
+                start_looks_inside = corner_vec_pointing_inside(line_segment.end, c);
+                if !start_looks_inside {
+                    return false;
+                }
+            }
+            if line_segment.end == self.corners[c] {
+                end_looks_inside = corner_vec_pointing_inside(line_segment.start, c);
+                if !end_looks_inside {
+                    return false;
+                }
+            }
+        }
+
+        if start_looks_inside && end_looks_inside {
+            return true;
+        }
+
+        /* Check the intersections of the line segment with all polygon edges and see if it is
+         * piercing through any of them.
+         */
+        for c in 0..self.corners.len() {
+            let next = (c + 1) % self.corners.len();
+
+            let current_edge = LineSegment::new(self.corners[c], self.corners[next]);
+
+            if LineSegment::intersect(&line_segment, &current_edge) {
+                let orientation_start = math::triplet_orientation(
+                    current_edge.start,
+                    current_edge.end,
+                    line_segment.start,
+                );
+                let orientation_end = math::triplet_orientation(
+                    current_edge.start,
+                    current_edge.end,
+                    line_segment.end,
+                );
+                match (orientation_start, orientation_end) {
+                    /* If at least one of the points is on the edge, make sure, the line points
+                     * inside of the polygon, not to the outside.
+                     */
+                    (TripletOrientation::Collinear, o) => {
+                        if !start_looks_inside && o == TripletOrientation::Clockwise {
+                            return false;
+                        }
+                    }
+                    (o, TripletOrientation::Collinear) => {
+                        if !end_looks_inside && o == TripletOrientation::Clockwise {
+                            return false;
+                        }
+                    }
+                    /* Start and endpoint are on different sides of the edge, therefore the line
+                     * must be partially outside.
+                     */
+                    _ => return false,
+                }
+            }
+        }
+
+        true
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use super::*;
+
+    #[test]
+    fn polygon_contains() {
+        let polygon = Polygon::new(vec![
+            Vec2::new(0., 0.),
+            Vec2::new(-1., 1.),
+            Vec2::new(0., 2.),
+            Vec2::new(1., 3.),
+            Vec2::new(3., 1.5),
+            Vec2::new(2., 0.),
+            Vec2::new(1., 1.),
+        ]);
+
+        assert!(!polygon.contains_point(&Vec2::new(1., -2.)));
+        assert!(!polygon.contains_point(&Vec2::new(-1., 0.5)));
+        assert!(polygon.contains_point(&Vec2::new(0., 0.5)));
+        assert!(polygon.contains_point(&Vec2::new(0.5, 1.)));
+        assert!(polygon.contains_point(&Vec2::new(0.5, 1.5)));
+        assert!(!polygon.contains_point(&Vec2::new(-2., 1.9)));
+        assert!(!polygon.contains_point(&Vec2::new(0., 3.)));
+        assert!(polygon.contains_point(&Vec2::new(1., 3.)));
+    }
+
+    #[test]
+    fn contains_line_segment() {
+        let polygon = Polygon::new(vec![
+            Vec2::new(0., 0.),
+            Vec2::new(0., 4.5),
+            Vec2::new(6.5, 4.5),
+            Vec2::new(5.5, 0.),
+            Vec2::new(5.5, 3.),
+            Vec2::new(1.5, 3.),
+            Vec2::new(1.5, 1.),
+            Vec2::new(2., 0.5),
+            Vec2::new(4., 2.),
+            Vec2::new(4., 0.),
+        ]);
+
+        /* NOTE: From now on, inside means inside the polygon, but might be on an edge or on a
+         * corner point, really inside means inside and not on an edge.
+         */
+
+        // Start point really inside, end point really inside. Line not completely inside.
+        assert!(!polygon
+            .contains_line_segment(&LineSegment::new(Vec2::new(2.5, 0.5), Vec2::new(0.5, 2.5))));
+
+        // Start point on edge, end point on corner, line completely outside.
+        assert!(!polygon
+            .contains_line_segment(&LineSegment::new(Vec2::new(1.5, 2.), Vec2::new(4., 2.))));
+
+        // Start point on edge, end point on edge, line inside.
+        assert!(polygon
+            .contains_line_segment(&LineSegment::new(Vec2::new(3.5, 3.), Vec2::new(3.5, 4.5))));
+
+        // Start point on corner, end point on corner, line inside.
+        assert!(polygon
+            .contains_line_segment(&LineSegment::new(Vec2::new(5.5, 3.), Vec2::new(6.5, 4.5))));
+
+        // Start point really inside, end point on edge. Line not inside.
+        assert!(!polygon
+            .contains_line_segment(&LineSegment::new(Vec2::new(3.5, 0.5), Vec2::new(5.5, 0.5))));
+
+        // Start point and endpoint outside. Line completely outside.
+        assert!(!polygon
+            .contains_line_segment(&LineSegment::new(Vec2::new(7.0, 0.), Vec2::new(7.5, 1.))));
+
+        // Start point on vertex, pointing in same dir as one of the adjacent edge normals,
+        // completely inside.
+        assert!(
+            polygon.contains_line_segment(&LineSegment::new(Vec2::new(2., 0.5), Vec2::new(4., 0.)))
+        );
+
+        // Start and end point on vertex, not pointing in the dir of adjacent edge normals,
+        // not completely inside.
+        assert!(
+            !polygon.contains_line_segment(&LineSegment::new(Vec2::new(4., 2.), Vec2::new(0., 0.)))
+        );
+    }
+
+    #[test]
+    fn polygon_union() {
+        let first = Polygon::new(vec![
+            Vec2::new(-2., 1.),
+            Vec2::new(-0.5, 2.5),
+            Vec2::new(2., 2.),
+            Vec2::new(0.5, 1.5),
+            Vec2::new(1., 0.),
+            Vec2::new(-0.5, 1.),
+        ]);
+
+        let second = Polygon::new(vec![
+            Vec2::new(0., 0.),
+            Vec2::new(-2., 2.),
+            Vec2::new(3., 2.),
+            Vec2::new(1.5, 0.),
+        ]);
+
+        let union = first.unite(second);
+        assert_eq!(union.len(), 1);
+        let union = &union[0];
+
+        println!("Union of the two polygons: {:?}", union);
+
+        assert_eq!(union.corners.len(), 11);
+        assert!(union
+            .corners
+            .iter()
+            .find(|&p| p.x == 0. && p.y == 0.)
+            .is_some());
+    }
+}
diff --git a/src/math/polygon_graph.rs b/src/math/polygon/polygon_graph.rs
index 7721cbf..9477fbc 100644
--- a/src/math/polygon_graph.rs
+++ b/src/math/polygon/polygon_graph.rs
@@ -1,4 +1,5 @@
-use super::{LineSegment, Polygon, Vec2};
+use super::Polygon;
+use crate::math::{self, LineSegment, Vec2};
 use nalgebra::{RealField, Scalar};
 use std::cmp::{Ordering, PartialOrd};
 
@@ -9,7 +10,7 @@ struct Node<T: Scalar + Copy> {
 }
 
 struct EdgeIterator<'a, T: Scalar + Copy> {
-    nodes: &'a Vec<Node<T>>,
+    nodes: &'a [Node<T>],
     pos: (usize, usize),
 }
 
@@ -39,7 +40,7 @@ fn find_node<T: Scalar + Copy + PartialOrd>(
 }
 
 impl<'a, T: Scalar + Copy> EdgeIterator<'a, T> {
-    pub fn new(nodes: &'a Vec<Node<T>>) -> Self {
+    pub fn new(nodes: &'a [Node<T>]) -> Self {
         Self { nodes, pos: (0, 0) }
     }
 }
@@ -99,11 +100,7 @@ impl<T: Scalar + Copy + PartialOrd> PolygonGraph<T> {
     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
-            }
+            find_vec2(&self.nodes[from].adjacent, to).is_ok()
         } else {
             false
         }
@@ -277,8 +274,8 @@ impl<T: Scalar + Copy + PartialOrd> PolygonGraph<T> {
                 .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))
+                    math::triplet_angle(last_vec, current_node.vec, *a)
+                        .partial_cmp(&math::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)");
diff --git a/src/math/polygon/triangulate.rs b/src/math/polygon/triangulate.rs
new file mode 100644
index 0000000..78dfa03
--- /dev/null
+++ b/src/math/polygon/triangulate.rs
@@ -0,0 +1,149 @@
+//! Module for turning a polygon into a number of non-overlapping triangles.
+
+use super::Polygon;
+use crate::math::{self, LineSegment, Surface, Triangle};
+use nalgebra::{RealField, Scalar};
+
+/// Type that saves the flags that match a corner in a space efficient manner.
+type Flags = u8;
+
+/// Tells the algorithm that this corner of the polygon is an ear. An ear means the adjacent corners
+/// form a triangle with this corner of which the area is entirely contained by the polygon itself.
+const FLAG_EAR: Flags = 0b0000_0001;
+/// Tells the algorithm that this corner is convex, meaning its internal angle is less than Pi.
+/// Useful, because this is a necessary condition for earness. False if the vertex is reflex.
+// TODO: The convex flag is a remnant from the previous algorithm, but currently it's not being
+// used. Consider removing it entirely.
+const FLAG_CONVEX: Flags = 0b0000_0010;
+
+fn flag_corner<T: Scalar + Copy>(polygon: &Polygon<T>, corner: usize) -> Flags
+where
+    T: RealField,
+{
+    // First, check if it is convex. If it is not, it can also not be an ear.
+    let prev = (corner + polygon.corners.len() - 1) % polygon.corners.len();
+    let next = (corner + 1) % polygon.corners.len();
+
+    /* Since the angle is also in counterclockwise direction, like the polygon itself, the corner
+     * is convex if and only if the angle is **not**.
+     */
+    if math::triplet_angle(
+        polygon.corners[prev],
+        polygon.corners[corner],
+        polygon.corners[next],
+    ) < T::pi()
+    {
+        // The corner is reflex.
+        return 0b0;
+    }
+
+    // The corner is convex, check if it is also an ear.
+    if polygon.contains_line_segment(&LineSegment::new(
+        polygon.corners[prev],
+        polygon.corners[next],
+    )) {
+        // Corner is an ear.
+        FLAG_EAR | FLAG_CONVEX
+    } else {
+        // Corner is not an ear.
+        FLAG_CONVEX
+    }
+}
+
+/// Uses earclipping algorithm (see https://www.geometrictools.com/Documentation/TriangulationByEarClipping.pdf)
+/// to find an explanation of what exactly is happening.
+/// Currently only handles simple polygons, but once the polygon struct supports holes must be
+/// extended to also support those.
+pub fn triangulate<T: Scalar + Copy>(mut polygon: Polygon<T>) -> Vec<Triangle<T>>
+where
+    T: RealField,
+{
+    assert!(polygon.corners.len() >= 3);
+    /* Information about the corner of the polygon. See the flags constant for information about
+     * what the bits mean.
+     */
+    let mut flags = Vec::with_capacity(polygon.corners.len());
+    for c in 0..polygon.corners.len() {
+        flags.push(flag_corner(&polygon, c));
+    }
+
+    let mut triangles = Vec::with_capacity(polygon.corners.len() - 2);
+    // Clip ears until there's only the last triangle left.
+    /* NOTE: This could be changed to > 2 and the last triangle would be pushed inside the loop,
+     * because it is also detected as an ear, however this is more logical to the original idea
+     * imo.
+     */
+    while polygon.corners.len() > 3 {
+        // Find the ear with the highest index.
+        let ear = flags
+            .iter()
+            .rposition(|&x| (x & FLAG_EAR) != 0)
+            .expect("Polygon has more than three vertices, but no ear.");
+
+        // Add the ear's triangle to the list.
+        {
+            let prev = (ear + polygon.corners.len() - 1) % polygon.corners.len();
+            let next = (ear + 1) % polygon.corners.len();
+            triangles.push(Triangle::new(
+                polygon.corners[prev],
+                polygon.corners[ear],
+                polygon.corners[next],
+            ));
+
+            // Remove the ear from the polygon and the flag list.
+            polygon.corners.remove(ear);
+            flags.remove(ear);
+        }
+
+        // Reassess the status of the two adjacent points. Notice that since the ear was removed,
+        // their array positions have changed.
+        let prev = if ear == 0 || ear == polygon.corners.len() {
+            polygon.corners.len() - 1
+        } else {
+            ear - 1
+        };
+        let next = if ear == polygon.corners.len() { 0 } else { ear };
+
+        flags[prev] = flag_corner(&polygon, prev);
+        flags[next] = flag_corner(&polygon, next);
+    }
+
+    // Push the remaining triangle into the list.
+    triangles.push(Triangle::new(
+        polygon.corners[0],
+        polygon.corners[1],
+        polygon.corners[2],
+    ));
+
+    triangles
+}
+
+#[cfg(test)]
+mod test {
+    use super::*;
+    use crate::math::Vec2;
+
+    #[test]
+    fn triangulate() {
+        let polygon = Polygon::new(vec![
+            Vec2::new(0., 0.),
+            Vec2::new(0., 4.5),
+            Vec2::new(6.5, 4.5),
+            Vec2::new(5.5, 0.),
+            Vec2::new(5.5, 3.),
+            Vec2::new(1.5, 3.),
+            Vec2::new(1.5, 1.),
+            Vec2::new(2., 0.5),
+            Vec2::new(4., 2.),
+            Vec2::new(4., 0.),
+        ]);
+
+        let triangles = super::triangulate(polygon);
+
+        assert_eq!(triangles.len(), 8);
+        assert_eq!(
+            triangles[0],
+            (Triangle::new(Vec2::new(2., 0.5), Vec2::new(4., 2.), Vec2::new(4., 0.)))
+        );
+    }
+}
diff --git a/src/math/rect.rs b/src/math/rect.rs
index 6988b2c..876e728 100644
--- a/src/math/rect.rs
+++ b/src/math/rect.rs
@@ -1,6 +1,6 @@
-use super::Vec2;
+use super::{LineSegment, Surface, Vec2};
 //use alga::general::{Additive, Identity};
-use nalgebra::{RealField, Scalar};
+use nalgebra::{ClosedAdd, RealField, Scalar};
 use num_traits::identities::Zero;
 use serde::{Deserialize, Serialize};
 use std::ops::{Add, AddAssign, Sub};
@@ -18,48 +18,6 @@ pub struct Rect<T: Scalar + Copy> {
     pub h: T,
 }
 
-// This is sad, but also sadly necessary :/
-impl<T: Into<f32> + Scalar + Copy> Into<raylib::ffi::Rectangle> for Rect<T> {
-    fn into(self) -> raylib::ffi::Rectangle {
-        raylib::ffi::Rectangle {
-            x: self.x.into(),
-            y: self.y.into(),
-            width: self.w.into(),
-            height: self.h.into(),
-        }
-    }
-}
-impl<T: From<f32> + Scalar + Copy> From<raylib::ffi::Rectangle> for Rect<T> {
-    fn from(r: raylib::ffi::Rectangle) -> Self {
-        Self {
-            x: T::from(r.x),
-            y: T::from(r.y),
-            w: T::from(r.width),
-            h: T::from(r.height),
-        }
-    }
-}
-impl<T: Into<f32> + Scalar + Copy> Into<raylib::math::Rectangle> for Rect<T> {
-    fn into(self) -> raylib::math::Rectangle {
-        raylib::math::Rectangle {
-            x: self.x.into(),
-            y: self.y.into(),
-            width: self.w.into(),
-            height: self.h.into(),
-        }
-    }
-}
-impl<T: From<f32> + Scalar + Copy> From<raylib::math::Rectangle> for Rect<T> {
-    fn from(r: raylib::math::Rectangle) -> Self {
-        Self {
-            x: T::from(r.x),
-            y: T::from(r.y),
-            w: T::from(r.width),
-            h: T::from(r.height),
-        }
-    }
-}
-
 impl<T: Scalar + Copy> Rect<T> {
     pub fn new(x: T, y: T, w: T, h: T) -> Self {
         Self { x, y, w, h }
@@ -105,17 +63,6 @@ impl<T: Scalar + Copy> Rect<T> {
             || this.y + this.h < other.y)
     }
 
-    /// Check if the point is inside this Rect and return true if so.
-    pub fn contains(&self, point: Vec2<T>) -> bool
-    where
-        T: PartialOrd + Add<Output = T>,
-    {
-        point.x >= self.x
-            && point.x <= self.x + self.w
-            && point.y >= self.y
-            && point.y <= self.y + self.h
-    }
-
     /// Returns true if the entire rect is contained inside this rectangle.
     pub fn contains_rect(&self, rect: Rect<T>) -> bool
     where
@@ -181,6 +128,61 @@ impl<T: Scalar + Copy> Rect<T> {
     }
 }
 
+impl<T: Scalar + Copy + PartialOrd + ClosedAdd> Surface<T> for Rect<T> {
+    fn contains_point(&self, point: &Vec2<T>) -> bool {
+        point.x >= self.x
+            && point.x <= self.x + self.w
+            && point.y >= self.y
+            && point.y <= self.y + self.h
+    }
+
+    fn contains_line_segment(&self, line_segment: &LineSegment<T>) -> bool {
+        self.contains_point(&line_segment.start) && self.contains_point(&line_segment.end)
+    }
+}
+
+// This is sad, but also sadly necessary :/
+impl<T: Into<f32> + Scalar + Copy> Into<raylib::ffi::Rectangle> for Rect<T> {
+    fn into(self) -> raylib::ffi::Rectangle {
+        raylib::ffi::Rectangle {
+            x: self.x.into(),
+            y: self.y.into(),
+            width: self.w.into(),
+            height: self.h.into(),
+        }
+    }
+}
+impl<T: From<f32> + Scalar + Copy> From<raylib::ffi::Rectangle> for Rect<T> {
+    fn from(r: raylib::ffi::Rectangle) -> Self {
+        Self {
+            x: T::from(r.x),
+            y: T::from(r.y),
+            w: T::from(r.width),
+            h: T::from(r.height),
+        }
+    }
+}
+impl<T: Into<f32> + Scalar + Copy> Into<raylib::math::Rectangle> for Rect<T> {
+    fn into(self) -> raylib::math::Rectangle {
+        raylib::math::Rectangle {
+            x: self.x.into(),
+            y: self.y.into(),
+            width: self.w.into(),
+            height: self.h.into(),
+        }
+    }
+}
+impl<T: From<f32> + Scalar + Copy> From<raylib::math::Rectangle> for Rect<T> {
+    fn from(r: raylib::math::Rectangle) -> Self {
+        Self {
+            x: T::from(r.x),
+            y: T::from(r.y),
+            w: T::from(r.width),
+            h: T::from(r.height),
+        }
+    }
+}
+
 #[cfg(test)]
 mod test {
     use super::*;
diff --git a/src/math/triangle.rs b/src/math/triangle.rs
new file mode 100644
index 0000000..35bdcec
--- /dev/null
+++ b/src/math/triangle.rs
@@ -0,0 +1,251 @@
+use super::{LineSegment, Vec2};
+use alga::general::{ClosedMul, ClosedSub};
+use nalgebra::{RealField, Scalar};
+use num_traits::Zero;
+
+/// Represents a triangle
+#[derive(Debug)]
+pub struct Triangle<T: Scalar + Copy> {
+    /// The three corners of the triangle. Internally, it is made sure that the corners are always
+    /// ordered in a counterclockwise manner, to make operations like contains simpler.
+    corners: [Vec2<T>; 3],
+}
+
+impl<T: Scalar + Copy> Triangle<T> {
+    /// Create a new Triangle as defined by its three corner points
+    pub fn new(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> Self
+    where
+        T: ClosedSub + ClosedMul + PartialOrd + Zero,
+    {
+        // Make sure the three points are in counterclockwise order.
+        match triplet_orientation(a, b, c) {
+            TripletOrientation::Counterclockwise => Self { corners: [a, b, c] },
+            TripletOrientation::Clockwise => Self { corners: [a, c, b] },
+            TripletOrientation::Collinear => {
+                warn!(
+                    "Creating triangle without any area: [{:?}, {:?}, {:?}]",
+                    a, b, c
+                );
+                Self { corners: [a, b, c] }
+            }
+        }
+    }
+
+    /// Get the corners immutably
+    pub fn corners(&self) -> &[Vec2<T>; 3] {
+        &self.corners
+    }
+
+    /// Create a new Triangle from a three-point slice, instead of the three points one after
+    /// another.
+    pub fn from_slice(corners: [Vec2<T>; 3]) -> Self
+    where
+        T: ClosedSub + ClosedMul + PartialOrd + Zero,
+    {
+        Self::new(corners[0], corners[1], corners[2])
+    }
+
+    /// Check if the triangle contains a given point. If the point is right on an edge, it still
+    /// counts as inside it.
+    pub fn contains_point(&self, point: Vec2<T>) -> bool
+    where
+        T: ClosedSub + ClosedMul + PartialOrd + Zero,
+    {
+        // Since the points are ordered counterclockwise, check if the point is to the left of all
+        // edges (or on an edge) from one point to the next. When the point is to the left of all
+        // edges, it must be inside the triangle.
+        for i in 0..3 {
+            if triplet_orientation(self.corners[i], self.corners[(i + 1) % 3], point)
+                == TripletOrientation::Clockwise
+            {
+                return false;
+            }
+        }
+
+        true
+    }
+}
+
+/// Convert a three-point-slice into a triangle
+impl<T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero> From<[Vec2<T>; 3]>
+    for Triangle<T>
+{
+    fn from(corners: [Vec2<T>; 3]) -> Self {
+        Self::new(corners[0], corners[1], corners[2])
+    }
+}
+/// Convert a triangle into a three-point-slice. The corners are in counterclockwise order.
+impl<T: Scalar + Copy> Into<[Vec2<T>; 3]> for Triangle<T> {
+    fn into(self) -> [Vec2<T>; 3] {
+        self.corners
+    }
+}
+
+impl<T: Scalar + Copy + PartialEq> PartialEq for Triangle<T> {
+    fn eq(&self, other: &Self) -> bool {
+        // The indexes of the elements are not important, so try all shifting options.
+        for shift in 0..=2 {
+            if self
+                .corners
+                .iter()
+                .enumerate()
+                .find(|(i, &c)| c != other.corners[(i + shift) % 3])
+                .is_none()
+            {
+                return true;
+            }
+        }
+
+        false
+    }
+}
+
+#[derive(PartialEq, Eq)]
+pub(crate) enum TripletOrientation {
+    Clockwise,
+    Counterclockwise,
+    Collinear,
+}
+
+/// Helper function to determine which direction one would turn to traverse from the first point
+/// over the second to the third point. The third option is collinear, in which case the three points
+/// are on the same line.
+pub(crate) fn triplet_orientation<T>(a: Vec2<T>, b: Vec2<T>, c: Vec2<T>) -> TripletOrientation
+where
+    T: Scalar + Copy + ClosedSub + ClosedMul + PartialOrd + Zero,
+{
+    /* Check the slopes of the vector from a to b and b to c. If the slope of ab is greater than
+     * that of bc, the rotation is clockwise. If ab is smaller than bc it's counterclockwise. If
+     * 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::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.
+    match orientation {
+        TripletOrientation::Counterclockwise => T::pi() + (T::pi() - angle),
+        _ => angle,
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use super::*;
+    use std::f64::consts::PI;
+
+    #[test]
+    fn new() {
+        let a = Vec2::new(0., 0.);
+        let b = Vec2::new(0., 1.);
+        let c = Vec2::new(1., 1.);
+
+        // Create with counterclockwise order.
+        let triangle = Triangle::new(a, b, c);
+        assert_eq!(triangle.corners(), &[a, b, c]);
+
+        // Create with clockwise order.
+        let triangle = Triangle::new(a, c, b);
+        assert_eq!(triangle.corners(), &[a, b, c]);
+
+        // Create with different starting corner
+        let triangle = Triangle::from([b, c, a]);
+        assert_eq!(triangle.corners(), &[b, c, a]);
+
+        // Create with collinear corners
+        let triangle = Triangle::new(c, c, b);
+        assert_eq!(triangle.corners(), &[c, c, b]);
+    }
+
+    #[test]
+    fn contains_point() {
+        let a = Vec2::new(0., 0.);
+        let b = Vec2::new(-1., -1.);
+        let c = Vec2::new(-2., 0.);
+
+        let triangle = Triangle::new(a, b, c);
+
+        assert!(triangle.contains_point(b));
+        assert!(triangle.contains_point(Vec2::new(-0.5, -0.5)));
+        assert!(triangle.contains_point(Vec2::new(-1., -0.5)));
+        assert!(!triangle.contains_point(Vec2::new(-2., -2.)));
+    }
+
+    #[test]
+    fn equality() {
+        let a = Vec2::new(0., 0.);
+        let b = Vec2::new(-1., -1.);
+        let c = Vec2::new(-2., 0.);
+        let d = Vec2::new(-3., 0.);
+
+        let cmp = Triangle::new(a, b, c);
+
+        assert_eq!(Triangle::new(a, b, c), cmp);
+        assert_eq!(Triangle::new(c, b, a), cmp);
+        assert_eq!(Triangle::new(b, a, c), cmp);
+        assert!(Triangle::new(a, b, d) != cmp);
+    }
+
+    #[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
+        );
+    }
+}