diff --git a/crates/pathing/src/exclusion.rs b/crates/pathing/src/exclusion.rs new file mode 100644 index 000000000..5ed489ea9 --- /dev/null +++ b/crates/pathing/src/exclusion.rs @@ -0,0 +1,110 @@ +use bevy::prelude::GlobalTransform; +use de_index::Ichnography; +use glam::EulerRot; +use parry2d::{ + math::{Isometry, Point, Vector}, + query, + shape::ConvexPolygon, +}; +use rstar::{Envelope, RTree, RTreeObject, SelectionFunction, AABB}; + +const EXCLUSION_OFFSET: f32 = 2.; + +#[derive(Clone)] +pub(crate) struct ExclusionArea { + polygon: ConvexPolygon, +} + +impl ExclusionArea { + pub(crate) fn build<'a, T: Iterator>( + ichnographies: T, + ) -> Vec { + let mut exclusions: Vec = ichnographies + .map(|(transform, ichnography)| Self::from_ichnography(transform, ichnography)) + .collect(); + + let mut rtree = RTree::new(); + for exclusion in exclusions.drain(..) { + let mut to_merge: Vec = + rtree.drain_with_selection_function(&exclusion).collect(); + + if to_merge.is_empty() { + rtree.insert(exclusion); + } else { + to_merge.push(exclusion); + rtree.insert(ExclusionArea::merged(&to_merge)); + } + } + + // TODO: avoid cloning here + rtree.iter().cloned().collect() + } + + fn from_ichnography(transform: &GlobalTransform, ichnography: &Ichnography) -> Self { + let angle = transform.rotation.to_euler(EulerRot::YXZ).0; + let translation = Vector::new(transform.translation.x, transform.translation.z); + let isometry = Isometry::new(translation, angle); + let vertices: Vec> = ichnography + .bounds() + .points() + .iter() + .map(|&p| isometry * p) + .collect(); + + Self::new( + ConvexPolygon::from_convex_polyline(vertices) + .unwrap() + .offseted(EXCLUSION_OFFSET), + ) + } + + pub(crate) fn new(polygon: ConvexPolygon) -> Self { + Self { polygon } + } + + fn merged(exclusions: &[ExclusionArea]) -> Self { + let points: Vec> = exclusions + .iter() + .flat_map(|e| e.points()) + .cloned() + .collect(); + Self { + polygon: ConvexPolygon::from_convex_hull(&points).unwrap(), + } + } + + pub(crate) fn points(&self) -> &[Point] { + self.polygon.points() + } +} + +impl RTreeObject for ExclusionArea { + type Envelope = AABB<[f32; 2]>; + + fn envelope(&self) -> Self::Envelope { + let aabb = self.polygon.local_aabb(); + AABB::from_corners([aabb.mins.x, aabb.mins.y], [aabb.maxs.x, aabb.maxs.y]) + } +} + +impl SelectionFunction for &ExclusionArea { + fn should_unpack_parent(&self, envelope: &AABB<[f32; 2]>) -> bool { + self.envelope().intersects(envelope) + } + + fn should_unpack_leaf(&self, leaf: &ExclusionArea) -> bool { + query::intersection_test( + &Isometry::identity(), + &self.polygon, + &Isometry::identity(), + &leaf.polygon, + ) + .unwrap() + } +} + +impl PartialEq for ExclusionArea { + fn eq(&self, other: &Self) -> bool { + self.points() == other.points() + } +} diff --git a/crates/pathing/src/lib.rs b/crates/pathing/src/lib.rs index d7f8091f0..8d8961506 100644 --- a/crates/pathing/src/lib.rs +++ b/crates/pathing/src/lib.rs @@ -1,11 +1,19 @@ mod astar; mod chain; +mod exclusion; mod finder; mod funnel; mod geometry; mod graph; mod path; +mod systems; +mod triangulation; mod utils; +// TODO add documentation of the whole path finding algorithm +// TODO remove all unnecessary dependencies +// TODO make all triangles right handed + pub use finder::PathFinder; pub use path::Path; +pub use systems::{PathFinderUpdated, PathingPlugin}; diff --git a/crates/pathing/src/systems.rs b/crates/pathing/src/systems.rs new file mode 100644 index 000000000..335bc2dd9 --- /dev/null +++ b/crates/pathing/src/systems.rs @@ -0,0 +1,84 @@ +use bevy::{prelude::*, transform::TransformSystem}; +use de_core::{objects::StaticSolid, state::GameState}; +use de_index::Ichnography; +use de_map::size::MapBounds; +use iyes_loopless::prelude::*; + +use crate::{exclusion::ExclusionArea, triangulation::triangulate, PathFinder}; + +pub struct PathingPlugin; + +impl Plugin for PathingPlugin { + fn build(&self, app: &mut App) { + app.add_event::() + .add_event::() + .add_enter_system(GameState::Playing, setup) + .add_system_to_stage( + CoreStage::PostUpdate, + removed.run_in_state(GameState::Playing), + ) + .add_system_to_stage( + CoreStage::PostUpdate, + updated + .run_in_state(GameState::Playing) + .label("updated") + .after(TransformSystem::TransformPropagate), + ) + .add_system_to_stage( + CoreStage::PostUpdate, + update.run_in_state(GameState::Playing).after("updated"), + ); + } +} + +struct ChangeEvent; + +pub struct PathFinderUpdated; + +fn setup(mut commands: Commands, bounds: Res, mut events: EventWriter) { + commands.insert_resource(PathFinder::new(bounds.as_ref())); + events.send(ChangeEvent); +} + +fn removed(mut events: EventWriter, removed: RemovedComponents) { + if removed.iter().next().is_some() { + events.send(ChangeEvent); + } +} + +fn updated( + mut events: EventWriter, + changed: Query< + Entity, + ( + With, + Or<(Changed, Changed)>, + ), + >, +) { + if changed.iter().next().is_some() { + events.send(ChangeEvent); + } +} + +fn update( + mut changes: EventReader, + mut pf_updated: EventWriter, + bounds: Res, + mut finder: ResMut, + entities: Query<(&GlobalTransform, &Ichnography), With>, +) { + // It is desirable to consume all events during the check. + if changes.iter().count() == 0 { + return; + } + + let exclusions = ExclusionArea::build(entities.iter()); + + // TODO make sure that this does not take more than a few ms even on a + // large set of elements on the map, move it to a separate thread otherwise + let triangles = triangulate(bounds.as_ref(), &exclusions); + finder.update(triangles); + + pf_updated.send(PathFinderUpdated); +} diff --git a/crates/pathing/src/triangulation.rs b/crates/pathing/src/triangulation.rs new file mode 100644 index 000000000..62540f485 --- /dev/null +++ b/crates/pathing/src/triangulation.rs @@ -0,0 +1,316 @@ +use ahash::AHashMap; +use bevy::core::FloatOrd; +use de_map::size::MapBounds; +use parry2d::{math::Point, shape::Triangle}; +use spade::{ConstrainedDelaunayTriangulation, Point2, Triangulation}; + +use crate::exclusion::ExclusionArea; + +/// Returns a triangulation of rectangular area given by `bounds` with +/// exclusion zones. +/// +/// The returned triangles: +/// +/// * do not intersect each other, +/// * cover the rectangle given by `bounds` except areas in `exclusions`, +/// * do not intersect any exclusion area given by `exclusions`. +/// +/// # Arguments: +/// +/// * `bounds` - area to be triangulated. +/// +/// * `exclusions` - TODO +pub(crate) fn triangulate(bounds: &MapBounds, exclusions: &[ExclusionArea]) -> Vec { + let mut triangulation = ConstrainedDelaunayTriangulation::>::new(); + let aabb = bounds.aabb(); + triangulation + .insert(Point2::new(aabb.mins.x, aabb.mins.y)) + .unwrap(); + triangulation + .insert(Point2::new(aabb.mins.x, aabb.maxs.y)) + .unwrap(); + triangulation + .insert(Point2::new(aabb.maxs.x, aabb.maxs.y)) + .unwrap(); + triangulation + .insert(Point2::new(aabb.maxs.x, aabb.mins.y)) + .unwrap(); + + let mut polygon_ids = VertexPolygons::new(); + for edge in AreaEdges::new(exclusions) { + polygon_ids.add(edge.a(), edge.polygon_id()); + triangulation + .add_constraint_edge(edge.a_point2(), edge.b_point2()) + .unwrap(); + } + + triangulation + .inner_faces() + .filter_map(|f| { + let vertices = f.vertices().map(|v| { + let v = v.as_ref(); + Point::new(v.x, v.y) + }); + let triangle = Triangle::new(vertices[0], vertices[1], vertices[2]); + if polygon_ids.is_excluded(&triangle) { + None + } else { + Some(triangle) + } + }) + .collect() +} + +/// This struct holds a mapping from vertices to polygon IDs. +struct VertexPolygons { + mapping: AHashMap<(FloatOrd, FloatOrd), usize>, +} + +impl VertexPolygons { + fn new() -> VertexPolygons { + Self { + mapping: AHashMap::new(), + } + } + + fn point_to_key(point: Point) -> (FloatOrd, FloatOrd) { + (FloatOrd(point.x), FloatOrd(point.y)) + } + + fn add(&mut self, point: Point, polygon_id: usize) { + let old = self.mapping.insert(Self::point_to_key(point), polygon_id); + debug_assert!(old.is_none()); + } + + /// Returns true if the triangle is contained in an exclusion area. + fn is_excluded(&self, triangle: &Triangle) -> bool { + // We are using these facts in the following test: + // * all exclusion areas are convex + // * no exclusion areas overlap + // + // Knowing the above, it can be shown that a triangle is inside an + // exclusion area iff all its vertices are part of the same exclusion + // area polygon. + + let vertices = triangle + .vertices() + .map(|p| self.mapping.get(&Self::point_to_key(p))); + vertices[0].is_some() && vertices[0] == vertices[1] && vertices[1] == vertices[2] + } +} + +struct AreaEdges<'a> { + exclusions: &'a [ExclusionArea], + index: usize, + current: Option>, +} + +impl<'a> AreaEdges<'a> { + fn new(exclusions: &'a [ExclusionArea]) -> Self { + Self { + exclusions, + index: 0, + current: None, + } + } +} + +impl<'a> Iterator for AreaEdges<'a> { + type Item = ExclusionEdge; + + fn next(&mut self) -> Option { + match self.current.as_mut().and_then(|c| c.next()) { + Some(edge) => Some(edge), + None => match self.exclusions.get(self.index) { + Some(exclusion) => { + self.current = Some(PolygonEdges::new(exclusion, self.index)); + self.index += 1; + self.current.as_mut().unwrap().next() + } + None => None, + }, + } + } +} + +struct PolygonEdges<'a> { + polygon: &'a ExclusionArea, + polygon_id: usize, + index: usize, +} + +impl<'a> PolygonEdges<'a> { + fn new(polygon: &'a ExclusionArea, polygon_id: usize) -> Self { + Self { + polygon, + polygon_id, + index: 0, + } + } +} + +impl<'a> Iterator for PolygonEdges<'a> { + type Item = ExclusionEdge; + + fn next(&mut self) -> Option { + let points = self.polygon.points(); + if self.index >= points.len() { + return None; + } + + let a = points[self.index]; + self.index += 1; + let b = points[self.index.rem_euclid(points.len())]; + Some(ExclusionEdge::new(self.polygon_id, a, b)) + } +} + +/// Edge of a polygon of an exclusion area. +struct ExclusionEdge { + /// ID of the polygon this edge belongs to. + polygon_id: usize, + a: Point, + b: Point, +} + +impl ExclusionEdge { + fn new(polygon_id: usize, a: Point, b: Point) -> Self { + Self { polygon_id, a, b } + } + + fn polygon_id(&self) -> usize { + self.polygon_id + } + + fn a(&self) -> Point { + self.a + } + + fn a_point2(&self) -> Point2 { + Point2::new(self.a.x, self.b.y) + } + + fn b_point2(&self) -> Point2 { + Point2::new(self.b.x, self.b.y) + } +} + +#[cfg(test)] +mod tests { + use std::hash::Hash; + + use ahash::AHashSet; + use glam::Vec2; + use parry2d::shape::ConvexPolygon; + + use super::*; + use crate::utils::HashableSegment; + + #[test] + fn test_triangulation_empty() { + let obstacles = Vec::new(); + // <- 2.5 to left, <- 4.5 upwards + let triangles = triangulate(&MapBounds::new(Vec2::new(19., 13.)), &obstacles); + assert_eq!(triangles.len(), 2); + + let a = triangles[0]; + let b = triangles[1]; + assert_eq!( + a, + Triangle::new( + Point::new(9.5, 6.5), + Point::new(-9.5, 6.5), + Point::new(-9.5, -6.5), + ) + ); + assert_eq!( + b, + Triangle::new( + Point::new(9.5, -6.5), + Point::new(9.5, 6.5), + Point::new(-9.5, -6.5), + ) + ); + } + + #[test] + fn test_triangulation() { + let obstacles = vec![ExclusionArea::new( + ConvexPolygon::from_convex_polyline(vec![ + Point::new(-0.1, 1.1), + Point::new(-0.1, 1.3), + Point::new(1.0, 1.3), + Point::new(1.0, 1.1), + ]) + .unwrap(), + )]; + + // <- 2.5 to left, <- 4.5 upwards + let triangles: AHashSet = + triangulate(&MapBounds::new(Vec2::new(19., 13.)), &obstacles) + .iter() + .map(HashableTriangle::new) + .collect(); + let expected: AHashSet = vec![ + Triangle::new( + Point::new(-0.1, 1.1), + Point::new(-9.5, 6.5), + Point::new(-9.5, -6.5), + ), + Triangle::new( + Point::new(-9.5, -6.5), + Point::new(9.5, -6.5), + Point::new(-0.1, 1.1), + ), + Triangle::new( + Point::new(1.0, 1.1), + Point::new(-0.1, 1.1), + Point::new(9.5, -6.5), + ), + Triangle::new( + Point::new(-0.1, 1.3), + Point::new(9.5, 6.5), + Point::new(-9.5, 6.5), + ), + Triangle::new( + Point::new(-0.1, 1.3), + Point::new(-9.5, 6.5), + Point::new(-0.1, 1.1), + ), + Triangle::new( + Point::new(9.5, 6.5), + Point::new(-0.1, 1.3), + Point::new(1.0, 1.3), + ), + Triangle::new( + Point::new(9.5, -6.5), + Point::new(9.5, 6.5), + Point::new(1.0, 1.1), + ), + Triangle::new( + Point::new(1.0, 1.3), + Point::new(1.0, 1.1), + Point::new(9.5, 6.5), + ), + ] + .iter() + .map(HashableTriangle::new) + .collect(); + + assert_eq!(triangles, expected); + } + + #[derive(Clone, Copy, PartialEq, Eq, Hash, Debug)] + struct HashableTriangle(HashableSegment, HashableSegment, HashableSegment); + + impl HashableTriangle { + fn new(triangle: &Triangle) -> Self { + let edges = triangle.edges(); + HashableTriangle( + HashableSegment::new(edges[0]), + HashableSegment::new(edges[1]), + HashableSegment::new(edges[2]), + ) + } + } +} diff --git a/crates/pathing/src/utils.rs b/crates/pathing/src/utils.rs new file mode 100644 index 000000000..6da6a36c2 --- /dev/null +++ b/crates/pathing/src/utils.rs @@ -0,0 +1,59 @@ +use std::hash::Hash; + +use bevy::core::FloatOrd; +use parry2d::shape::Segment; + +/// Line segment whose hash and equivalence class don't change with +/// orientation. +#[derive(Clone, Copy, PartialEq, Eq, Hash, Debug)] +pub(crate) struct HashableSegment((FloatOrd, FloatOrd), (FloatOrd, FloatOrd)); + +impl HashableSegment { + pub(crate) fn new(segment: Segment) -> Self { + let a = (FloatOrd(segment.a.x), FloatOrd(segment.a.y)); + let b = (FloatOrd(segment.b.x), FloatOrd(segment.b.y)); + if a < b { + Self(a, b) + } else { + Self(b, a) + } + } +} + +#[cfg(test)] +mod tests { + use std::collections::hash_map::DefaultHasher; + use std::hash::{Hash, Hasher}; + + use parry2d::math::Point; + + use super::*; + + #[test] + fn test_hashable_segment() { + let a = hash(HashableSegment::new(Segment::new( + Point::new(1., 2.), + Point::new(3., 4.), + ))); + let b = hash(HashableSegment::new(Segment::new( + Point::new(3., 4.), + Point::new(1., 2.), + ))); + let c = hash(HashableSegment::new(Segment::new( + Point::new(2., 1.), + Point::new(3., 4.), + ))); + + assert_eq!(a, b); + assert_ne!(a, c); + } + + fn hash(obj: T) -> u64 + where + T: Hash, + { + let mut hasher = DefaultHasher::new(); + obj.hash(&mut hasher); + hasher.finish() + } +}