Simplify and fix various issues with pathfinding.

src/pathfinding.rs

@@ -1,5 +1,3 @@
-use std::marker::PhantomData;
-
 use bevy::{
     ecs::entity::Entities,
     prelude::*,
@@ -55,7 +53,7 @@ impl CostMap {
         }
     }
 
-    pub fn modifier(&self, x: usize, y: usize) -> Option<&f32> {
+    pub fn modifier(&self, x: i32, y: i32) -> Option<&f32> {
         let idx = (y as usize) * self.width + (x as usize);
         self.costs.get(idx)
     }
@@ -81,7 +79,8 @@ pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
                     for tile in map.get_available_exits(p.0 as usize, p.1 as usize) {
                         let mut cost = tile.2 * 100.;
                         if let Some(cost_map) = cost_map {
-                            if let Some(modifier) = cost_map.modifier(tile.0, tile.1) {
+                            if let Some(modifier) = cost_map.modifier(tile.0 as i32, tile.1 as i32)
+                            {
                                 cost *= modifier;
                             }
                         }
@@ -96,11 +95,10 @@ pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
     )
 }
 
-fn find_path_for_shape<D: 'static + Clone + Default + Send + Sync>(
+fn find_path_for_shape(
     initiator: ColliderHandle,
     start: Transform,
     destination: Destination,
-    map: Map<D>,
     cost_map: &Option<CostMap>,
     query_pipeline: QueryPipeline,
     collider_set: ColliderSet,
@@ -111,36 +109,42 @@ fn find_path_for_shape<D: 'static + Clone + Default + Send + Sync>(
         &start.i32(),
         |p| {
             let mut successors: Vec<((i32, i32), u32)> = vec![];
-            if let Some(tile) = map.at(p.0 as usize, p.1 as usize) {
+            let x = p.0 as i32;
-                if tile.is_walkable() {
+            let y = p.1 as i32;
-                    for tile in map.get_available_exits(p.0 as usize, p.1 as usize) {
+            let exits = vec![
-                        let mut should_push = true;
+                ((x - 1, y), 1.),
-                        let shape_pos = Isometry2::new(
+                ((x + 1, y), 1.),
-                            Vector2::new(tile.0 as f32 + 0.5, tile.1 as f32 + 0.5),
+                ((x, y - 1), 1.),
-                            0.,
+                ((x, y + 1), 1.),
-                        );
+                ((x - 1, y - 1), 1.5),
-                        query_pipeline.intersections_with_shape(
+                ((x + 1, y - 1), 1.5),
-                            &rigid_body_set,
+                ((x - 1, y + 1), 1.5),
-                            &collider_set,
+                ((x + 1, y + 1), 1.5),
-                            &shape_pos,
+            ];
-                            &*shape.raw,
+            for exit in &exits {
-                            bevy_rapier2d::rapier::pipeline::QueryFilter::new()
+                let mut should_push = true;
-                                .predicate(&|h, _c| h != initiator),
+                let shape_pos =
-                            |_handle| {
+                    Isometry2::new(Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32), 0.);
-                                should_push = false;
+                query_pipeline.intersections_with_shape(
-                                false
+                    &rigid_body_set,
-                            },
+                    &collider_set,
-                        );
+                    &shape_pos,
-                        if should_push {
+                    &*shape.raw,
-                            let mut cost = tile.2 * 100.;
+                    bevy_rapier2d::rapier::pipeline::QueryFilter::new()
-                            if let Some(cost_map) = cost_map {
+                        .predicate(&|h, _c| h != initiator),
-                                if let Some(modifier) = cost_map.modifier(tile.0, tile.1) {
+                    |_handle| {
-                                    cost *= modifier;
+                        should_push = false;
-                                }
+                        false
-                            }
+                    },
-                            successors.push(((tile.0 as i32, tile.1 as i32), cost as u32));
+                );
+                if should_push {
+                    let mut cost = exit.1 * 100.;
+                    if let Some(cost_map) = cost_map {
+                        if let Some(modifier) = cost_map.modifier(exit.0 .0, exit.0 .1) {
+                            cost *= modifier;
                         }
                     }
+                    successors.push((exit.0, cost as u32));
                 }
             }
             successors
@@ -155,7 +159,7 @@ fn find_path_for_shape<D: 'static + Clone + Default + Send + Sync>(
     }
 }
 
-fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
+fn calculate_path(
     mut commands: Commands,
     rapier_context: Res<RapierContext>,
     obstructions: Query<&RapierColliderHandle, With<MapObstruction>>,
@@ -170,7 +174,6 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
         ),
         Changed<Destination>,
     >,
-    map: Query<&Map<D>>,
 ) {
     for (entity, handle, destination, coordinates, shape, cost_map) in &query {
         if coordinates.i32() == **destination {
@@ -183,48 +186,50 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
                 .remove::<Speed>();
             continue;
         }
-        if let Ok(map) = map.get_single() {
+        let coordinates_clone = *coordinates;
-            let coordinates_clone = *coordinates;
+        let destination_clone = *destination;
-            let destination_clone = *destination;
+        let query_pipeline = rapier_context.query_pipeline.clone();
-            let query_pipeline = rapier_context.query_pipeline.clone();
+        let cost_map_clone = cost_map.cloned();
-            let map_clone = map.clone();
+        let handle_clone = *handle;
-            let cost_map_clone = cost_map.cloned();
+        let mut collider_set = rapier_context.colliders.clone();
-            let handle_clone = *handle;
+        let mut to_remove = vec![];
-            let mut collider_set = rapier_context.colliders.clone();
+        for handle in collider_set.iter() {
-            let mut to_remove = vec![];
+            if !obstructions.iter().map(|v| v.0).any(|x| x == handle.0) {
-            for handle in collider_set.iter() {
+                to_remove.push(handle.0);
-                if !obstructions.iter().map(|v| v.0).any(|x| x == handle.0) {
-                    to_remove.push(handle.0);
-                }
             }
-            let mut bodies = rapier_context.bodies.clone();
-            if !to_remove.is_empty() {
-                let mut islands = rapier_context.islands.clone();
-                for handle in to_remove {
-                    collider_set.remove(handle, &mut islands, &mut bodies, false);
-                }
-            }
-            let shape_clone = (*shape).clone();
-            let pool = AsyncComputeTaskPool::get();
-            let task = pool.spawn(async move {
-                find_path_for_shape(
-                    handle_clone.0,
-                    coordinates_clone,
-                    destination_clone,
-                    map_clone,
-                    &cost_map_clone,
-                    query_pipeline,
-                    collider_set,
-                    bodies,
-                    shape_clone,
-                )
-            });
-            commands.run_if_exists(entity, |mut entity| {
-                entity.insert(Calculating(task));
-                entity.remove::<Path>();
-                entity.remove::<NoPath>();
-            });
         }
+        let mut bodies = rapier_context.bodies.clone();
+        if !to_remove.is_empty() {
+            let mut islands = rapier_context.islands.clone();
+            for handle in to_remove {
+                collider_set.remove(handle, &mut islands, &mut bodies, false);
+            }
+        }
+        let shape_clone = (*shape).clone();
+        trace!(
+            "{:?}: path: calculating from {:?} to {:?}",
+            entity,
+            coordinates.i32(),
+            destination
+        );
+        let pool = AsyncComputeTaskPool::get();
+        let task = pool.spawn(async move {
+            find_path_for_shape(
+                handle_clone.0,
+                coordinates_clone,
+                destination_clone,
+                &cost_map_clone,
+                query_pipeline,
+                collider_set,
+                bodies,
+                shape_clone,
+            )
+        });
+        commands.run_if_exists(entity, |mut entity| {
+            entity.insert(Calculating(task));
+            entity.remove::<Path>();
+            entity.remove::<NoPath>();
+        });
     }
 }
 
@@ -232,8 +237,10 @@ fn poll_tasks(mut commands: Commands, mut query: Query<(Entity, &mut Calculating
     for (entity, mut calculating) in query.iter_mut() {
         if let Some(result) = future::block_on(future::poll_once(&mut **calculating)) {
             if let Some(path) = result {
+                trace!("{:?}: path: result: {:?}", entity, path);
                 commands.entity(entity).insert(path);
             } else {
+                trace!("{:?}: path: no path", entity);
                 commands.entity(entity).insert(NoPath);
             }
             commands.entity(entity).remove::<Calculating>();
@@ -264,53 +271,63 @@ fn negotiate_path(
         Option<&RotationSpeed>,
     )>,
 ) {
-    for (entity, mut path, mut transform, mut velocity, speed, rotation_speed) in query.iter_mut() {
+    for (entity, mut path, mut transform, mut velocity, speed, rotation_speed) in &mut query {
         let start_i32 = (transform.translation.x, transform.translation.y).i32();
-        let mut new_path = path
+        let mut cleanup = false;
-            .iter()
+        if let Some(last) = path.last() {
-            .cloned()
+            if last == &start_i32 {
-            .skip_while(|v| *v == start_i32)
+                trace!("{:?}: path: at destination", entity);
-            .collect::<Vec<(i32, i32)>>();
+                cleanup = true;
-        new_path.retain(|v| *v != start_i32);
-        **path = new_path;
-        if let Some(next) = path.first() {
-            let start = Vec2::new(transform.translation.x, transform.translation.y);
-            let next = Vec2::new(next.0 as f32 + 0.5, next.1 as f32 + 0.5);
-            let mut direction = next - start;
-            direction = direction.normalize();
-            direction *= **speed;
-            velocity.linvel = direction;
-            if rotation_speed.is_some() {
-                let v = next - start;
-                let angle = v.y.atan2(v.x);
-                transform.rotation = Quat::from_rotation_z(angle);
             }
-            continue;
+        }
-        } else {
+        if !cleanup {
+            if let Some(position) = path.iter().position(|v| v == &start_i32) {
+                trace!("{:?}: Path contains start", entity);
+                let (_, new_path) = path.split_at(position + 1);
+                **path = new_path.to_vec();
+            }
+            if let Some(next) = path.first() {
+                trace!(
+                    "{:?}: path: moving from {:?} to {:?}",
+                    entity,
+                    start_i32,
+                    next
+                );
+                let start = transform.translation.truncate();
+                let next = Vec2::new(next.0 as f32 + 0.5, next.1 as f32 + 0.5);
+                let mut direction = next - start;
+                direction = direction.normalize();
+                trace!("{:?}: path: direction: {:?}", entity, direction);
+                direction *= **speed;
+                velocity.linvel = direction;
+                trace!("{:?}: path: velocity: {:?}", entity, velocity.linvel);
+                if rotation_speed.is_some() {
+                    let v = next - start;
+                    let angle = v.y.atan2(v.x);
+                    transform.rotation = Quat::from_rotation_z(angle);
+                }
+            } else {
+                cleanup = true;
+            }
+        }
+        if cleanup {
             commands
                 .entity(entity)
                 .remove::<Path>()
                 .remove::<NoPath>()
                 .remove::<Destination>()
                 .remove::<Speed>();
-            velocity.linvel = Vec2::ZERO;
         }
     }
 }
 
-pub struct PathfindingPlugin<D: 'static + Clone + Default + Send + Sync>(PhantomData<D>);
+pub struct PathfindingPlugin;
 
-impl<D: 'static + Clone + Default + Send + Sync> Default for PathfindingPlugin<D> {
+impl Plugin for PathfindingPlugin {
-    fn default() -> Self {
-        Self(Default::default())
-    }
-}
-
-impl<D: 'static + Clone + Default + Send + Sync> Plugin for PathfindingPlugin<D> {
     fn build(&self, app: &mut App) {
-        app.add_system(calculate_path::<D>)
+        app.add_system(calculate_path)
             .add_system_to_stage(CoreStage::PostUpdate, remove_destination)
             .add_system(poll_tasks)
-            .add_system(negotiate_path.after(crate::navigation::limit_speed));
+            .add_system_to_stage(CoreStage::PostUpdate, negotiate_path);
     }
 }