From badd3113d53e257049ef45d4c606552f770c29cd Mon Sep 17 00:00:00 2001 From: Dylan Kile Date: Sun, 29 Dec 2019 20:58:21 +0000 Subject: [PATCH] hierarchical pathfinding --- common/src/hierarchical.rs | 130 +++++++++++++++++++++++++++++++++++++ common/src/lib.rs | 1 + common/src/pathfinding.rs | 24 +++++-- common/src/sys/agent.rs | 6 +- server/src/cmd.rs | 4 +- 5 files changed, 157 insertions(+), 8 deletions(-) create mode 100644 common/src/hierarchical.rs diff --git a/common/src/hierarchical.rs b/common/src/hierarchical.rs new file mode 100644 index 0000000000..0bfe94aec7 --- /dev/null +++ b/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, + pub dest: Vec3, + pub chunk_path: Option>>, +} + +impl ChunkPath { + pub fn new( + vol: &VolGrid2d, + from: Vec3, + dest: Vec3, + ) -> Self { + let ifrom: Vec3 = Vec3::from(from.map(|e| e.floor() as i32)); + let idest: Vec3 = 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( + _vol: &VolGrid2d, + pos: &Vec2, + ) -> impl IntoIterator> { + 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> = directions.into_iter().map(|dir| dir + pos).collect(); + + neighbors.into_iter() + } + pub fn worldpath_get_neighbors( + &mut self, + vol: &VolGrid2d, + pos: Vec3, + ) -> impl IntoIterator> { + 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> = 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( + &mut self, + vol: &VolGrid2d, + pos: Vec3, + ) -> 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( + &mut self, + vol: &VolGrid2d, + ) -> 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, end: &Vec2) -> 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, _end: &Vec2) -> f32 { + 1.0f32 +} diff --git a/common/src/lib.rs b/common/src/lib.rs index 2550d7db7e..8540e359fb 100644 --- a/common/src/lib.rs +++ b/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; diff --git a/common/src/pathfinding.rs b/common/src/pathfinding.rs index 26bc6e5ef5..f970498141 100644 --- a/common/src/pathfinding.rs +++ b/common/src/pathfinding.rs @@ -13,19 +13,31 @@ pub struct WorldPath { } impl WorldPath { - pub fn new(vol: &V, from: Vec3, dest: Vec3) -> Self { + pub fn new( + vol: &V, + from: Vec3, + dest: Vec3, + get_neighbors: impl FnMut(&V, &Vec3) -> I, + ) -> Self + where + I: IntoIterator>, + { let ifrom: Vec3 = Vec3::from(from.map(|e| e.floor() as i32)); let idest: Vec3 = 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( + pub fn get_path( vol: &V, from: Vec3, dest: Vec3, - ) -> Option>> { + mut get_neighbors: impl FnMut(&V, &Vec3) -> I, + ) -> Option>> + where + I: IntoIterator>, + { 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::::from(ipos) == Vec2::::from(*next_pos) { diff --git a/common/src/sys/agent.rs b/common/src/sys/agent.rs index 5ce18dacab..ed337de06c 100644 --- a/common/src/sys/agent.rs +++ b/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::(), rand::random::(), 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( diff --git a/server/src/cmd.rs b/server/src/cmd.rs index cca446f1a9..36793f6e66 100644 --- a/server/src/cmd.rs +++ b/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::(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,