hierarchical pathfinding

common/src/hierarchical.rs

@@ -0,0 +1,130 @@
+use crate::{
+    astar::astar,
+    pathfinding::WorldPath,
+    vol::{ReadVol, RectRasterableVol},
+    volumes::vol_grid_2d::VolGrid2d,
+};
+
+use std::fmt::Debug;
+use vek::*;
+
+#[derive(Clone, Debug, Default)]
+pub struct ChunkPath {
+    pub from: Vec3<f32>,
+    pub dest: Vec3<f32>,
+    pub chunk_path: Option<Vec<Vec2<i32>>>,
+}
+
+impl ChunkPath {
+    pub fn new<V: RectRasterableVol + ReadVol + Debug>(
+        vol: &VolGrid2d<V>,
+        from: Vec3<f32>,
+        dest: Vec3<f32>,
+    ) -> Self {
+        let ifrom: Vec3<i32> = Vec3::from(from.map(|e| e.floor() as i32));
+        let idest: Vec3<i32> = Vec3::from(dest.map(|e| e.floor() as i32));
+
+        let start_chunk = vol.pos_key(ifrom);
+        let end_chunk = vol.pos_key(idest);
+
+        let chunk_path = astar(
+            start_chunk,
+            end_chunk,
+            chunk_euclidean_distance,
+            |pos| ChunkPath::chunk_get_neighbors(vol, pos),
+            chunk_transition_cost,
+        );
+
+        Self {
+            from,
+            dest,
+            chunk_path,
+        }
+    }
+
+    pub fn chunk_get_neighbors<V: RectRasterableVol + ReadVol + Debug>(
+        _vol: &VolGrid2d<V>,
+        pos: &Vec2<i32>,
+    ) -> impl IntoIterator<Item = Vec2<i32>> {
+        let directions = vec![
+            Vec2::new(1, 0),  // Right chunk
+            Vec2::new(-1, 0), // Left chunk
+            Vec2::new(0, 1),  // Top chunk
+            Vec2::new(0, -1), // Bottom chunk
+        ];
+
+        let neighbors: Vec<Vec2<i32>> = directions.into_iter().map(|dir| dir + pos).collect();
+
+        neighbors.into_iter()
+    }
+    pub fn worldpath_get_neighbors<V: RectRasterableVol + ReadVol + Debug>(
+        &mut self,
+        vol: &VolGrid2d<V>,
+        pos: Vec3<i32>,
+    ) -> impl IntoIterator<Item = Vec3<i32>> {
+        let directions = vec![
+            Vec3::new(0, 1, 0),   // Forward
+            Vec3::new(0, 1, 1),   // Forward upward
+            Vec3::new(0, 1, 2),   // Forward Upwardx2
+            Vec3::new(0, 1, -1),  // Forward downward
+            Vec3::new(1, 0, 0),   // Right
+            Vec3::new(1, 0, 1),   // Right upward
+            Vec3::new(1, 0, 2),   // Right Upwardx2
+            Vec3::new(1, 0, -1),  // Right downward
+            Vec3::new(0, -1, 0),  // Backwards
+            Vec3::new(0, -1, 1),  // Backward Upward
+            Vec3::new(0, -1, 2),  // Backward Upwardx2
+            Vec3::new(0, -1, -1), // Backward downward
+            Vec3::new(-1, 0, 0),  // Left
+            Vec3::new(-1, 0, 1),  // Left upward
+            Vec3::new(-1, 0, 2),  // Left Upwardx2
+            Vec3::new(-1, 0, -1), // Left downward
+        ];
+
+        let neighbors: Vec<Vec3<i32>> = directions
+            .into_iter()
+            .map(|dir| dir + pos)
+            .filter(|new_pos| self.is_valid_space(vol, *new_pos))
+            .collect();
+        neighbors.into_iter()
+    }
+    pub fn is_valid_space<V: RectRasterableVol + ReadVol + Debug>(
+        &mut self,
+        vol: &VolGrid2d<V>,
+        pos: Vec3<i32>,
+    ) -> bool {
+        let is_walkable_position = WorldPath::is_walkable_space(vol, pos);
+        let mut is_within_chunk = false;
+        match self.chunk_path.clone() {
+            Some(chunk_path) => {
+                is_within_chunk = chunk_path
+                    .iter()
+                    .any(|new_pos| new_pos.cmpeq(&vol.pos_key(pos)).iter().all(|e| *e));
+            }
+            _ => {
+                println!("No chunk path");
+            }
+        }
+        return is_walkable_position && is_within_chunk;
+    }
+    pub fn get_worldpath<V: RectRasterableVol + ReadVol + Debug>(
+        &mut self,
+        vol: &VolGrid2d<V>,
+    ) -> WorldPath {
+        let wp = WorldPath::new(vol, self.from, self.dest, |vol, pos| {
+            self.worldpath_get_neighbors(vol, *pos)
+        });
+        println!("Fetching world path from hierarchical path: {:?}", wp);
+        wp
+    }
+}
+
+pub fn chunk_euclidean_distance(start: &Vec2<i32>, end: &Vec2<i32>) -> f32 {
+    let istart = start.map(|e| e as f32);
+    let iend = end.map(|e| e as f32);
+    istart.distance(iend)
+}
+
+pub fn chunk_transition_cost(_start: &Vec2<i32>, _end: &Vec2<i32>) -> f32 {
+    1.0f32
+}

