From 7befb3ef772d7532fb71627bcb25d8f8f7b79fc2 Mon Sep 17 00:00:00 2001 From: Martin Indra Date: Sun, 12 Mar 2023 14:36:30 +0100 Subject: [PATCH] Use Polyanya for path finding https://www.ijcai.org/proceedings/2017/0070.pdf Fixes #407. Fixes #406. Fixes #168. --- crates/pathing/src/chain.rs | 35 +-- crates/pathing/src/dijkstra.rs | 270 ----------------- crates/pathing/src/finder.rs | 90 +++--- crates/pathing/src/funnel.rs | 312 ------------------- crates/pathing/src/geometry.rs | 186 +++++++++--- crates/pathing/src/graph.rs | 117 ++++--- crates/pathing/src/interval.rs | 539 +++++++++++++++++++++++++++++++++ crates/pathing/src/lib.rs | 5 +- crates/pathing/src/node.rs | 219 ++++++++++++++ crates/pathing/src/polyanya.rs | 176 +++++++++++ 10 files changed, 1208 insertions(+), 741 deletions(-) delete mode 100644 crates/pathing/src/dijkstra.rs delete mode 100644 crates/pathing/src/funnel.rs create mode 100644 crates/pathing/src/interval.rs create mode 100644 crates/pathing/src/node.rs create mode 100644 crates/pathing/src/polyanya.rs diff --git a/crates/pathing/src/chain.rs b/crates/pathing/src/chain.rs index 8b03e914c..e3010baa1 100644 --- a/crates/pathing/src/chain.rs +++ b/crates/pathing/src/chain.rs @@ -6,9 +6,8 @@ use std::rc::Rc; use de_types::path::Path; use parry2d::math::Point; -use crate::geometry::{which_side, Side}; - /// A linked list of points which keeps track of its length in meters. +#[derive(Clone)] pub(crate) struct PointChain { prev: Option>, point: Point, @@ -58,26 +57,6 @@ impl PointChain { self.length } - /// Returns true if the point has no predecessor. - pub(crate) fn is_first(&self) -> bool { - self.prev.is_none() - } - - /// Returns relative side of a point to `self` from the perspective of the - /// parent point. Returns `None` if `self` has no parent. - /// - /// See [`crate::geometry::which_side`]. - /// - /// # Panics - /// - /// May panic if self is a degenerate point chain or of `point` coincides - /// with last but one point in self. - pub(crate) fn which_side(&self, point: Point) -> Option { - self.prev - .as_ref() - .map(|p| which_side(p.point(), self.point, point)) - } - /// Returns an iterator over points in this linked list. The iterator /// starts at `self` and traverses all predecessors. pub(crate) fn iter(&self) -> Predecessors { @@ -123,7 +102,6 @@ mod tests { fn test_chain() { let chain = PointChain::first(Point::new(1., 2.)); assert!(chain.prev().is_none()); - assert!(chain.is_first()); assert_eq!(chain.point(), Point::new(1., 2.)); assert_eq!(chain.length(), 0.); let collected: Vec> = chain.iter().map(|p| p.point()).collect(); @@ -131,23 +109,12 @@ mod tests { let chain = PointChain::extended(&Rc::new(chain), Point::new(3., 2.)); assert!(chain.prev().is_some()); - assert!(!chain.is_first()); assert_eq!(chain.point(), Point::new(3., 2.)); assert_eq!(chain.length(), 2.); let collected: Vec> = chain.iter().map(|p| p.point()).collect(); assert_eq!(collected, vec![Point::new(3., 2.), Point::new(1., 2.)]); } - #[test] - fn test_which_side() { - let chain = PointChain::first(Point::new(1., 2.)); - assert!(chain.which_side(Point::new(2., 1.)).is_none()); - - let chain = PointChain::extended(&Rc::new(chain), Point::new(3., 2.)); - assert_eq!(chain.which_side(Point::new(2., 1.)).unwrap(), Side::Left); - assert_eq!(chain.which_side(Point::new(2., 3.)).unwrap(), Side::Right); - } - #[test] fn test_to_path() { let chain = PointChain::extended( diff --git a/crates/pathing/src/dijkstra.rs b/crates/pathing/src/dijkstra.rs deleted file mode 100644 index 4f9067ef8..000000000 --- a/crates/pathing/src/dijkstra.rs +++ /dev/null @@ -1,270 +0,0 @@ -//! This module contains visibility graph based path finding algorithm. - -use std::{cmp::Ordering, collections::BinaryHeap}; - -use ahash::AHashSet; -use bevy::utils::FloatOrd; -use de_types::path::Path; -use parry2d::{math::Point, na, query::PointQuery, shape::Segment}; - -use crate::{ - funnel::Funnel, - geometry::{orient, which_side, Side}, - graph::VisibilityGraph, - PathQueryProps, -}; - -/// Finds and returns a reasonable path between two points. -/// -/// Source and target points must not lie inside or on the edge of the same -/// triangle of the triangulation from which `graph` was created. -pub(crate) fn find_path( - graph: &VisibilityGraph, - source: PointContext, - target: PointContext, - properties: PathQueryProps, -) -> Option { - let mut open_set = OpenSet::new(); - let mut explored = AHashSet::new(); - - let funnel = Funnel::new(source.point()); - for &edge_id in source.neighbours() { - open_set.push(Step::from_segment( - source.point(), - &funnel, - graph.geometry(edge_id).segment(), - edge_id, - )); - } - - let mut sufficient: Option = None; - while let Some(step) = open_set.pop() { - if !explored.insert(step.edge_id()) { - continue; - } - - let geometry = graph.geometry(step.edge_id()); - let segment = geometry.segment(); - - let projection = segment.project_local_point(&target.point(), true); - let projection_dist = na::distance(&target.point(), &projection.point); - - if properties.max_distance() > 0. && projection_dist < properties.max_distance() { - let funnel = step.funnel().clone(); - let incomplete = Incomplete::new(funnel, projection.point, projection_dist); - - if projection_dist < properties.distance() { - return incomplete.closed(properties.distance()); - } - - if sufficient - .as_ref() - .map(|s| s.distance() > incomplete.distance()) - .unwrap_or(true) - { - sufficient = Some(incomplete); - } - } - - if target.has_neighbour(step.edge_id()) - && step.side() != which_side(segment.a, segment.b, target.point()) - { - return step - .funnel() - .closed(target.point()) - .truncated(properties.distance()); - } - - for &next_edge_id in graph.neighbours(step.edge_id()) { - if explored.contains(&next_edge_id) { - continue; - } - - let next_geom = graph.geometry(next_edge_id); - if step.side() == which_side(segment.a, segment.b, next_geom.midpoint()) { - continue; - } - - open_set.push(Step::from_segment( - geometry.midpoint(), - step.funnel(), - next_geom.segment(), - next_edge_id, - )); - } - } - - sufficient.and_then(|s| s.closed(properties.distance())) -} - -pub(crate) struct PointContext { - point: Point, - neighbours: Vec, -} - -impl PointContext { - /// Creates a new point context. - /// - /// # Arguments - /// - /// * `point` - position of the point in the map - /// - /// * `neighbours` - edge IDs of all neighboring edges. If the point lies - /// on en edge or its end points, the edge should not be included in the - /// vector. - pub(crate) fn new(point: Point, neighbours: Vec) -> Self { - Self { point, neighbours } - } - - fn point(&self) -> Point { - self.point - } - - fn neighbours(&self) -> &[u32] { - self.neighbours.as_slice() - } - - fn has_neighbour(&self, edge_id: u32) -> bool { - self.neighbours.contains(&edge_id) - } -} - -/// A priority queue of path exploration expansion steps. -struct OpenSet { - heap: BinaryHeap, -} - -impl OpenSet { - fn new() -> Self { - Self { - heap: BinaryHeap::new(), - } - } - - fn pop(&mut self) -> Option { - self.heap.pop() - } - - fn push(&mut self, step: Step) { - self.heap.push(step); - } -} - -/// A path exploration step -- a jump between two neighboring triangle edges / -/// line segments -- used in the edge/triangle graph traversal algorithm. -struct Step { - score: FloatOrd, - /// From which side the edge was approached. This is the side from the - /// perspective of the edge's line segment before orientation. - side: Side, - /// Funnel expanded by all traversed edges from the starting point. - /// `edge_id` is the first edge not used in the funnel expansion. - funnel: Funnel, - /// Edge to be traversed. - edge_id: u32, -} - -impl Step { - fn from_segment(eye: Point, funnel: &Funnel, segment: Segment, edge_id: u32) -> Self { - let side = which_side(segment.a, segment.b, eye); - let segment = orient(eye, segment); - let funnel = funnel.extended(segment); - let dist = segment.distance_to_local_point(&funnel.tail().point(), true); - Self::new(funnel.tail().length() + dist, side, funnel, edge_id) - } - - fn new(score: f32, side: Side, funnel: Funnel, edge_id: u32) -> Self { - Self { - score: FloatOrd(score), - side, - funnel, - edge_id, - } - } - - fn side(&self) -> Side { - self.side - } - - fn funnel(&self) -> &Funnel { - &self.funnel - } - - fn edge_id(&self) -> u32 { - self.edge_id - } -} - -impl PartialEq for Step { - fn eq(&self, other: &Step) -> bool { - self.edge_id == other.edge_id && self.score == other.score - } -} - -impl Eq for Step {} - -impl PartialOrd for Step { - fn partial_cmp(&self, other: &Self) -> Option { - Some(self.cmp(other)) - } -} - -impl Ord for Step { - fn cmp(&self, other: &Self) -> Ordering { - (other.score, other.edge_id).cmp(&(self.score, self.edge_id)) - } -} - -struct Incomplete { - funnel: Funnel, - closest: Point, - distance: f32, -} - -impl Incomplete { - fn new(funnel: Funnel, closest: Point, distance: f32) -> Self { - debug_assert!(distance >= 0.); - Self { - funnel, - closest, - distance, - } - } - - fn distance(&self) -> f32 { - self.distance - } - - fn closed(self, truncation: f32) -> Option { - let path = self.funnel.closed(self.closest); - let truncation = truncation - self.distance; - if truncation <= 0. { - Some(path) - } else { - path.truncated(truncation) - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_open_set() { - let mut set = OpenSet::new(); - set.push(Step::new(2., Side::Left, Funnel::new(Point::origin()), 1)); - set.push(Step::new(1.1, Side::Left, Funnel::new(Point::origin()), 2)); - set.push(Step::new(4., Side::Left, Funnel::new(Point::origin()), 3)); - assert_eq!(set.pop().unwrap().edge_id(), 2); - assert_eq!(set.pop().unwrap().edge_id(), 1); - assert_eq!(set.pop().unwrap().edge_id(), 3); - } - - #[test] - fn test_step_ord() { - let step_a = Step::new(2., Side::Left, Funnel::new(Point::origin()), 1); - let step_b = Step::new(2.1, Side::Left, Funnel::new(Point::origin()), 2); - assert!(step_b < step_a); - } -} diff --git a/crates/pathing/src/finder.rs b/crates/pathing/src/finder.rs index dc2148849..174046a56 100644 --- a/crates/pathing/src/finder.rs +++ b/crates/pathing/src/finder.rs @@ -14,9 +14,9 @@ use rstar::{PointDistance, RTree, RTreeObject, AABB}; use tinyvec::{ArrayVec, TinyVec}; use crate::{ - dijkstra::{find_path, PointContext}, exclusion::ExclusionArea, - graph::VisibilityGraph, + graph::{Step, VisibilityGraph}, + polyanya::{find_path, PointContext}, utils::HashableSegment, PathTarget, }; @@ -67,7 +67,7 @@ impl PathFinder { /// covered by `triangles`. There is no intersection between the /// `exclusions` and `triangles`. pub(crate) fn from_triangles( - triangles: Vec, + mut triangles: Vec, mut exclusions: Vec, ) -> Self { let mut graph = VisibilityGraph::new(); @@ -77,7 +77,9 @@ impl PathFinder { AHashMap::with_capacity(triangles.len() * 3); let mut tri_edge_ids = [0, 0, 0]; - for triangle in triangles { + for (triangle_id, triangle) in triangles.drain(..).enumerate() { + let triangle_id: u32 = triangle_id.try_into().unwrap(); + let segments = triangle.edges(); for i in 0..3 { let segment = segments[i]; @@ -91,13 +93,13 @@ impl PathFinder { } }; } - indexed_triangles.push(GraphTriangle::new(triangle, tri_edge_ids)); + indexed_triangles.push(GraphTriangle::new(triangle, triangle_id, tri_edge_ids)); for [edge_id, neighbour_a, neighbour_b] in [ [tri_edge_ids[0], tri_edge_ids[1], tri_edge_ids[2]], [tri_edge_ids[1], tri_edge_ids[2], tri_edge_ids[0]], [tri_edge_ids[2], tri_edge_ids[0], tri_edge_ids[1]], ] { - graph.add_neighbours(edge_id, neighbour_a, neighbour_b); + graph.add_neighbours(edge_id, triangle_id, neighbour_a, neighbour_b); } } @@ -157,13 +159,11 @@ impl PathFinder { return None; } - if source_edges - .iter() - .filter(|s| target_edges.contains(s)) - .take(2) - .count() - >= 2 - { + if source_edges.iter().any(|s| { + target_edges + .iter() + .any(|t| s.polygon_id() == t.polygon_id()) + }) { debug!( "Trivial path from {:?} to {:?} found, trimming...", from, to @@ -192,18 +192,24 @@ impl PathFinder { } } - fn locate_triangle_edges(&self, point: Point) -> Vec { - self.triangles - .locate_all_at_point(&[point.x, point.y]) - .flat_map(|t| t.neighbours(point)) - .collect() + fn locate_triangle_edges(&self, point: Point) -> Vec { + let mut result = Vec::new(); + for triangle in self.triangles.locate_all_at_point(&[point.x, point.y]) { + for edge_id in triangle.neighbours(point) { + result.push(Step::new(edge_id, triangle.triangle_id())) + } + } + result } - fn locate_exclusion_edges(&self, point: Point) -> Vec { + fn locate_exclusion_edges(&self, point: Point) -> Vec { self.exclusions .locate_all_at_point(&[point.x, point.y]) - .flat_map(|t| t.neighbours()) - .cloned() + .flat_map(|t| { + t.neighbours() + .iter() + .map(|&edge_id| Step::new(edge_id, u32::MAX)) + }) .collect() } } @@ -211,14 +217,23 @@ impl PathFinder { /// A triangle used for spatial indexing inside the edge visibility graph. struct GraphTriangle { triangle: Triangle, + triangle_id: u32, /// IDs of edges of the triangle. These correspond to edges AB, BC and CA /// respectively. edges: [u32; 3], } impl GraphTriangle { - fn new(triangle: Triangle, edges: [u32; 3]) -> Self { - Self { triangle, edges } + fn new(triangle: Triangle, triangle_id: u32, edges: [u32; 3]) -> Self { + Self { + triangle, + triangle_id, + edges, + } + } + + fn triangle_id(&self) -> u32 { + self.triangle_id } /// Returns (up to 3) IDs of the triangle edges excluding edges which @@ -355,6 +370,7 @@ mod tests { PathTarget::new(Vec2::new(450., 950.), PathQueryProps::exact(), false), ) .unwrap(); + assert_eq!( first_path.waypoints(), &[ @@ -410,9 +426,10 @@ mod tests { ), ) .unwrap(); + assert_eq!( forth_path.waypoints(), - &[Vec2::new(18.003086, -48.81555), Vec2::new(0.2, -950.),] + &[Vec2::new(18.003227, -48.80841), Vec2::new(0.2, -950.),] ); let fifth_path = finder @@ -440,30 +457,17 @@ mod tests { #[timeout(100)] fn test_unreachable() { let triangles = vec![ - Triangle::new( - Point::new(0., 0.), - Point::new(-1., 1.), - Point::new(-1., -1.), - ), - Triangle::new(Point::new(0., 0.), Point::new(1., 1.), Point::new(-1., 1.)), - Triangle::new(Point::new(0., 0.), Point::new(1., -1.), Point::new(1., 1.)), - Triangle::new( - Point::new(0., 0.), - Point::new(-1., -1.), - Point::new(1., -1.), - ), - Triangle::new( - Point::new(0., 30.), - Point::new(0., 20.), - Point::new(10., 20.), - ), + Triangle::new(Point::new(0., 0.), Point::new(1., 1.), Point::new(1., 0.)), + Triangle::new(Point::new(0., 0.), Point::new(0., 1.), Point::new(1., 1.)), + Triangle::new(Point::new(0., 2.), Point::new(1., 3.), Point::new(1., 2.)), + Triangle::new(Point::new(0., 2.), Point::new(0., 3.), Point::new(1., 3.)), ]; let finder = PathFinder::from_triangles(triangles, vec![]); assert!(finder .find_path( - Point::new(-0.5, 0.), - PathTarget::new(Vec2::new(2., 22.), PathQueryProps::exact(), false) + Point::new(0.5, 2.5), + PathTarget::new(Vec2::new(0.5, 0.5), PathQueryProps::exact(), false) ) .is_none()) } diff --git a/crates/pathing/src/funnel.rs b/crates/pathing/src/funnel.rs deleted file mode 100644 index 613518cb7..000000000 --- a/crates/pathing/src/funnel.rs +++ /dev/null @@ -1,312 +0,0 @@ -//! Implementation of funnel algorithm based on linked lists of points and -//! incremental funnel extension API. The API makes the algorithm usable & -//! efficient when used from graph traversal algorithms. - -use std::rc::Rc; - -use de_types::path::Path; -use parry2d::{math::Point, shape::Segment}; - -use crate::{chain::PointChain, geometry::Side}; - -/// The funnel consists of a tail and left & right bounds. -/// -/// The tail and bounds are represented by [`crate::chain::PointChain`], thus -/// the funnel can be cheaply cloned, expanded and used in graph traversal -/// algorithms. -/// -/// The left and right bounds are represented by a list of points (going -/// backwards from last funnel expansion to the tip of the tail) such that line -/// segments in between them represent gradually closing funnel. -/// -/// The tail is represented by a list of points where the funnel was already -/// "closed", id est area where space between bounds narrowed to or below 0. -/// -/// No operation on the funnel affects other funnels sharing parts of the point -/// lists with bounds and tail. -#[derive(Clone)] -pub(crate) struct Funnel { - tail: Rc, - left: Rc, - right: Rc, -} - -impl Funnel { - /// Creates a new funnel with a single point. - pub(crate) fn new(start: Point) -> Self { - Self { - tail: Rc::new(PointChain::first(start)), - left: Rc::new(PointChain::first(start)), - right: Rc::new(PointChain::first(start)), - } - } - - /// Creates a new funnel where `a` represents side bound on `side` and `b` - /// represents the opposing bound. - /// - /// # Panics - /// - /// Panics if `side` is not [`Side::Left`] or [`Side::Right`]. - fn from_sides(side: Side, tail: Rc, a: Rc, b: Rc) -> Self { - let (left, right) = match side { - Side::Left => (a, b), - Side::Right => (b, a), - _ => panic!("Only Left and Right sides are accepted, got: {side:?}"), - }; - Self { tail, left, right } - } - - pub(crate) fn tail(&self) -> &PointChain { - Rc::as_ref(&self.tail) - } - - pub(crate) fn left(&self) -> &PointChain { - Rc::as_ref(&self.left) - } - - pub(crate) fn right(&self) -> &PointChain { - Rc::as_ref(&self.right) - } - - /// Returns the full shortest path inside the funnel to point `by`. - pub fn closed(&self, by: Point) -> Path { - let closed = self - .extended_by_point(Side::Left, by) - .extended_by_point(Side::Right, by); - - let left_count = closed.left().iter().count(); - let right_count = closed.right().iter().count(); - debug_assert!(left_count <= 2); - debug_assert!(right_count <= 2); - - // due to rounding errors, tail might not contain the very last point - // (i.e. `by`). - if (left_count + right_count) == 2 { - closed.tail().to_path() - } else { - PointChain::extended(&closed.tail, by).to_path() - } - } - - /// Returns a new funnel which is an extension of `self` by a line segment. - /// - /// It is supposed that `by.a` is on the left side from the point of view - /// of the middle of the last expansion segment. - pub fn extended(&self, by: Segment) -> Self { - self.extended_by_point(Side::Left, by.a) - .extended_by_point(Side::Right, by.b) - } - - /// Returns a new funnel with a side bound of the funnel extended & - /// modified by a point. - /// - /// In the case that the point narrows down the funnel, the operation - /// results in the removal of an ending portion of the side bound. - /// - /// In case that the side bound gets narrowed down beyond the opposing side - /// bound, the "closed" portion of the funnel is moved to the tail. See - /// [`close`]. - fn extended_by_point(&self, side: Side, by: Point) -> Self { - let (chain, opposing) = self.sides(side); - - if chain.point() == by { - self.clone() - } else if chain.which_side(by).map(|s| s == side).unwrap_or(true) { - self.extended_side(side, by) - } else { - let first_to_remove = chain - .iter() - .take_while(|b| b.which_side(by).map(|s| s != side).unwrap_or(false)) - .last() - // At least one item was taken, otherwise previous if else - // would not be skipped. - .unwrap(); - let last_to_keep = first_to_remove.prev().unwrap(); - let chain = Rc::new(PointChain::extended(last_to_keep, by)); - - if last_to_keep.is_first() { - let (tail, chain, opposing) = - close(side, Rc::clone(&self.tail), chain, Rc::clone(opposing)); - Self::from_sides(side, tail, chain, opposing) - } else { - Self::from_sides(side, Rc::clone(&self.tail), chain, Rc::clone(opposing)) - } - } - } - - /// Returns a new funnel with a side bound extended by a point. The point - /// is simple appended on the tip of the side bound. - fn extended_side(&self, side: Side, by: Point) -> Self { - let (chain, opposing) = self.sides(side); - let extended = Rc::new(PointChain::extended(chain, by)); - Self::from_sides(side, Rc::clone(&self.tail), extended, Rc::clone(opposing)) - } - - fn sides(&self, side: Side) -> (&Rc, &Rc) { - match side { - Side::Left => (&self.left, &self.right), - Side::Right => (&self.right, &self.left), - _ => panic!("Only Left and Right sides are accepted, got: {side:?}"), - } - } -} - -/// Moves "closed" part of `opposing` to the tail. -/// -/// # Arguments -/// -/// * `side` - Side of `chain` bound. -/// -/// * `tail` - tail to be expanded by the closed points of `chain`. -/// -/// * `chain` - a side bound which "closes" `opposing`. -/// -/// * `opposing` - a side bound to be closed. -fn close( - side: Side, - tail: Rc, - chain: Rc, - opposing: Rc, -) -> (Rc, Rc, Rc) { - debug_assert!(side == Side::Left || side == Side::Right); - - let by = chain.point(); - let keep = match opposing - .iter() - .position(|b| b.which_side(by).map(|s| s != side).unwrap_or(false)) - { - Some(index) => index, - None => return (tail, chain, opposing), - }; - - let bounds: Vec> = opposing.iter().map(|b| b.point()).collect(); - - let mut tail = tail; - // First item in side chains is equal to tail point. It must be skipped - // here. - for &point in bounds[keep..].iter().rev() { - if tail.point() != point { - tail = Rc::new(PointChain::extended(&tail, point)); - } - } - - let mut chain = Rc::new(PointChain::first(tail.point())); - if chain.point() != by { - chain = Rc::new(PointChain::extended(&chain, by)); - } - - let mut opposing = Rc::new(PointChain::first(tail.point())); - for &point in bounds[..keep].iter().rev() { - opposing = Rc::new(PointChain::extended(&opposing, point)); - } - - (tail, chain, opposing) -} - -#[cfg(test)] -mod tests { - use approx::assert_abs_diff_eq; - use glam::Vec2; - - use super::*; - - #[test] - fn test_funnel() { - let point_a = Point::new(1., 1.); - let point_b = Point::new(2., 0.); - let point_c = Point::new(2., 2.); - let point_d = Point::new(3., 1.); - let point_e = Point::new(2.5, 3.); - let point_f = Point::new(1.1, 3.); - let point_g = Point::new(1.7, 4.); - let point_h = Point::new(2., 5.); - let point_j = Point::new(2.5, 4.5); - - let funnel = Funnel::new(point_a) - .extended(Segment::new(point_b, point_c)) - .extended(Segment::new(point_d, point_c)) - .extended(Segment::new(point_e, point_c)) - .extended(Segment::new(point_e, point_f)) - .extended(Segment::new(point_g, point_f)) - .extended(Segment::new(point_g, point_h)) - .extended(Segment::new(point_g, point_j)); - - let path: Vec> = funnel.tail().iter().map(|p| p.point()).collect(); - assert_abs_diff_eq!(funnel.tail().length(), 3.436, epsilon = 0.001); - assert_eq!(path, vec![point_g, point_c, point_a]); - } - - #[test] - fn test_close() { - // Funnel opening to the right from the point of view of point a. - let point_a = Point::new(1., 2.); - let opposing_a = Rc::new(PointChain::first(point_a)); - let point_b = Point::new(2., 3.); - let opposing_b = Rc::new(PointChain::extended(&opposing_a, point_b)); - let point_c = Point::new(3., 5.); - let opposing_c = Rc::new(PointChain::extended(&opposing_b, point_c)); - let point_d = Point::new(4., 8.); - let opposing_d = Rc::new(PointChain::extended(&opposing_c, point_d)); - - // Point is to the left from all part of the chain, should not close. - let (tail, chain, opposing) = close( - Side::Left, - Rc::new(PointChain::first(Point::new(1., 2.))), - Rc::new(PointChain::first(Point::new(5., -1.))), - Rc::clone(&opposing_d), - ); - assert_eq!(tail.to_path().waypoints(), &[Vec2::new(1., 2.)]); - assert_eq!(chain.to_path().waypoints(), &[Vec2::new(5., -1.)]); - assert_eq!( - opposing.to_path().waypoints(), - &[ - Vec2::new(4., 8.), - Vec2::new(3., 5.), - Vec2::new(2., 3.), - Vec2::new(1., 2.), - ] - ); - - // Point is to the right from all but last two points. - let (tail, chain, opposing) = close( - Side::Left, - Rc::new(PointChain::first(Point::new(1., 2.))), - Rc::new(PointChain::first(Point::new(5., 8.9))), - Rc::clone(&opposing_d), - ); - assert_eq!( - tail.to_path().waypoints(), - &[Vec2::new(2., 3.), Vec2::new(1., 2.)] - ); - assert_eq!( - chain.to_path().waypoints(), - &[Vec2::new(5., 8.9), Vec2::new(2., 3.)] - ); - assert_eq!( - opposing.to_path().waypoints(), - &[Vec2::new(4., 8.), Vec2::new(3., 5.), Vec2::new(2., 3.)] - ); - - // Point is to the right from all points. - let (tail, chain, opposing) = close( - Side::Left, - Rc::new(PointChain::first(Point::new(1., 2.))), - Rc::new(PointChain::first(Point::new(5., 13.))), - Rc::clone(&opposing_d), - ); - assert_eq!( - tail.to_path().waypoints(), - &[ - Vec2::new(4., 8.), - Vec2::new(3., 5.), - Vec2::new(2., 3.), - Vec2::new(1., 2.), - ] - ); - assert_eq!( - chain.to_path().waypoints(), - &[Vec2::new(5., 13.), Vec2::new(4., 8.)] - ); - assert_eq!(opposing.to_path().waypoints(), &[Vec2::new(4., 8.)]); - } -} diff --git a/crates/pathing/src/geometry.rs b/crates/pathing/src/geometry.rs index 35c9fe9bf..52d826711 100644 --- a/crates/pathing/src/geometry.rs +++ b/crates/pathing/src/geometry.rs @@ -1,20 +1,6 @@ //! Various low level geometrical operations. -use parry2d::{math::Point, shape::Segment}; - -/// Reorients the segment so that end point `a` appears on the left side of end -/// point `b` from the perspective of `eye`. -/// -/// # Panics -/// -/// May panic if `eye` coincides with end points of the segment. -pub(crate) fn orient(eye: Point, segment: Segment) -> Segment { - if which_side(eye, segment.a, segment.b) == Side::Left { - Segment::new(segment.b, segment.a) - } else { - segment - } -} +use parry2d::{math::Point, query::Ray, shape::Segment}; /// Returns the side at which point `new` appears relative to point `old` from /// the perspective of `eye`. @@ -24,10 +10,9 @@ pub(crate) fn orient(eye: Point, segment: Segment) -> Segment { /// /// # Panics /// -/// May panic if `eye` coincides with `old` or `new`. +/// May panic if `eye` coincides with `old`. pub(crate) fn which_side(eye: Point, old: Point, new: Point) -> Side { debug_assert!(Point::from(old - eye) != Point::origin()); - debug_assert!(Point::from(new - eye) != Point::origin()); let perp: f32 = (eye - old).perp(&(eye - new)); if perp < 0. { Side::Left @@ -45,33 +30,107 @@ pub(crate) enum Side { Right, } +/// Projection of a ray onto a line segment. +#[derive(Clone, Copy)] +pub(crate) struct RayProjection { + parameter: Option, + endpoint_a_side: SimpleSide, +} + +impl RayProjection { + pub(crate) fn calculate(ray: Ray, target: Segment) -> Self { + let segment_dir = target.scaled_direction(); + + let origin_diff = target.a - ray.origin; + let ray_perp_origin = ray.dir.perp(&origin_diff); + let ray_perp_dir = ray.dir.perp(&segment_dir); + let dir_perp_origin = segment_dir.perp(&origin_diff); + + // TODO constant + // This is true when the ray is parallel with the segment. + let is_parallel = ray_perp_dir.abs() < 0.0001; + // This is true when the ray points away from the line given by the + // segment. + let is_behind = dir_perp_origin * ray_perp_dir > 0.; + + let parameter = if is_parallel || is_behind { + None + } else { + let parameter = -ray_perp_origin / ray_perp_dir; + if (0. ..=1.).contains(¶meter) { + Some(parameter) + } else { + None + } + }; + + let endpoint_a_side = if ray_perp_origin < 0. { + SimpleSide::Left + } else if ray_perp_origin > 0. { + SimpleSide::Right + } else if ray.dir.perp(&(target.b - ray.origin)) > 0. { + // When ray goes through endpoint A (or directly away from it), we + // pretend that the endpoint lies at the opposite site than + // endpoint B so that the ray "crosses" the segment. + SimpleSide::Left + } else { + SimpleSide::Right + }; + + Self::new(parameter, endpoint_a_side) + } + + pub(crate) fn new(parameter: Option, endpoint_a_side: SimpleSide) -> Self { + #[cfg(debug_assertions)] + if let Some(parameter) = parameter { + assert!(parameter.is_finite()); + assert!(0. <= parameter); + assert!(parameter <= 1.); + } + Self { + parameter, + endpoint_a_side, + } + } + + /// A value between 0 and 1 (inclusive). The parameter is None if the ray + /// does not intersect the segment. + /// + /// The intersection point is given by `segment.a + (segment.b - segment.a) + /// * parameter`. + pub(crate) fn parameter(&self) -> Option { + self.parameter + } + + /// Side of endpoint a of the segment relative to the ray. + /// + /// In the case that endpoint lies on the line given by the ray, it is + /// assumed that endpoint a lies on the opposite site than endpoint b. + pub(crate) fn endpoint_a_side(&self) -> SimpleSide { + self.endpoint_a_side + } +} + +#[derive(Clone, Copy, PartialEq, Eq, Debug)] +pub(crate) enum SimpleSide { + Left, + Right, +} + +impl PartialEq for SimpleSide { + fn eq(&self, other: &Side) -> bool { + match self { + Self::Left => *other == Side::Left, + Self::Right => *other == Side::Right, + } + } +} + #[cfg(test)] mod tests { - use super::*; + use nalgebra::Vector2; - #[test] - fn test_orient() { - let eye = Point::new(-100., -200.); - let segment_a = Segment::new(Point::new(1., 2.), Point::new(3., 8.)); - let segment_b = Segment::new(Point::new(3., 8.), Point::new(1., 2.)); - - assert_eq!(orient(eye, segment_a), segment_a); - assert_eq!(orient(eye, segment_b), segment_a); - - assert_eq!( - orient( - Point::new(-450.0, -950.0), - Segment::new( - Point::new(18.612133, -18.612133), - Point::new(-500.0, -1000.) - ) - ), - Segment::new( - Point::new(18.612133, -18.612133), - Point::new(-500.0, -1000.), - ) - ); - } + use super::*; #[test] fn test_which_side() { @@ -92,4 +151,49 @@ mod tests { assert_eq!(which_side(eye, a, a), Side::Straight); assert_eq!(which_side(Point::origin(), a, 0.5 * a), Side::Straight); } + + #[test] + fn test_ray_projection() { + let segment = Segment::new(Point::new(3., 1.), Point::new(1., 3.)); + + let proj = + RayProjection::calculate(Ray::new(Point::origin(), Vector2::new(1., 1.)), segment); + assert_eq!(0.5, proj.parameter().unwrap()); + assert_eq!(proj.endpoint_a_side(), SimpleSide::Left); + + let proj = + RayProjection::calculate(Ray::new(Point::origin(), Vector2::new(2., 2.)), segment); + assert_eq!(0.5, proj.parameter().unwrap()); + assert_eq!(proj.endpoint_a_side(), SimpleSide::Left); + + let proj = + RayProjection::calculate(Ray::new(Point::new(2., 1.), Vector2::new(1., 0.)), segment); + assert_eq!(0., proj.parameter().unwrap()); + assert_eq!(proj.endpoint_a_side(), SimpleSide::Left); + + let proj = RayProjection::calculate( + Ray::new(Point::new(2., 1.), Vector2::new(1., -0.5)), + segment, + ); + assert!(proj.parameter().is_none()); + assert_eq!(proj.endpoint_a_side(), SimpleSide::Right); + + let proj = + RayProjection::calculate(Ray::new(Point::origin(), Vector2::new(1., -1.)), segment); + assert!(proj.parameter().is_none()); + + let proj = + RayProjection::calculate(Ray::new(Point::origin(), Vector2::new(-1., 1.)), segment); + assert!(proj.parameter().is_none()); + + let proj = + RayProjection::calculate(Ray::new(Point::origin(), Vector2::new(0., -3.)), segment); + assert!(proj.parameter().is_none()); + assert_eq!(proj.endpoint_a_side(), SimpleSide::Right); + + let proj = + RayProjection::calculate(Ray::new(Point::origin(), Vector2::new(-3., 0.)), segment); + assert!(proj.parameter().is_none()); + assert_eq!(proj.endpoint_a_side(), SimpleSide::Left); + } } diff --git a/crates/pathing/src/graph.rs b/crates/pathing/src/graph.rs index a690bf788..957edc213 100644 --- a/crates/pathing/src/graph.rs +++ b/crates/pathing/src/graph.rs @@ -1,8 +1,8 @@ //! This module contains implementation of a edge-based visibility graph used //! in shortest path search on the game map. -use parry2d::{math::Point, shape::Segment}; -use tinyvec::TinyVec; +use parry2d::shape::Segment; +use tinyvec::ArrayVec; /// Edge based visibility sub-graph. /// @@ -50,100 +50,134 @@ impl VisibilityGraph { /// * `segment` - line segment of the triangle edge. pub(crate) fn new_node(&mut self, segment: Segment) -> u32 { let id = self.nodes.len().try_into().unwrap(); - self.nodes.push(Node::new(EdgeGeometry::new(segment))); + self.nodes.push(Node::new(segment)); id } /// Add 2 neighbours to a graph node (triangle edge). /// + /// # Arguments + /// + /// * `edge_id` - ID of the edge whose neighbors are added. + /// + /// * `polygon_id` - ID of the traversed polygon (i.e. the polygon which + /// contains the source and target edges). + /// + /// * `neighbour_a_id` - edge ID of the a neighbor. + /// + /// * `neighbour_b_id` - edge ID of the a neighbor. + /// /// # Panics /// /// Panics if `edge_id` already stores more than two neighbours. pub(crate) fn add_neighbours( &mut self, edge_id: u32, + polygon_id: u32, neighbour_a_id: u32, neighbour_b_id: u32, ) { let index: usize = edge_id.try_into().unwrap(); let node = self.nodes.get_mut(index).unwrap(); - node.add_neighbour(neighbour_a_id); - node.add_neighbour(neighbour_b_id); + node.add_neighbour(Step::new(neighbour_a_id, polygon_id)); + node.add_neighbour(Step::new(neighbour_b_id, polygon_id)); } /// Returns a geometry of a graph node (triangle edge). - pub(crate) fn geometry(&self, edge_id: u32) -> &EdgeGeometry { + pub(crate) fn segment(&self, edge_id: u32) -> Segment { let index: usize = edge_id.try_into().unwrap(); - self.nodes[index].geometry() + self.nodes[index].segment() } /// Returns all neighbors of a graph node (triangle edge). - pub(crate) fn neighbours(&self, edge_id: u32) -> &[u32] { + pub(crate) fn neighbours(&self, edge_id: u32) -> &[Step] { let index: usize = edge_id.try_into().unwrap(); self.nodes[index].neighbours() } + + pub(crate) fn polygons(&self, edge_id: u32) -> &[u32] { + let index: usize = edge_id.try_into().unwrap(); + self.nodes[index].polygons() + } } /// A node in the visibility graph. struct Node { - geometry: EdgeGeometry, - /// Neighbor IDs. - neighbours: TinyVec<[u32; 4]>, + segment: Segment, + /// Graph steps to reach direct neighbors. + neighbours: ArrayVec<[Step; 4]>, + /// IDs of polygons which contain the edge (node). + polygons: ArrayVec<[u32; 2]>, } impl Node { - fn new(geometry: EdgeGeometry) -> Self { + fn new(segment: Segment) -> Self { Self { - geometry, - neighbours: TinyVec::new(), + segment, + neighbours: ArrayVec::new(), + polygons: ArrayVec::new(), } } - fn geometry(&self) -> &EdgeGeometry { - &self.geometry + fn segment(&self) -> Segment { + self.segment } - fn neighbours(&self) -> &[u32] { + fn neighbours(&self) -> &[Step] { self.neighbours.as_slice() } + fn polygons(&self) -> &[u32] { + self.polygons.as_slice() + } + /// Adds a neighbor to the node. /// /// Each node can store up to 4 neighbors. /// /// # Panics /// - /// Panics if the number of already stored neighbors is 4. - fn add_neighbour(&mut self, edge_id: u32) { - self.neighbours.push(edge_id); + /// * If the number of already stored neighbors is 4. + /// + /// * If the number of already stored polygons is 2. + fn add_neighbour(&mut self, step: Step) { + self.neighbours.push(step); + if !self.polygons.contains(&step.polygon_id()) { + self.polygons.push(step.polygon_id()); + } } } -pub(crate) struct EdgeGeometry { - segment: Segment, - /// Middle of `segment` cached for efficiency reasons. - midpoint: Point, +/// A step in the polygon edge neighbor graph. +#[derive(Clone, Copy, Default, Debug, PartialEq, Eq)] +pub(crate) struct Step { + edge_id: u32, + polygon_id: u32, } -impl EdgeGeometry { - fn new(segment: Segment) -> Self { +impl Step { + pub(crate) fn new(edge_id: u32, polygon_id: u32) -> Self { Self { - segment, - midpoint: segment.a.coords.lerp(&segment.b.coords, 0.5).into(), + edge_id, + polygon_id, } } - pub(crate) fn segment(&self) -> Segment { - self.segment + /// A target edge ID (reached from neighboring edge). + pub(crate) fn edge_id(&self) -> u32 { + self.edge_id } - pub(crate) fn midpoint(&self) -> Point { - self.midpoint + /// ID of the traversed polygon (to reach [`Self::edge_id()`]. + pub(crate) fn polygon_id(&self) -> u32 { + self.polygon_id } } #[cfg(test)] mod tests { + use parry2d::math::Point; + use super::*; #[test] @@ -153,12 +187,17 @@ mod tests { let edge_id_a = graph.new_node(Segment::new(Point::new(1., 2.), Point::new(2., 3.))); let edge_id_b = graph.new_node(Segment::new(Point::new(2., 3.), Point::new(5., 6.))); let edge_id_c = graph.new_node(Segment::new(Point::new(5., 6.), Point::new(1., 2.))); - graph.add_neighbours(edge_id_a, edge_id_b, edge_id_c); - graph.add_neighbours(edge_id_b, edge_id_c, edge_id_a); - - assert_eq!(graph.neighbours(edge_id_a), &[edge_id_b, edge_id_c]); - assert_eq!(graph.neighbours(edge_id_b), &[edge_id_c, edge_id_a]); - assert_eq!(graph.neighbours(edge_id_c), &[] as &[u32]); - assert_eq!(graph.geometry(edge_id_a).midpoint(), Point::new(1.5, 2.5)); + graph.add_neighbours(edge_id_a, 1, edge_id_b, edge_id_c); + graph.add_neighbours(edge_id_b, 1, edge_id_c, edge_id_a); + + assert_eq!( + graph.neighbours(edge_id_a), + &[Step::new(edge_id_b, 1), Step::new(edge_id_c, 1)] + ); + assert_eq!( + graph.neighbours(edge_id_b), + &[Step::new(edge_id_c, 1), Step::new(edge_id_a, 1)] + ); + assert_eq!(graph.neighbours(edge_id_c), &[] as &[Step]); } } diff --git a/crates/pathing/src/interval.rs b/crates/pathing/src/interval.rs new file mode 100644 index 000000000..bc1fe46bf --- /dev/null +++ b/crates/pathing/src/interval.rs @@ -0,0 +1,539 @@ +use parry2d::{ + math::Point, + query::{PointQuery, Ray, RayCast}, + shape::Segment, +}; + +use crate::geometry::{which_side, RayProjection, Side}; + +#[derive(Clone)] +pub(crate) struct SegmentInterval { + segment: Segment, + is_a_corner: bool, + is_b_corner: bool, + edge_id: u32, +} + +impl SegmentInterval { + /// Creates a new interval from projection parameters. + /// + /// # Arguments + /// + /// * `segment` - an original segment. Projection parameters correspond to + /// this segment. + /// + /// * `projection` - parameters of endpoints a and b of the interval to be + /// created. + /// + /// * `edge_id` - ID of the original edge / segment. + /// + /// # Panics + /// + /// May panic if projection parameters are not between 0 and 1 (inclusive) + /// or if first projection parameter is larger or equal to the second + /// projection parameter. + pub(crate) fn from_projection(segment: Segment, projection: ParamPair, edge_id: u32) -> Self { + Self::new( + projection.apply(segment), + projection.includes_corner_a(), + projection.includes_corner_b(), + edge_id, + ) + } + + /// Creates a new segment interval. + /// + /// # Panics + /// + /// May panic if `segment` has zero length. + pub(crate) fn new( + segment: Segment, + is_a_corner: bool, + is_b_corner: bool, + edge_id: u32, + ) -> Self { + // TODO this fails + debug_assert!(segment.length() > 0.); + Self { + segment, + is_a_corner, + is_b_corner, + edge_id, + } + } + + /// Returns the corner point of the original edge (see [`Self::edge_id()`]) + /// if it corresponds to the endpoint of `self`. + pub(crate) fn a_corner(&self) -> Option> { + if self.is_a_corner { + Some(self.segment.a) + } else { + None + } + } + + /// Returns the corner point of the original edge (see [`Self::edge_id()`]) + /// if it corresponds to the endpoint of `self`. + pub(crate) fn b_corner(&self) -> Option> { + if self.is_b_corner { + Some(self.segment.b) + } else { + None + } + } + + /// Returns edge ID of the original edge. + pub(crate) fn edge_id(&self) -> u32 { + self.edge_id + } + + pub(crate) fn distance_to_point(&self, point: Point) -> f32 { + self.segment.distance_to_local_point(&point, false) + } + + pub(crate) fn project_point(&self, point: Point) -> Point { + self.segment.project_local_point(&point, false).point + } + + /// Calculates the cross point of an optimal path from a point `a` to a + /// point `b` via the interval. + pub(crate) fn cross(&self, a: Point, b: Point) -> SegmentCross { + let ray = Ray::new(a, b - a); + let direct_cross = self + .segment + .cast_local_ray(&ray, 1., false) + .map(|param| ray.point_at(param)); + + match direct_cross { + Some(point) => SegmentCross::Direct(point), + None => { + let dist_a = (self.segment.a - a).magnitude() + (self.segment.a - b).magnitude(); + let dist_b = (self.segment.b - a).magnitude() + (self.segment.b - b).magnitude(); + + if dist_a <= dist_b { + SegmentCross::Corner(self.segment.a) + } else { + SegmentCross::Corner(self.segment.b) + } + } + } + } + + /// Returns projection of self onto a target segment from a given + /// perspective. + /// + /// # Arguments + /// + /// * `eye` - projection perspective. + /// + /// * `target` - self is projected onto this target. + pub(crate) fn project_onto_segment( + &self, + eye: Point, + target: Segment, + ) -> SegmentProjection { + let ray_a = self.ray_a(eye); + let ray_b = self.ray_b(eye); + debug_assert_eq!(ray_a.origin, ray_b.origin); + + let a = RayProjection::calculate(ray_a, target); + let b = RayProjection::calculate(ray_b, target); + + let side = which_side(ray_a.origin, ray_a.point_at(1.), ray_b.point_at(1.)); + debug_assert!(side != Side::Straight || ray_a.dir.dot(&ray_b.dir) < 0.); + SegmentProjection::new(a, b, side) + } + + /// Returns a ray with a given origin and pointing towards the endpoint a + /// of the interval. + /// + /// When `origin` corresponds to the endpoint a, the direction of the + /// returned ray will correspond to (a - b). + pub(crate) fn ray_a(&self, origin: Point) -> Ray { + Self::endpoint_ray(origin, self.segment.a, self.segment.b) + } + + /// See [`Self::ray_a()`]. + pub(crate) fn ray_b(&self, origin: Point) -> Ray { + Self::endpoint_ray(origin, self.segment.b, self.segment.a) + } + + fn endpoint_ray(origin: Point, endpoint: Point, other_endpoint: Point) -> Ray { + let eye = if origin == endpoint { + other_endpoint + } else { + origin + }; + Ray::new(origin, endpoint - eye) + } +} + +#[derive(Clone, Copy)] +pub(crate) enum SegmentCross { + /// The crossed line segment intersects with the line segment between the + /// points `a` and `b`. + Direct(Point), + /// The crossed line segment does not intersect with the line segment + /// between the points `a` and `b` and thus the optimal path traverses an + /// endpoint of the crossed line segment. + Corner(Point), +} + +impl SegmentCross { + /// Returns the crossing point. + pub(crate) fn point(&self) -> Point { + match self { + Self::Direct(point) => *point, + Self::Corner(point) => *point, + } + } +} + +// TODO improve the docs +/// The projection can be looked at as a shadow cast by `self` onto `target` +/// with the source of light placed at `eye`. +#[derive(Clone, Copy)] +pub(crate) struct SegmentProjection { + a: RayProjection, + b: RayProjection, + ray_b_side: Side, +} + +impl SegmentProjection { + fn new(a: RayProjection, b: RayProjection, ray_b_side: Side) -> Self { + Self { a, b, ray_b_side } + } + + // TODO document + pub(crate) fn side_a(&self) -> Option { + if self.ray_b_side == Side::Straight { + return None; + } + + let first = self.a.parameter().unwrap_or(1.); + let second = if self.a.endpoint_a_side() == self.ray_b_side { + 1. + } else { + 0. + }; + + ParamPair::ordered(first, second) + } + + // TODO document + pub(crate) fn side_b(&self) -> Option { + if self.ray_b_side == Side::Straight { + return None; + } + + let first = self.b.parameter().unwrap_or(1.); + let second = if self.b.endpoint_a_side() != self.ray_b_side { + 1. + } else { + 0. + }; + + ParamPair::ordered(first, second) + } + + // TODO document + pub(crate) fn middle(&self) -> Option { + if self.ray_b_side == Side::Straight { + return Some(ParamPair::new(0., 1.)); + } + + match (self.a.parameter(), self.b.parameter()) { + (Some(a), Some(b)) => ParamPair::ordered(a, b), + (None, None) => { + if self.a.endpoint_a_side() == self.b.endpoint_a_side() { + None + } else { + Some(ParamPair::new(0., 1.)) + } + } + (Some(first), None) | (None, Some(first)) => { + let second = if self.a.endpoint_a_side() == self.b.endpoint_a_side() { + 1. + } else { + 0. + }; + ParamPair::ordered(first, second) + } + } + } +} + +/// Parameters of a (sub-)segment of a line segment. +pub(crate) struct ParamPair(f32, f32); + +impl ParamPair { + fn round(parameter: f32) -> f32 { + // TODO use constants + + // Due to the nature of the algorithm, the ray and the segment + // frequently intersect near one of the endpoints. To avoid rounding issues, + if parameter < 0.0001 { + 0. + } else if parameter > 0.9999 { + 1. + } else { + parameter + } + } + + // TODO document + fn ordered(a: f32, b: f32) -> Option { + let a = Self::round(a); + let b = Self::round(b); + + if a < b { + Some(Self::new(a, b)) + } else if a > b { + Some(Self::new(b, a)) + } else { + None + } + } + + fn new(a: f32, b: f32) -> Self { + debug_assert!(0. <= a); + debug_assert!(a < b); + debug_assert!(b <= 1.); + Self(a, b) + } + + /// Apply the parameters on the parent line segment and return the + /// (sub-)segment. + fn apply(&self, segment: Segment) -> Segment { + debug_assert!(segment.length() > 0.); + let dir = segment.scaled_direction(); + Segment::new( + if self.0 == 0. { + // To avoid rounding errors around corners. + segment.a + } else { + segment.a + self.0 * dir + }, + if self.1 == 1. { + segment.b + } else { + segment.a + self.1 * dir + }, + ) + } + + /// Returns true if the first parameter coincides with endpoint a of the + /// parent line segment. + fn includes_corner_a(&self) -> bool { + self.0 == 0. + } + + /// Returns true if the first parameter coincides with endpoint b of the + /// parent line segment. + fn includes_corner_b(&self) -> bool { + self.1 == 1. + } +} + +#[cfg(test)] +mod tests { + use nalgebra::Vector2; + + use super::*; + use crate::geometry::SimpleSide; + + #[test] + fn test_from_projection() { + let interval = SegmentInterval::from_projection( + Segment::new(Point::new(2., 4.), Point::new(2., 0.)), + ParamPair::new(0.25, 0.5), + 3, + ); + assert_eq!(interval.segment.a, Point::new(2., 3.)); + assert_eq!(interval.segment.b, Point::new(2., 2.)); + assert_eq!(interval.a_corner(), None); + assert_eq!(interval.b_corner(), None); + assert_eq!(interval.edge_id(), 3); + + let interval = SegmentInterval::from_projection( + Segment::new(Point::new(2., 4.), Point::new(2., 0.)), + ParamPair::new(0., 1.), + 7, + ); + assert_eq!(interval.segment.a, Point::new(2., 4.)); + assert_eq!(interval.segment.b, Point::new(2., 0.)); + assert_eq!(interval.a_corner().unwrap(), Point::new(2., 4.)); + assert_eq!(interval.b_corner().unwrap(), Point::new(2., 0.)); + assert_eq!(interval.edge_id(), 7); + } + + #[test] + fn test_project_point() { + let interval = SegmentInterval::new( + Segment::new(Point::new(2., 4.), Point::new(2., 1.)), + true, + true, + 0, + ); + assert_eq!( + interval.project_point(Point::new(8., 2.)), + Point::new(2., 2.) + ); + assert_eq!( + interval.project_point(Point::new(8., 10.)), + Point::new(2., 4.) + ); + } + + #[test] + fn test_project_onto_segment() { + let interval = SegmentInterval::new( + Segment::new(Point::new(2., 4.), Point::new(2., 1.)), + true, + true, + 0, + ); + + let projection = interval.project_onto_segment( + Point::new(0., 4.), + Segment::new(Point::new(4., 2.), Point::new(4., 10.)), + ); + assert_eq!(projection.ray_b_side, Side::Left); + assert_eq!(projection.a.parameter().unwrap(), 0.25); + assert_eq!(projection.a.endpoint_a_side(), SimpleSide::Left); + assert!(projection.b.parameter().is_none()); + assert_eq!(projection.b.endpoint_a_side(), SimpleSide::Right); + + let projection = interval.project_onto_segment( + Point::new(0., 4.), + Segment::new(Point::new(4., 10.), Point::new(4., 2.)), + ); + assert_eq!(projection.a.parameter().unwrap(), 0.75); + assert_eq!(projection.a.endpoint_a_side(), SimpleSide::Right); + assert!(projection.b.parameter().is_none()); + assert_eq!(projection.a.endpoint_a_side(), SimpleSide::Right); + } + + #[test] + fn test_left_corner() { + let a = Point::new(-1., 1.); + let b = Point::new(1., 1.); + let c = Point::new(-3., 0.); + let eye = Point::new(-2., 2.); + + let target = Segment::new(b, c); + + let parameters = [ + ((a, b), (a, c)), + ((b, a), (a, c)), + ((a, b), (c, a)), + ((b, a), (c, a)), + ]; + for ((aa, ab), (ba, bb)) in parameters { + let interval = SegmentInterval::new(Segment::new(aa, ab), true, true, 0); + let proj = interval.project_onto_segment(eye, Segment::new(ba, bb)); + assert!(proj.middle().is_none()); + + let pair = match (proj.side_a(), proj.side_b()) { + (Some(pair), None) => pair, + (None, Some(pair)) => pair, + _ => unreachable!("The segment fully behind one corner."), + }; + + assert!(pair.includes_corner_a()); + assert!(pair.includes_corner_b()); + + let result = pair.apply(target); + assert!(result.a == b || result.b == b); + assert!(result.a == c || result.b == c); + } + } + + #[test] + fn test_right_corner() { + let a = Point::new(-1., 1.); + let b = Point::new(1., 1.); + let c = Point::new(3., 0.); + let eye = Point::new(2., 2.); + + let target = Segment::new(b, c); + + let parameters = [ + ((a, b), (b, c)), + ((b, a), (b, c)), + ((a, b), (c, b)), + ((b, a), (c, b)), + ]; + for ((aa, ab), (ba, bb)) in parameters { + let interval = SegmentInterval::new(Segment::new(aa, ab), true, true, 0); + let proj = interval.project_onto_segment(eye, Segment::new(ba, bb)); + assert!(proj.middle().is_none()); + + let pair = match (proj.side_a(), proj.side_b()) { + (Some(pair), None) => pair, + (None, Some(pair)) => pair, + _ => unreachable!("The segment fully behind one corner."), + }; + + assert!(pair.includes_corner_a()); + assert!(pair.includes_corner_b()); + + let result = pair.apply(target); + assert!(result.a == b || result.b == b); + assert!(result.a == c || result.b == c); + } + } + + #[test] + fn test_eye_on_endpoint() { + let a = Point::new(-1., 1.); + let b = Point::new(1., 1.); + let c = Point::new(3., 0.); + let eye = b; + + let target = Segment::new(b, c); + + let parameters = [ + ((a, b), (b, c)), + ((b, a), (b, c)), + ((a, b), (c, b)), + ((b, a), (c, b)), + ]; + for ((aa, ab), (ba, bb)) in parameters { + let interval = SegmentInterval::new(Segment::new(aa, ab), true, true, 0); + let proj = interval.project_onto_segment(eye, Segment::new(ba, bb)); + assert!(proj.side_a().is_none()); + assert!(proj.side_b().is_none()); + + let pair = proj.middle().unwrap(); + assert!(pair.includes_corner_a()); + assert!(pair.includes_corner_b()); + + let result = pair.apply(target); + assert!(result.a == b || result.b == b); + assert!(result.a == c || result.b == c); + } + } + + #[test] + fn test_ray() { + let interval = SegmentInterval::new( + Segment::new(Point::new(2., 4.), Point::new(2., 1.)), + true, + true, + 0, + ); + + let ray = interval.ray_a(Point::new(0.5, 2.5)); + assert_eq!(ray.origin, Point::new(0.5, 2.5)); + assert_eq!(ray.dir, Vector2::new(1.5, 1.5)); + + let ray = interval.ray_b(Point::new(0.5, 2.5)); + assert_eq!(ray.origin, Point::new(0.5, 2.5)); + assert_eq!(ray.dir, Vector2::new(1.5, -1.5)); + + let ray = interval.ray_a(Point::new(2., 4.)); + assert_eq!(ray.origin, Point::new(2., 4.)); + assert_eq!(ray.dir, Vector2::new(0., 3.)); + } +} diff --git a/crates/pathing/src/lib.rs b/crates/pathing/src/lib.rs index 7a03814af..4154c4a9d 100644 --- a/crates/pathing/src/lib.rs +++ b/crates/pathing/src/lib.rs @@ -3,14 +3,15 @@ //! game map. mod chain; -mod dijkstra; mod exclusion; mod finder; mod fplugin; -mod funnel; mod geometry; mod graph; +mod interval; +mod node; mod path; +mod polyanya; mod pplugin; mod query; mod syncing; diff --git a/crates/pathing/src/node.rs b/crates/pathing/src/node.rs new file mode 100644 index 000000000..877fff148 --- /dev/null +++ b/crates/pathing/src/node.rs @@ -0,0 +1,219 @@ +use std::{cmp::Ordering, rc::Rc}; + +use de_types::path::Path; +use parry2d::{math::Point, shape::Segment}; + +use crate::{ + chain::PointChain, + graph::Step, + interval::{ParamPair, SegmentCross, SegmentInterval}, +}; + +/// Polyanya search node. +/// +/// The node consists of a path prefix (whose last point is root point of the +/// node), an interval (a segment or the target point) and search heuristic. +#[derive(Clone)] +pub(super) struct Node { + prefix: Rc, + interval: Interval, + polygon_id: u32, + min_distance: f32, + /// Lower bound of the path length from the root via the interval the + /// target. + heuristic: f32, +} + +impl Node { + /// Creates an initial node, i.e. a node whose prefix consists of a single + /// point: `source`. + /// + /// # Arguments + /// + /// * `source` - starting point. + /// + /// * `target` - path finding target point. + /// + /// * `segment` - first segment to be traversed. + /// + /// * `step` - first point-to-edge step in the polygon edge neighboring + /// graph. + pub(super) fn initial( + source: Point, + target: Point, + segment: Segment, + step: Step, + ) -> Self { + Self::from_segment_interval( + Rc::new(PointChain::first(source)), + SegmentInterval::new(segment, true, true, step.edge_id()), + step.polygon_id(), + target, + ) + } + + /// Creates a new Polyanya node from a path prefix and an interval. Node + /// heuristic is computed. + fn from_segment_interval( + prefix: Rc, + interval: SegmentInterval, + polygon_id: u32, + target: Point, + ) -> Self { + let cross = interval.cross(prefix.point(), target).point(); + let heuristic = (cross - prefix.point()).magnitude() + (target - cross).magnitude(); + let min_distance = interval.distance_to_point(target); + + Self { + prefix, + interval: Interval::Segment(interval), + polygon_id, + min_distance, + heuristic, + } + } + + pub(super) fn root(&self) -> Point { + self.prefix.point() + } + + pub(super) fn edge_id(&self) -> Option { + match self.interval { + Interval::Target => None, + Interval::Segment(ref interval) => Some(interval.edge_id()), + } + } + + pub(crate) fn polygon_id(&self) -> u32 { + self.polygon_id + } + + /// Returns distance of the node's interval and the target point. + pub(super) fn min_distance(&self) -> f32 { + self.min_distance + } + + pub(super) fn expand_to_edge( + &self, + segment: Segment, + step: Step, + target: Point, + ) -> [Option; 3] { + let Interval::Segment(ref interval) = self.interval else { + panic!("Cannot expand point interval.") + }; + + let projection = interval.project_onto_segment(self.prefix.point(), segment); + + let node_a = if let Some(a_corner) = interval.a_corner() { + projection + .side_a() + .map(|projection| self.corner(step, segment, a_corner, projection, target)) + } else { + None + }; + + let node_mid = if let Some(projection) = projection.middle() { + let interval = SegmentInterval::from_projection(segment, projection, step.edge_id()); + Some(Node::from_segment_interval( + Rc::clone(&self.prefix), + interval, + step.polygon_id(), + target, + )) + } else { + None + }; + + let node_b = if let Some(b_corner) = interval.b_corner() { + projection + .side_b() + .map(|projection| self.corner(step, segment, b_corner, projection, target)) + } else { + None + }; + + [node_a, node_mid, node_b] + } + + fn corner( + &self, + step: Step, + segment: Segment, + corner: Point, + projection: ParamPair, + target: Point, + ) -> Node { + let interval = SegmentInterval::from_projection(segment, projection, step.edge_id()); + let prefix = if self.root() == corner { + Rc::clone(&self.prefix) + } else { + Rc::new(PointChain::extended(&self.prefix, corner)) + }; + + Node::from_segment_interval(prefix, interval, step.polygon_id(), target) + } + + pub(super) fn expand_to_target(&self, target: Point, polygon_id: u32) -> Option { + let Interval::Segment(ref interval) = self.interval else { + panic!("Cannot expand point interval.") + }; + + let prefix = match interval.cross(self.root(), target) { + SegmentCross::Corner(point) => Rc::new(PointChain::extended(&self.prefix, point)), + _ => Rc::clone(&self.prefix), + }; + let heuristic = (target - prefix.point()).magnitude(); + Some(Self { + prefix, + interval: Interval::Target, + polygon_id, + min_distance: 0., + heuristic, + }) + } + + pub(super) fn close(self, target: Point) -> Path { + let chain = match self.interval { + Interval::Target => PointChain::extended(&self.prefix, target), + Interval::Segment(ref interval) => { + PointChain::extended(&self.prefix, interval.project_point(target)) + } + }; + chain.to_path() + } + + pub(super) fn root_score(&self) -> f32 { + self.prefix.length() + } + + fn score(&self) -> f32 { + self.root_score() + self.heuristic + } +} + +impl PartialEq for Node { + fn eq(&self, other: &Node) -> bool { + self.score() == other.score() && self.prefix.point() == other.prefix.point() + } +} + +impl Eq for Node {} + +impl PartialOrd for Node { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} + +impl Ord for Node { + fn cmp(&self, other: &Self) -> Ordering { + other.score().partial_cmp(&self.score()).unwrap() + } +} + +#[derive(Clone)] +enum Interval { + Target, + Segment(SegmentInterval), +} diff --git a/crates/pathing/src/polyanya.rs b/crates/pathing/src/polyanya.rs new file mode 100644 index 000000000..a88be2eaf --- /dev/null +++ b/crates/pathing/src/polyanya.rs @@ -0,0 +1,176 @@ +//! This module contains visibility graph based path finding algorithm. + +use std::collections::BinaryHeap; + +use ahash::AHashMap; +use bevy::utils::FloatOrd; +use de_types::path::Path; +use parry2d::math::Point; + +use crate::{ + graph::{Step, VisibilityGraph}, + node::Node, + PathQueryProps, +}; + +/// Finds and returns a reasonable path between two points. +/// +/// Source and target points must not lie inside or on the edge of the same +/// triangle of the triangulation from which `graph` was created. +pub(crate) fn find_path( + graph: &VisibilityGraph, + source: PointContext, + target: PointContext, + properties: PathQueryProps, +) -> Option { + let mut open_set = BinaryHeap::new(); + let mut visited = Visited::new(); + + for &step in source.neighbours() { + open_set.push(Node::initial( + source.point(), + target.point(), + graph.segment(step.edge_id()), + step, + )); + } + + let Some(mut best) = open_set.peek().cloned() else { + return None; + }; + + let mut counter = 0; + while let Some(node) = open_set.pop() { + counter += 1; + if counter > 1_000_000 { + // TODO use a constant a better message + panic!("Path finding took too long."); + } + if open_set.len() > 1_000_000 { + // TODO constant and a better message + panic!("Too many opened nodes."); + } + + let Some(edge_id) = node.edge_id() else { + best = node.clone(); + break; + }; + + let improved = visited.test_push(node.root(), node.root_score()); + if !improved { + continue; + } + + if best.min_distance() > node.min_distance() { + best = node.clone(); + } + + if let Some(target_step) = target + .neighbours() + .iter() + .find(|step| step.edge_id() == edge_id) + { + if let Some(expansion) = node.expand_to_target(target.point(), target_step.polygon_id()) + { + open_set.push(expansion); + } + continue; + } + + for &step in graph.neighbours(edge_id) { + let next_polygons = graph.polygons(step.edge_id()); + if next_polygons.contains(&node.polygon_id()) { + // Allow only path forward (not backward through the just + // traversed polygon). + continue; + } + + let next_segment = graph.segment(step.edge_id()); + for expansion in node + .expand_to_edge(next_segment, step, target.point()) + .into_iter() + .flatten() + { + open_set.push(expansion); + } + } + } + + let path = best.close(target.point()); + let dist_to_target = path.waypoints()[0].distance(target.point().into()); + if dist_to_target > properties.max_distance() { + None + } else if dist_to_target < properties.distance() { + path.truncated(properties.distance() - dist_to_target) + } else { + Some(path) + } +} + +pub(crate) struct PointContext { + point: Point, + neighbours: Vec, +} + +impl PointContext { + /// Creates a new point context. + /// + /// # Arguments + /// + /// * `point` - position of the point in the map + /// + /// * `neighbours` - steps to all neighboring edges. If the point lies + /// on an edge or its end points, the edge should not be included in the + /// vector. + pub(crate) fn new(point: Point, neighbours: Vec) -> Self { + Self { point, neighbours } + } + + fn point(&self) -> Point { + self.point + } + + fn neighbours(&self) -> &[Step] { + self.neighbours.as_slice() + } +} + +struct Visited(AHashMap<(FloatOrd, FloatOrd), f32>); + +impl Visited { + fn new() -> Self { + Self(AHashMap::new()) + } + + /// Marks a point as visited and stores/updates its associated score. + /// Returns true when the point was not yet visited or the previous score + /// was grater or equal to the new score. + fn test_push(&mut self, point: Point, score: f32) -> bool { + let key = (FloatOrd(point.x), FloatOrd(point.y)); + let current_score = self.0.get(&key).cloned().unwrap_or(f32::INFINITY); + if current_score > score { + self.0.insert(key, score); + true + } else { + current_score == score + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_visited() { + let mut visited = Visited::new(); + + assert!(visited.test_push(Point::new(1., 2.), 8.)); + assert!(visited.test_push(Point::new(1., 2.), 7.)); + assert!(!visited.test_push(Point::new(1., 2.), 7.5)); + + assert!(visited.test_push(Point::new(3., 2.), 11.)); + assert!(!visited.test_push(Point::new(3., 2.), 12.)); + assert!(visited.test_push(Point::new(3., 2.), 7.)); + } +}