path: root/src/math/polygon_graph.rs

use super::{LineSegment, Polygon, Vec2};
use nalgebra::Scalar;
use std::cmp::{Ordering, PartialOrd};

struct Node<T: Scalar + Copy> {
    pub vec: Vec2<T>,
    pub adjacent: Vec<Vec2<T>>,
}

/// An undirected graph, that is optimised for polygon edge operations. Since edges of a polygon
/// are an undirected graph, this structure also holds both directions. This makes it rather space
/// inefficient, but makes edge operations rather swift. ß
pub struct PolygonGraph<T: Scalar + Copy + PartialOrd> {
    /// The nodes of the graph, together with their adjacency list.
    nodes: Vec<Node<T>>,
}

// Helper functions to find nodes/vecs in sorted fields, so It doesn't always have to be written
// out.
#[inline]
fn find_vec2<T: Scalar + Copy + PartialOrd>(
    field: &[Vec2<T>],
    lookup: &Vec2<T>,
) -> Result<usize, usize> {
    field.binary_search_by(|n| n.partial_cmp(lookup).unwrap_or(Ordering::Greater))
}
#[inline]
fn find_node<T: Scalar + Copy + PartialOrd>(
    field: &[Node<T>],
    lookup: &Vec2<T>,
) -> Result<usize, usize> {
    field.binary_search_by(|n| n.vec.partial_cmp(lookup).unwrap_or(Ordering::Greater))
}

impl<T: Scalar + Copy + PartialOrd> PolygonGraph<T> {
    /// Create a new, empty polygon graph.
    pub fn new() -> Self {
        Self { nodes: Vec::new() }
    }

    /// Count the number of edges in the graph. Internally, for each two connected points there are
    /// two edges, but this returns the amount of polygon edges.
    pub fn num_edges(&self) -> usize {
        let mut num_edges = 0;
        for node in &self.nodes {
            for _ in &node.adjacent {
                num_edges += 1;
            }
        }

        assert!(num_edges % 2 == 0);
        num_edges / 2
    }

    /// Count the number of nodes in this graph. If this graph consists of multiple polygons, this
    /// can be different than the amount of corners, since corners with the same position are only
    /// counted once.
    pub fn num_nodes(&self) -> usize {
        self.nodes.len()
    }

    /// Checks if there is an edge between the two given vectors. Is commutative in respect to the
    /// two arguments.
    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
            }
        } else {
            false
        }
    }

    // Helper function to add the edge into the internal graph representation for one side only.
    // Since to the outside the graph should always be undirected, this must be private.
    fn add_edge_onesided(&mut self, from: &Vec2<T>, to: &Vec2<T>) -> bool {
        match find_node(&self.nodes, from) {
            Ok(pos) => match find_vec2(&self.nodes[pos].adjacent, to) {
                Ok(_) => return false,
                Err(i) => self.nodes[pos].adjacent.insert(i, *to),
            },
            Err(pos) => self.nodes.insert(
                pos,
                Node {
                    vec: *from,
                    adjacent: vec![*to],
                },
            ),
        }

        true
    }

    /// Add an edge between the given vectors. If the edge already exists, it does nothing and
    /// returns false, otherwise it returns true after addition.
    pub fn add_edge(&mut self, from: &Vec2<T>, to: &Vec2<T>) -> bool {
        if !self.add_edge_onesided(from, to) {
            return false;
        }

        let back_edge_succ = self.add_edge_onesided(to, from);
        assert!(back_edge_succ);

        true
    }

    // Helper function to remove the edge in the internal graph representation for one side only.
    // Since to the outside the graph should always be undirected, this must be private.
    fn remove_edge_onesided(&mut self, from: &Vec2<T>, to: &Vec2<T>) -> bool {
        if let Ok(from) = find_node(&self.nodes, from) {
            if let Ok(to) = find_vec2(&self.nodes[from].adjacent, to) {
                // Remove the edge from the vector.
                self.nodes[from].adjacent.remove(to);

                // If the node has no adjacent nodes anymore, remove it entirely.
                if self.nodes[from].adjacent.is_empty() {
                    self.nodes.remove(from);
                }

                true
            } else {
                false
            }
        } else {
            false
        }
    }

    /// Remove an edge between the given vectors. If there is no edge between them, it does nothing
    /// and returns false, otherwise it returns true after deletion.
    pub fn remove_edge(&mut self, from: &Vec2<T>, to: &Vec2<T>) -> bool {
        if !self.remove_edge_onesided(from, to) {
            return false;
        }

        let back_edge_succ = self.remove_edge_onesided(to, from);
        assert!(back_edge_succ);

        true
    }

    /// Constructs a new PolygonGraph from the provided polygon. Adds a node for every corner and
    /// an edge to all connected corners (which should be exactly two, for regular polygons)
    pub fn from_polygon(polygon: &Polygon<T>) -> Self {
        let mut graph = PolygonGraph {
            nodes: Vec::with_capacity(polygon.corners.len()),
        };

        graph.add_all(polygon);
        graph
    }

    /// Add all edges of the provided polygon into the graph. Requires roughly double as much space
    /// as the normal polygon.
    pub fn add_all(&mut self, polygon: &Polygon<T>) {
        for i in 0..polygon.corners.len() {
            self.add_edge(
                &polygon.corners[i],
                &polygon.corners[(i + 1) % polygon.corners.len()],
            );
        }
    }

    /// Calculates all points where the graph edges intersect with one another. It then adds them
    /// into the adjacency list such that the intersection point lies between the nodes of the
    /// lines.
    pub fn intersect_self(&mut self) {
        // Find all intersections with all other edges.
        for node in &self.nodes {
            for to in &node.adjacent {
                let current_segment = LineSegment::new(node.vec, *to);

                for node in &self.nodes {
                    for to in &node.adjacent {
                        let compare_segment = LineSegment::new(node.vec, *to);
                        if current_segment.eq_ignore_dir(&compare_segment) {
                            continue;
                        }
                    }
                }
            }
        }
    }

    /// Finds the minimal polygon that could hold this graph together, i.e. could contain the
    /// 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!()
    }
}