common/src/lib.rs

@@ -14,6 +14,7 @@ pub mod comp;
 pub mod effect;
 pub mod event;
 pub mod figure;
+pub mod hierarchical;
 pub mod msg;
 pub mod npc;
 pub mod pathfinding;

common/src/pathfinding.rs

@@ -13,19 +13,31 @@ pub struct WorldPath {
 }
 
 impl WorldPath {
-    pub fn new<V: ReadVol>(vol: &V, from: Vec3<f32>, dest: Vec3<f32>) -> Self {
+    pub fn new<V: ReadVol, I>(
+        vol: &V,
+        from: Vec3<f32>,
+        dest: Vec3<f32>,
+        get_neighbors: impl FnMut(&V, &Vec3<i32>) -> I,
+    ) -> Self
+    where
+        I: IntoIterator<Item = Vec3<i32>>,
+    {
         let ifrom: Vec3<i32> = Vec3::from(from.map(|e| e.floor() as i32));
         let idest: Vec3<i32> = Vec3::from(dest.map(|e| e.floor() as i32));
-        let path = WorldPath::get_path(vol, ifrom, idest);
+        let path = WorldPath::get_path(vol, ifrom, idest, get_neighbors);
 
         Self { from, dest, path }
     }
 
-    pub fn get_path<V: ReadVol>(
+    pub fn get_path<V: ReadVol, I>(
         vol: &V,
         from: Vec3<i32>,
         dest: Vec3<i32>,
-    ) -> Option<Vec<Vec3<i32>>> {
+        mut get_neighbors: impl FnMut(&V, &Vec3<i32>) -> I,
+    ) -> Option<Vec<Vec3<i32>>>
+    where
+        I: IntoIterator<Item = Vec3<i32>>,
+    {
         let new_start = WorldPath::get_z_walkable_space(vol, from);
         let new_dest = WorldPath::get_z_walkable_space(vol, dest);
 
@@ -34,7 +46,7 @@ impl WorldPath {
                 new_start,
                 new_dest,
                 euclidean_distance,
-                |pos| WorldPath::get_neighbors(vol, pos),
+                |pos| get_neighbors(vol, pos),
                 transition_cost,
             )
         } else {
@@ -136,7 +148,7 @@ impl WorldPath {
         if let Some(mut block_path) = self.path.clone() {
             if let Some(next_pos) = block_path.clone().last() {
                 if self.path_is_blocked(vol) {
-                    self.path = WorldPath::get_path(vol, ipos, idest)
+                    self.path = WorldPath::get_path(vol, ipos, idest, WorldPath::get_neighbors);
                 }
 
                 if Vec2::<i32>::from(ipos) == Vec2::<i32>::from(*next_pos) {

common/src/sys/agent.rs

@@ -1,6 +1,7 @@
 use crate::comp::{
     Agent, CharacterState, Controller, MountState, MovementState::Glide, Pos, Stats,
 };
+use crate::hierarchical::ChunkPath;
 use crate::pathfinding::WorldPath;
 use crate::terrain::TerrainGrid;
 use rand::{seq::SliceRandom, thread_rng};
@@ -72,7 +73,10 @@ impl<'a> System<'a> for Sys {
                         const MAX_TRAVEL_DIST: f32 = 200.0;
                         let new_dest = Vec3::new(rand::random::<f32>(), rand::random::<f32>(), 0.0)
                             * MAX_TRAVEL_DIST;
-                        new_path = Some(WorldPath::new(&*terrain, pos.0, pos.0 + new_dest));
+                        new_path = Some(
+                            ChunkPath::new(&*terrain, pos.0, pos.0 + new_dest)
+                                .get_worldpath(&*terrain),
+                        );
                     };
 
                     path.move_along_path(

server/src/cmd.rs

@@ -7,6 +7,7 @@ use chrono::{NaiveTime, Timelike};
 use common::{
     assets, comp,
     event::{EventBus, ServerEvent},
+    hierarchical::ChunkPath,
     msg::ServerMsg,
     npc::{get_npc_name, NpcKind},
     pathfinding::WorldPath,
@@ -517,7 +518,8 @@ fn handle_pathfind(server: &mut Server, player: EcsEntity, args: String, action:
                 .read_component_cloned::<comp::Pos>(target_entity)
             {
                 let target = start_pos.0 + Vec3::new(x, y, z);
-                let new_path = WorldPath::new(&*server.state.terrain(), start_pos.0, target);
+                let new_path = ChunkPath::new(&*server.state.terrain(), start_pos.0, target)
+                    .get_worldpath(&*server.state.terrain());
 
                 server.state.write_component(
                     target_entity,