Implement PathingPlugin

Relates to #12.

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<Item = (&'a GlobalTransform, &'a Ichnography)>>(
+        ichnographies: T,
+    ) -> Vec<Self> {
+        let mut exclusions: Vec<Self> = 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<ExclusionArea> =
+                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<Point<f32>> = 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<Point<f32>> = exclusions
+            .iter()
+            .flat_map(|e| e.points())
+            .cloned()
+            .collect();
+        Self {
+            polygon: ConvexPolygon::from_convex_hull(&points).unwrap(),
+        }
+    }
+
+    pub(crate) fn points(&self) -> &[Point<f32>] {
+        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<ExclusionArea> 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()
+    }
+}

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

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::<ChangeEvent>()
+            .add_event::<PathFinderUpdated>()
+            .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<MapBounds>, mut events: EventWriter<ChangeEvent>) {
+    commands.insert_resource(PathFinder::new(bounds.as_ref()));
+    events.send(ChangeEvent);
+}
+
+fn removed(mut events: EventWriter<ChangeEvent>, removed: RemovedComponents<StaticSolid>) {
+    if removed.iter().next().is_some() {
+        events.send(ChangeEvent);
+    }
+}
+
+fn updated(
+    mut events: EventWriter<ChangeEvent>,
+    changed: Query<
+        Entity,
+        (
+            With<StaticSolid>,
+            Or<(Changed<Ichnography>, Changed<GlobalTransform>)>,
+        ),
+    >,
+) {
+    if changed.iter().next().is_some() {
+        events.send(ChangeEvent);
+    }
+}
+
+fn update(
+    mut changes: EventReader<ChangeEvent>,
+    mut pf_updated: EventWriter<PathFinderUpdated>,
+    bounds: Res<MapBounds>,
+    mut finder: ResMut<PathFinder>,
+    entities: Query<(&GlobalTransform, &Ichnography), With<StaticSolid>>,
+) {
+    // 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);
+}