impl<T: Scalar + Copy + PartialOrd> Default for PolygonGraph<T> {
    fn default() -> Self {
        Self::new()
    }
}

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

    #[test]
    fn from_polygon() {
        let a = Vec2::new(0., 0.);
        let b = Vec2::new(0., 1.);
        let c = Vec2::new(0.5, 1.);

        let triangle = Polygon::new(vec![a, b, c]);

        let graph = PolygonGraph::from_polygon(&triangle);
        assert_eq!(graph.num_edges(), 3);

        assert!(graph.has_edge(&a, &b));
        assert!(graph.has_edge(&b, &a));

        assert!(graph.has_edge(&a, &c));
        assert!(graph.has_edge(&c, &a));

        assert!(graph.has_edge(&b, &c));
        assert!(graph.has_edge(&c, &b));
    }

    #[test]
    fn add_all() {
        let top_left = Vec2::new(0., 0.);
        let top_right = Vec2::new(1., 0.);
        let bot_left = Vec2::new(0., 1.);
        let bot_right = Vec2::new(1., 1.);

        let triangle = Polygon::new(vec![top_left, bot_right, top_right]);

        let square = Polygon::new(vec![bot_left, bot_right, top_right, top_left]);

        let mut graph = PolygonGraph::new();
        graph.add_all(&triangle);
        graph.add_all(&square);

        assert_eq!(graph.num_edges(), 5);
        assert_eq!(graph.num_nodes(), 4);

        assert!(graph.has_edge(&top_left, &top_right));
        assert!(graph.has_edge(&top_right, &top_left));

        assert!(graph.has_edge(&top_left, &bot_left));
        assert!(graph.has_edge(&bot_left, &top_left));

        assert!(graph.has_edge(&bot_left, &bot_right));
        assert!(graph.has_edge(&bot_right, &bot_left));

        assert!(graph.has_edge(&bot_right, &top_right));
        assert!(graph.has_edge(&top_right, &bot_right));

        assert!(graph.has_edge(&top_left, &bot_right));
        assert!(graph.has_edge(&bot_right, &top_left));
    }

    #[test]
    fn intersect_self() {
        let first = Polygon::new(vec![
            Vec2::new(0., 0.),
            Vec2::new(0., 2.),
            Vec2::new(2., 2.),
            Vec2::new(3., 1.),
            Vec2::new(2., 0.),
        ]);

        let second = Polygon::new(vec![
            Vec2::new(2.5, -0.5),
            Vec2::new(0., 2.),
            Vec2::new(2., 2.),
            Vec2::new(2., 0.5),
            Vec2::new(2.5, 0.),
        ]);

        let mut graph = PolygonGraph::from_polygon(&first);
        graph.add_all(&second);

        graph.intersect_self();

        assert_eq!(graph.num_nodes(), 9);
        assert_eq!(graph.num_edges(), 12);

        assert!(graph.has_edge(&Vec2::new(2., 0.), &Vec2::new(2.25, 0.25)));
        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(0., 2.), &Vec2::new(2., 0.)));
        assert!(!graph.has_edge(&Vec2::new(0., 2.), &Vec2::new(2.5, -0.5)));
    }

    #[test]
    fn bounding_polygon() {
        let first = Polygon::new(vec![
            Vec2::new(0., 0.),
            Vec2::new(0., 2.),
            Vec2::new(2., 2.),
            Vec2::new(3., 1.),
            Vec2::new(2., 0.),
        ]);

        let second = Polygon::new(vec![
            Vec2::new(2.5, -0.5),
            Vec2::new(0., 2.),
            Vec2::new(2., 2.),
            Vec2::new(2., 0.5),
            Vec2::new(2.5, 0.),
        ]);

        let mut graph = PolygonGraph::from_polygon(&first);
        graph.add_all(&second);

        let bounding = graph.bounding_polygon();

        let num_corners = 8;
        assert_eq!(bounding.corners.len(), num_corners);

        // Run around the polygon to see if it was constructed correctly.
        let start_i = bounding
            .corners
            .iter()
            .position(|&x| x == Vec2::new(0., 0.))
            .expect("Starting vector does not exist in polygon.");

        assert_eq!(
            bounding.corners[(start_i + 1) % num_corners],
            Vec2::new(0., 2.)
        );
        assert_eq!(
            bounding.corners[(start_i + 2) % num_corners],
            Vec2::new(2., 2.)
        );
        assert_eq!(
            bounding.corners[(start_i + 3) % num_corners],
            Vec2::new(3., 1.)
        );
        assert_eq!(
            bounding.corners[(start_i + 4) % num_corners],
            Vec2::new(2.25, 0.25)
        );
        assert_eq!(
            bounding.corners[(start_i + 5) % num_corners],
            Vec2::new(2.5, 0.0)
        );
        assert_eq!(
            bounding.corners[(start_i + 6) % num_corners],
            Vec2::new(2.5, -0.5)
        );
        assert_eq!(
            bounding.corners[(start_i + 7) % num_corners],
            Vec2::new(2., 0.)
        );
    }
}