Refactor pathfinding #33

crates/common/src/traits/neighbors.rs

@@ -16,17 +16,19 @@ pub trait Neighbors<T> {
     fn all_neighbors(&self) -> Vec<(T, u32)>;
 }
 
+pub type SquaredDistance = u32;
+
 #[rustfmt::skip]
 impl Neighbors<IVec3> for IVec3 {
-    fn same_layer_neighbors(&self) -> Vec<(IVec3, u32)> {
+    fn same_layer_neighbors(&self) -> Vec<(IVec3, SquaredDistance)> {
         vec![
             (self + IVec3::new(-1,  1,  0), 2), (self + IVec3::new( 0,  1,  0), 1), (self + IVec3::new( 1,  1,  0), 2),
             (self + IVec3::new(-1,  0,  0), 1),                                     (self + IVec3::new( 1,  0,  0), 1),
             (self + IVec3::new(-1, -1,  0), 2), (self + IVec3::new( 0, -1,  0), 1), (self + IVec3::new( 1, -1,  0), 2),
         ]
     }
 
-    fn all_neighbors(&self) -> Vec<(IVec3, u32)> {
+    fn all_neighbors(&self) -> Vec<(IVec3, SquaredDistance)> {
         vec![
             // layer above
             (self + IVec3::new(-1,  1,  1), 3), (self + IVec3::new( 0,  1,  1), 2), (self + IVec3::new( 1,  1,  1), 3),

crates/map_generation/src/block_type.rs

@@ -39,6 +39,11 @@ impl SolidMaterial {
             SolidMaterial::Grass => TileTextureIndex(1),
         }
     }
+
+    /// Returns the cost of an entity leaving this field.
+    pub const fn traversal_cost(&self) -> f32 {
+        1.0
+    }
 }
 
 impl BlockType {

crates/map_generation/src/world_map.rs

@@ -47,7 +47,8 @@ impl WorldMap {
             .or_insert(Chunk::new(coordinates, self.noise))
     }
 
-    /// Tries to fetch a block from world. Will return None, if the chunk doesn't exist or the block is of type BlockType::None
+    /// Tries to fetch a block from world. Will return None, if the chunk
+    /// doesn't exist or the block is of type BlockType::None
     pub fn get_block(&self, coordinates: IWorldCoordinates) -> Option<BlockType> {
         let (chunk_coordinates, block_coordinates) = coordinates.to_chunk_and_block();
         let index = to_index(block_coordinates);
@@ -59,7 +60,8 @@ impl WorldMap {
             })
     }
 
-    /// Returns a result of type BlockType, if the corresponding chunk has been found. Returns an empty error, when the chunk is not loaded.
+    /// Returns an option of type BlockType, if the corresponding chunk has been
+    /// found. Returns None when the chunk is not loaded.
     pub fn get_raw_block(&self, coordinates: IWorldCoordinates) -> Option<BlockType> {
         let (chunk_coordinate, block_coordinates) = coordinates.to_chunk_and_block();
         let index = to_index(block_coordinates);

crates/pathfinding/src/lib.rs

@@ -1,56 +1,11 @@
 use bevy::prelude::*;
-use common::traits::AddNamedObserver;
-use map_generation::world_map::WorldMap;
 use path::Path;
-use pathfinder::{Pathfinder, PathfinderListener, PathfindingErrors, PathfindingState};
 
 pub mod path;
 pub mod pathfinder;
-
-pub fn plugin(app: &mut App) {
-    app.register_type::<Pathfinder>()
-        .register_type::<Path>()
-        .add_systems(Update, calculate_path.run_if(resource_exists::<WorldMap>))
-        .add_systems(
-            Update,
-            (path::tick_path, path::follow_path, check_pathfinder).chain(),
-        )
-        .add_named_observer(listen_for_path, "listen_for_path");
-}
-
-fn calculate_path(
-    world_map: Res<WorldMap>,
-    mut query: Query<(Entity, &mut Pathfinder)>,
-    mut commands: Commands,
-) {
-    for (entity, mut path) in &mut query {
-        match path.calculate_step(&world_map) {
-            PathfindingState::Calculating => (),
-            PathfindingState::Failed(err) => match err {
-                PathfindingErrors::NotEnoughChunks => {
-                    debug!("not enough chunks");
-                }
-                PathfindingErrors::Unreachable => {
-                    debug!("pathfinding failed");
-                    commands.trigger(PathfindingCalculationEvent {
-                        entity,
-                        calculation: PathfindingCalculation::Failed,
-                    });
-
-                    commands.entity(entity).despawn();
-                }
-            },
-            PathfindingState::Complete(path) => {
-                debug!("pathfinder {} done", entity);
-                commands.trigger(PathfindingCalculationEvent {
-                    entity,
-                    calculation: PathfindingCalculation::Succeeded(path),
-                });
-                commands.entity(entity).despawn();
-            }
-        }
-    }
-}
+mod pathfinding;
+pub mod pathfinding_map;
+pub use pathfinding::plugin;
 
 #[derive(EntityEvent)]
 pub struct PathEvent {
@@ -76,54 +31,3 @@ enum PathfindingCalculation {
     Failed,
     Succeeded(Path),
 }
-
-fn listen_for_path(
-    trigger: On<PathfindingCalculationEvent>,
-    listeners: Query<&PathfinderListener>,
-    mut commands: Commands,
-) {
-    // if the event is triggered on a listener, we insert the path
-    if let PathfindingCalculation::Succeeded(path) = &trigger.calculation {
-        // if this is a successful path, we don't care about any other paths, so:
-        // add path to the listener entity and remove listener
-        if listeners.contains(trigger.entity) {
-            debug!(
-                "entity {} has found a successful path, removing all pathfinding children",
-                trigger.entity
-            );
-            commands
-                .entity(trigger.entity)
-                .remove::<PathfinderListener>()
-                .insert(path.clone());
-        }
-    }
-}
-
-fn check_pathfinder(
-    listeners: Query<(Entity, Option<&Children>), With<PathfinderListener>>,
-    pathfinders: Query<Entity, With<Pathfinder>>,
-    mut commands: Commands,
-) {
-    for (parent, children) in listeners {
-        if children.is_none_or(|x| {
-            pathfinders
-                .iter()
-                .filter(|element| x.contains(element))
-                .count()
-                == 0
-        }) {
-            // no children, so remove dis shit
-            debug!(
-                "entity {} has no more Pathfinder children, removing listener...",
-                parent
-            );
-            commands
-                .entity(parent)
-                .remove::<PathfinderListener>()
-                .trigger(|entity| PathEvent {
-                    entity,
-                    state: PathState::CalculationFailed,
-                });
-        }
-    }
-}

crates/pathfinding/src/pathfinder.rs

@@ -2,10 +2,9 @@ use std::cmp::Reverse;
 
 use bevy::{ecs::spawn::SpawnIter, platform::collections::HashMap, prelude::*};
 use common::{traits::Neighbors, types::IWorldCoordinates};
-use map_generation::{block_type::BlockType, world_map::WorldMap};
 use priority_queue::PriorityQueue;
 
-use crate::path::Path;
+use crate::{path::Path, pathfinding_map::PathfindingMap};
 
 /// Attach this to calculate and ultimately follow a path.
 ///
@@ -17,7 +16,7 @@ pub struct Pathfinder {
     #[reflect(ignore)]
     frontier: PriorityQueue<IVec3, Reverse<u32>>,
     came_from: HashMap<IVec3, Option<IVec3>>,
-    cost_so_far: HashMap<IVec3, u32>,
+    cost_so_far: HashMap<IVec3, f32>,
     steps: u32,
     allowed_failures: u8,
     current_failures: u8,
@@ -50,7 +49,7 @@ impl Pathfinder {
         let mut came_from = HashMap::default();
         came_from.insert(start.0, None);
         let mut cost_so_far = HashMap::default();
-        cost_so_far.insert(start.0, 0);
+        cost_so_far.insert(start.0, 0.0);
         Pathfinder {
             target: target.0,
             frontier,
@@ -93,75 +92,39 @@ impl Pathfinder {
         )
     }
 
-    pub(crate) fn calculate_step(&mut self, world_map: &WorldMap) -> PathfindingState {
-        let Some((current_coordinates, current_priority)) = self.frontier.pop() else {
-            debug!("No frontier available");
+    pub(crate) fn calculate_step(
+        &mut self,
+        pathfinding_map: &impl PathfindingMap,
+    ) -> PathfindingState {
+        let Some((current_coordinates, _current_priority)) = self.frontier.pop() else {
+            info!("No frontier available");
             return PathfindingState::Failed(PathfindingErrors::Unreachable);
         };
 
         if current_coordinates == self.target {
-            debug!("frontier is target");
+            info!("frontier is target");
             return PathfindingState::Complete(Path::new(self.to_path()));
         }
 
-        for (neighbor, neighbor_cost) in current_coordinates.all_neighbors() {
-            match self.is_floor_block(world_map, neighbor) {
-                Ok(true) => trace!("block is floor"),
-                Ok(false) => {
-                    trace!("block is NOT floor");
-                    continue;
-                }
-                Err(e) => {
-                    debug!("error, failure: {}", self.current_failures);
-                    self.current_failures += 1;
-                    self.frontier.push(current_coordinates, current_priority);
-                    return PathfindingState::Failed(e);
-                }
-            }
-
+        for (neighbor, neighbor_cost) in pathfinding_map.get_neighbors(current_coordinates) {
+            info!(
+                "current {:?} to neighbor {:?} would cost {}",
+                current_coordinates, neighbor, neighbor_cost
+            );
             let new_cost = self.cost_so_far.get(&current_coordinates).unwrap() + neighbor_cost;
             let current_cost = self.cost_so_far.get(&neighbor);
             if current_cost.is_none() || new_cost < *current_cost.unwrap() {
                 self.cost_so_far.insert(neighbor, new_cost);
-                let priority =
-                    new_cost + heuristic(world_map) + neighbor.distance_squared(self.target) as u32;
-                self.frontier.push(neighbor, Reverse(priority));
+                let priority = new_cost + heuristic(neighbor, self.target);
+                self.frontier
+                    .push(neighbor, Reverse(priority.round() as u32));
                 self.came_from.insert(neighbor, Some(current_coordinates));
             }
         }
         self.steps += 1;
         PathfindingState::Calculating
     }
 
-    fn is_floor_block(
-        &self,
-        world_map: &WorldMap,
-        neighbor: IVec3,
-    ) -> Result<bool, PathfindingErrors> {
-        let neighbor_block = world_map
-            .get_raw_block(IWorldCoordinates(neighbor))
-            .ok_or({
-                if self.current_failures >= self.allowed_failures {
-                    PathfindingErrors::Unreachable
-                } else {
-                    PathfindingErrors::NotEnoughChunks
-                }
-            })?;
-
-        trace!("checking {}, is {:?}", neighbor, neighbor_block);
-        let block_below = world_map
-            .get_raw_block(IWorldCoordinates(neighbor - IVec3::Z))
-            .ok_or({
-                if self.current_failures >= self.allowed_failures {
-                    PathfindingErrors::Unreachable
-                } else {
-                    PathfindingErrors::NotEnoughChunks
-                }
-            })?;
-        trace!("below {}, is {:?}", neighbor - IVec3::Z, block_below);
-        Ok(neighbor_block == BlockType::None && matches!(block_below, BlockType::Solid(_)))
-    }
-
     fn to_path(&self) -> Vec<IWorldCoordinates> {
         let mut points = vec![];
         let mut next = self.target;
@@ -179,8 +142,8 @@ impl Pathfinder {
     }
 }
 
-fn heuristic(_world_map: &WorldMap) -> u32 {
-    1
+fn heuristic(from: IVec3, to: IVec3) -> f32 {
+    from.distance_squared(to) as f32
 }
 
 pub(crate) enum PathfindingState {
@@ -190,6 +153,5 @@ pub(crate) enum PathfindingState {
 }
 
 pub(crate) enum PathfindingErrors {
-    NotEnoughChunks,
     Unreachable,
 }