path: root/src/math/polygon/mod.rs

//! 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, Rect, 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;
use thiserror::Error;

/// Describes errors that can happen when handling polygons, especially on creation.
#[derive(Debug, Error)]
pub enum PolygonError<T: Scalar + Copy> {
    /// Since the polygon is not allowed to be complex, self intersection is an error.
    #[error("the polygon intersects with itself with edges `{0:?}` and `{1:?}`")]
    SelfIntersect(LineSegment<T>, LineSegment<T>),
    #[error("polygons need at least 3 vertices to be valid, `{0}` where provided")]
    TooFewVertices(usize),
    #[error("vertex `{0:?}` with corner id `{1}` is or would be in the polygon twice")]
    VertexDoubling(Vec2<T>, usize),
    #[error("line `{0:?}` is not a polygon edge")]
    EdgeNotFound(LineSegment<T>),
}

#[derive(Clone, Debug, Deserialize, Serialize)]
// TODO: Support polygons with holes
pub struct Polygon<T: Scalar + Copy> {
    corners: Vec<Vec2<T>>,
}

impl<T: Scalar + Copy> Polygon<T> {
    /// Create a new polygon from the provided corners. If the corners are right-turning, they will
    /// be reversed to be left-turning.
    /// Checks if the corners make a valid polygon before creating it. If the check fails, an error
    /// will be returned.
    pub fn new(corners: Vec<Vec2<T>>) -> Result<Self, PolygonError<T>>
    where
        T: RealField,
    {
        Self::check_validity(&corners)?;

        let corners = if combined_angle(&corners) > T::zero() {
            corners
        } else {
            corners.into_iter().rev().collect()
        };

        Ok(Self { corners })
    }

    /// Like new, but does not perform any validity checks, so be careful when using this function.
    pub fn new_unchecked(corners: Vec<Vec2<T>>) -> Self
    where
        T: RealField,
    {
        assert!(Polygon::check_validity(&corners).is_ok());

        let corners = if combined_angle(&corners) > T::zero() {
            corners
        } else {
            corners.into_iter().rev().collect()
        };

        Self { corners }
    }

    /// Checks if a set of corners can be made into a polygon or not. Returns Ok if they can, or
    /// the reason they cannot in form of a PolygonError.
    pub fn check_validity(corners: &[Vec2<T>]) -> Result<(), PolygonError<T>>
    where
        T: RealField,
    {
        if corners.len() < 3 {
            return Err(PolygonError::TooFewVertices(corners.len()));
        }

        // Check that all vertices are in the slice only once.
        for i in 0..corners.len() {
            for j in (i + 1)..corners.len() {
                if corners[i] == corners[j] {
                    return Err(PolygonError::VertexDoubling(corners[i], i));
                }
            }
        }

        // Check that no edges cross paths, except the edges that are right next to each other,
        // which must intersect with each other on the common vertex.
        if corners.len() > 3 {
            for i in 0..corners.len() - 2 {
                let line_i = LineSegment::new(corners[i], corners[i + 1]);
                let end_j = if i == 0 {
                    corners.len() - 1
                } else {
                    corners.len()
                };
                for j in (i + 2)..end_j {
                    let next_j = (j + 1) % corners.len();
                    let line_j = LineSegment::new(corners[j], corners[next_j]);

                    if LineSegment::intersect(&line_i, &line_j) {
                        return Err(PolygonError::SelfIntersect(line_i, line_j));
                    }
                }
            }
        }

        Ok(())
    }

    /// 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. On success, the method returns the
    /// position of the corner in the corners list.
    ///
    /// # Failures
    /// If the corner is already present in the polygon's vertex list, the method will throw a
    /// VertexDoubling-Error. If no matching neighbour-pair can be found, an EdgeNotFound-Error
    /// will be thrown.
    pub fn add_corner(
        &mut self,
        corner: Vec2<T>,
        neighbour1: &Vec2<T>,
        neighbour2: &Vec2<T>,
    ) -> Result<usize, PolygonError<T>> {
        // Check that the corners do not contain the corner vector already.
        if let Some(pos) = self.corners.iter().position(|&c| c == corner) {
            return Err(PolygonError::VertexDoubling(corner, pos));
        }

        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 Ok(next);
            }
            if self.corners[i] == *neighbour2 && self.corners[next] == *neighbour1 {
                self.corners.insert(next, corner);
                return Ok(next);
            }
        }

        // No matching neighbour pair can be found.
        Err(PolygonError::EdgeNotFound(LineSegment::new(
            *neighbour1,
            *neighbour2,
        )))
    }

    /// 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.
    /// 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
    }

    fn contains_rect(&self, rect: &Rect<T>) -> bool {
        /* Turn the rectangle into a vector with its hull line segments. If all hull segments are
         * contained in the polygon, the rectangle is contained completely.
         */
        let hull_edges = [
            // Top left to bottom left.
            LineSegment::new(
                Vec2::new(rect.x, rect.y),
                Vec2::new(rect.x, rect.y + rect.h),
            ),
            // Bottom left to bottom right.
            LineSegment::new(
                Vec2::new(rect.x, rect.y + rect.h),
                Vec2::new(rect.x + rect.w, rect.y + rect.h),
            ),
            // Bottom right to top right.
            LineSegment::new(
                Vec2::new(rect.x + rect.w, rect.y + rect.h),
                Vec2::new(rect.x + rect.w, rect.y),
            ),
            // Top right to top left.
            LineSegment::new(
                Vec2::new(rect.x + rect.w, rect.y),
                Vec2::new(rect.x, rect.y),
            ),
        ];

        hull_edges
            .iter()
            .all(|edge| self.contains_line_segment(edge))
    }

    fn contains_polygon(&self, polygon: &Polygon<T>) -> bool {
        /* Check for all edges of the polygon that they are contained by the polygon. If they all
         * are, then the polygon itself must also be contained.
         */
        for i in 0..polygon.corners.len() {
            let next = (i + 1) % polygon.corners.len();
            if !self
                .contains_line_segment(&LineSegment::new(polygon.corners[i], polygon.corners[next]))
            {
                return false;
            }
        }

        true
    }
}

/* 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: &[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();
    }

    combined_angle
}

#[cfg(test)]
mod test {
    use super::*;

    #[test]
    fn check_validity() {
        Polygon::check_validity(&[Vec2::new(0., 0.), Vec2::new(1., 0.), Vec2::new(0., 1.)])
            .expect("Simple triangle does not pass validity check");
    }

    #[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.),
        ])
        .unwrap();

        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.),
        ])
        .unwrap();

        /* 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.),
        ])
        .unwrap();

        let second = Polygon::new(vec![
            Vec2::new(0., 0.),
            Vec2::new(-2., 2.),
            Vec2::new(3., 2.),
            Vec2::new(1.5, 0.),
        ])
        .unwrap();

        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());
    }
}