From 36e92e18ebe5c75b206257996220ff85633c1c00 Mon Sep 17 00:00:00 2001 From: Joshua Barretto Date: Thu, 23 Jan 2020 14:10:49 +0000 Subject: [PATCH] Added pauseable pathfinding, improved Chaser heuristics, etc. --- common/src/astar.rs | 100 +++++++++++++++++++- common/src/comp/agent.rs | 5 +- common/src/lib.rs | 2 - common/src/path.rs | 136 ++++++++++++++++++++++----- common/src/pathfinding.rs | 188 -------------------------------------- common/src/sys/agent.rs | 36 +------- server/src/cmd.rs | 52 ----------- server/src/lib.rs | 4 +- server/src/test_world.rs | 5 + 9 files changed, 223 insertions(+), 305 deletions(-) delete mode 100644 common/src/pathfinding.rs diff --git a/common/src/astar.rs b/common/src/astar.rs index f399712919..51dbbcd73f 100644 --- a/common/src/astar.rs +++ b/common/src/astar.rs @@ -1,3 +1,4 @@ +use crate::path::Path; use core::cmp::Ordering::Equal; use hashbrown::{HashMap, HashSet}; use std::cmp::Ordering; @@ -5,7 +6,7 @@ use std::collections::BinaryHeap; use std::f32; use std::hash::Hash; -#[derive(Copy, Clone)] +#[derive(Copy, Clone, Debug)] pub struct PathEntry { cost: f32, node: S, @@ -116,3 +117,100 @@ where None } + +pub enum PathResult { + None, + Exhausted, + Path(Path), + Pending, +} + +#[derive(Clone, Debug)] +pub struct Astar { + iter: usize, + max_iters: usize, + potential_nodes: BinaryHeap>, + came_from: HashMap, + cheapest_scores: HashMap, + final_scores: HashMap, + visited: HashSet, +} + +impl Astar { + pub fn new(max_iters: usize, start: S, heuristic: impl FnOnce(&S) -> f32) -> Self { + Self { + max_iters, + iter: 0, + potential_nodes: std::iter::once(PathEntry { + cost: 0.0, + node: start.clone(), + }) + .collect(), + came_from: HashMap::default(), + cheapest_scores: std::iter::once((start.clone(), 0.0)).collect(), + final_scores: std::iter::once((start.clone(), heuristic(&start))).collect(), + visited: std::iter::once(start).collect(), + } + } + + pub fn poll( + &mut self, + iters: usize, + mut heuristic: impl FnMut(&S) -> f32, + mut neighbors: impl FnMut(&S) -> I, + mut transition: impl FnMut(&S, &S) -> f32, + mut satisfied: impl FnMut(&S) -> bool, + ) -> PathResult + where + I: Iterator, + { + while self.iter < self.max_iters.min(self.iter + iters) { + if let Some(PathEntry { node, .. }) = self.potential_nodes.pop() { + if satisfied(&node) { + return PathResult::Path(self.reconstruct_path_to(node)); + } else { + for neighbor in neighbors(&node) { + let node_cheapest = self.cheapest_scores.get(&node).unwrap_or(&f32::MAX); + let neighbor_cheapest = + self.cheapest_scores.get(&neighbor).unwrap_or(&f32::MAX); + + let cost = node_cheapest + transition(&node, &neighbor); + if cost < *neighbor_cheapest { + self.came_from.insert(neighbor.clone(), node.clone()); + self.cheapest_scores.insert(neighbor.clone(), cost); + let neighbor_cost = cost + heuristic(&neighbor); + self.final_scores.insert(neighbor.clone(), neighbor_cost); + + if self.visited.insert(neighbor.clone()) { + self.potential_nodes.push(PathEntry { + node: neighbor.clone(), + cost: neighbor_cost, + }); + } + } + } + } + } else { + return PathResult::None; + } + + self.iter += 1 + } + + if self.iter >= self.max_iters { + PathResult::Exhausted + } else { + PathResult::Pending + } + } + + fn reconstruct_path_to(&mut self, end: S) -> Path { + let mut path = vec![end.clone()]; + let mut cnode = &end; + while let Some(node) = self.came_from.get(cnode) { + path.push(node.clone()); + cnode = node; + } + path.into_iter().rev().collect() + } +} diff --git a/common/src/comp/agent.rs b/common/src/comp/agent.rs index ff6d05485e..a6a6326f6a 100644 --- a/common/src/comp/agent.rs +++ b/common/src/comp/agent.rs @@ -1,4 +1,4 @@ -use crate::{path::Chaser, pathfinding::WorldPath}; +use crate::path::Chaser; use specs::{Component, Entity as EcsEntity}; use specs_idvs::IDVStorage; use vek::*; @@ -14,9 +14,6 @@ pub enum Agent { bearing: Vec2, target: Option, }, - Traveler { - path: WorldPath, - }, } impl Agent { diff --git a/common/src/lib.rs b/common/src/lib.rs index b6cfb55ea3..5bdebb171d 100644 --- a/common/src/lib.rs +++ b/common/src/lib.rs @@ -15,11 +15,9 @@ pub mod effect; pub mod event; pub mod figure; pub mod generation; -pub mod hierarchical; pub mod msg; pub mod npc; pub mod path; -pub mod pathfinding; pub mod ray; pub mod region; pub mod state; diff --git a/common/src/path.rs b/common/src/path.rs index ceee6962d2..33b93144a2 100644 --- a/common/src/path.rs +++ b/common/src/path.rs @@ -1,5 +1,5 @@ use crate::{ - pathfinding::WorldPath, + astar::{Astar, PathResult}, terrain::Block, vol::{BaseVol, ReadVol}, }; @@ -9,29 +9,29 @@ use vek::*; // Path #[derive(Default, Clone, Debug)] -pub struct Path { - nodes: Vec>, +pub struct Path { + nodes: Vec, } -impl FromIterator> for Path { - fn from_iter>>(iter: I) -> Self { +impl FromIterator for Path { + fn from_iter>(iter: I) -> Self { Self { nodes: iter.into_iter().collect(), } } } -impl Path { +impl Path { pub fn len(&self) -> usize { self.nodes.len() } - pub fn start(&self) -> Option> { - self.nodes.first().copied() + pub fn start(&self) -> Option<&T> { + self.nodes.first() } - pub fn end(&self) -> Option> { - self.nodes.last().copied() + pub fn end(&self) -> Option<&T> { + self.nodes.last() } } @@ -39,18 +39,18 @@ impl Path { #[derive(Default, Clone, Debug)] pub struct Route { - path: Path, + path: Path>, next_idx: usize, } -impl From for Route { - fn from(path: Path) -> Self { +impl From>> for Route { + fn from(path: Path>) -> Self { Self { path, next_idx: 0 } } } impl Route { - pub fn path(&self) -> &Path { + pub fn path(&self) -> &Path> { &self.path } @@ -85,10 +85,11 @@ impl Route { pub struct Chaser { last_search_tgt: Option>, route: Route, + astar: Option>>, } impl Chaser { - pub fn chase(&mut self, vol: &V, tgt: Vec3, pos: Vec3) -> Option> + pub fn chase(&mut self, vol: &V, pos: Vec3, tgt: Vec3) -> Option> where V: BaseVol + ReadVol, { @@ -98,7 +99,7 @@ impl Chaser { return None; } - let bearing = if let Some(end) = self.route.path().end() { + let bearing = if let Some(end) = self.route.path().end().copied() { let end_to_tgt = end.map(|e| e as f32).distance(tgt); if end_to_tgt > pos_to_tgt * 0.3 + 5.0 { None @@ -123,17 +124,104 @@ impl Chaser { .map(|last_tgt| last_tgt.distance(tgt) > pos_to_tgt * 0.15 + 5.0) .unwrap_or(true) { - let path: Path = WorldPath::find(vol, pos, tgt) - .ok() - .and_then(|wp| wp.path.map(|nodes| nodes.into_iter().rev())) - .into_iter() - .flatten() - .collect(); - - self.route = path.into(); + self.route = find_path(&mut self.astar, vol, pos, tgt).into(); } Some(tgt - pos) } } } + +fn find_path( + astar: &mut Option>>, + vol: &V, + start: Vec3, + end: Vec3, +) -> Path> +where + V: BaseVol + ReadVol, +{ + let is_walkable = |pos: &Vec3| { + vol.get(*pos - Vec3::new(0, 0, 1)) + .map(|b| b.is_solid()) + .unwrap_or(false) + && vol + .get(*pos + Vec3::new(0, 0, 0)) + .map(|b| !b.is_solid()) + .unwrap_or(true) + && vol + .get(*pos + Vec3::new(0, 0, 1)) + .map(|b| !b.is_solid()) + .unwrap_or(true) + }; + let get_walkable_z = |pos| { + let mut z_incr = 0; + for i in 0..32 { + let test_pos = pos + Vec3::unit_z() * z_incr; + if is_walkable(&test_pos) { + return Some(test_pos); + } + z_incr = -z_incr + if z_incr <= 0 { 1 } else { 0 }; + } + None + }; + + let (start, end) = match ( + get_walkable_z(start.map(|e| e.floor() as i32)), + get_walkable_z(end.map(|e| e.floor() as i32)), + ) { + (Some(start), Some(end)) => (start, end), + _ => return Path::default(), + }; + + let heuristic = |pos: &Vec3| (pos.distance_squared(end) as f32).sqrt(); + let neighbors = |pos: &Vec3| { + let pos = *pos; + const dirs: [Vec3; 17] = [ + 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 + Vec3::new(0, 0, -1), // Downwards + ]; + + dirs.iter() + .map(move |dir| pos + dir) + .filter(move |pos| is_walkable(pos)) + }; + let transition = |_: &Vec3, _: &Vec3| 1.0; + let satisfied = |pos: &Vec3| pos == &end; + + let mut new_astar = match astar.take() { + None => Astar::new(50000, start, heuristic.clone()), + Some(astar) => astar, + }; + + let path_result = new_astar.poll(30, heuristic, neighbors, transition, satisfied); + + *astar = Some(new_astar); + + match path_result { + PathResult::Path(path) => { + *astar = None; + path + } + PathResult::Pending => Path::default(), + _ => { + *astar = None; + Path::default() + } + } +} diff --git a/common/src/pathfinding.rs b/common/src/pathfinding.rs deleted file mode 100644 index ca8e8f1d45..0000000000 --- a/common/src/pathfinding.rs +++ /dev/null @@ -1,188 +0,0 @@ -use crate::comp::{ControllerInputs, Pos}; -use crate::{ - astar::astar, - vol::{ReadVol, Vox}, -}; -use vek::*; - -#[derive(Clone, Debug, Default)] -pub struct WorldPath { - pub from: Vec3, - pub dest: Vec3, - pub path: Option>>, -} - -impl WorldPath { - pub fn find<'a, V: ReadVol>(vol: &'a V, from: Vec3, dest: Vec3) -> Result { - Self::new(vol, from, dest, Self::get_neighbors) - } - - pub fn new<'a, V: ReadVol, I>( - vol: &'a V, - from: Vec3, - dest: Vec3, - get_neighbors: impl FnMut(&'a V, Vec3) -> I, - ) -> Result - where - I: Iterator> + 'a, - { - 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, get_neighbors).ok_or(())?; - - Ok(Self { - from, - dest, - path: Some(path), - }) - } - - pub fn get_path<'a, V: ReadVol, I>( - vol: &'a V, - from: Vec3, - dest: Vec3, - mut get_neighbors: impl FnMut(&'a V, Vec3) -> I, - ) -> Option>> - where - I: Iterator> + 'a, - { - let new_start = WorldPath::get_z_walkable_space(vol, from); - let new_dest = WorldPath::get_z_walkable_space(vol, dest); - - if let (Some(new_start), Some(new_dest)) = (new_start, new_dest) { - astar( - new_start, - new_dest, - euclidean_distance, - |pos| get_neighbors(vol, *pos), - transition_cost, - ) - } else { - None - } - } - - fn get_z_walkable_space(vol: &V, pos: Vec3) -> Option> { - let mut z_incr = 0; - for i in 0..32 { - let test_pos = pos + Vec3::unit_z() * z_incr; - if WorldPath::is_walkable_space(vol, test_pos) { - return Some(test_pos); - } - z_incr = -z_incr + if z_incr <= 0 { 1 } else { 0 }; - } - - None - } - - pub fn is_walkable_space(vol: &V, pos: Vec3) -> bool { - vol.get(pos - Vec3::unit_z()) - .map(|v| !v.is_empty()) - .unwrap_or(false) - && (0..2).all(|z| { - vol.get(pos + Vec3::new(0, 0, z)) - .map(|v| v.is_empty()) - .unwrap_or(true) - }) - } - - pub fn get_neighbors<'a, V: ReadVol>( - vol: &'a V, - pos: Vec3, - ) -> impl Iterator> + 'a { - 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 - Vec3::new(0, 0, -1), // Downwards - ]; - - directions - .into_iter() - .map(move |dir| dir + pos) - .filter(move |new_pos| Self::is_walkable_space(vol, *new_pos)) - } - - pub fn move_along_path( - &mut self, - vol: &V, - pos: &Pos, - inputs: &mut ControllerInputs, - is_destination: impl Fn(Vec3, Vec3) -> bool, - mut found_destination: impl FnMut(), - ) { - // No path available - if self.path == None { - return; - } - - let ipos = pos.0.map(|e| e.floor() as i32); - let idest = self.dest.map(|e| e.floor() as i32); - - // We have reached the end of the path - if is_destination(ipos, idest) { - found_destination(); - } - - 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, WorldPath::get_neighbors); - } - - if Vec2::::from(ipos) == Vec2::::from(*next_pos) { - block_path.pop(); - self.path = Some(block_path); - } - - // Move the input towards the next area on the path - inputs.move_dir = Vec2::from(next_pos.map(|e| (e as f32).floor() + 0.5) - pos.0); - - // Need to jump to continue - if next_pos.z >= ipos.z + 1 { - inputs.jump.set_state(true); - } - - // Need to glide - let min_z_glide_height = 3; - if next_pos.z - min_z_glide_height < ipos.z { - inputs.glide.set_state(true); - } - } else { - found_destination(); - } - } else { - found_destination(); - } - } - - pub fn path_is_blocked(&self, vol: &V) -> bool { - match self.path.clone() { - Some(path) => path - .iter() - .any(|pos| !WorldPath::is_walkable_space(vol, *pos)), - _ => false, - } - } -} - -pub fn euclidean_distance(start: &Vec3, end: &Vec3) -> f32 { - start.map(|e| e as f32).distance((*end).map(|e| e as f32)) -} - -pub fn transition_cost(_start: &Vec3, _end: &Vec3) -> f32 { - 1.0 -} diff --git a/common/src/sys/agent.rs b/common/src/sys/agent.rs index 8d18f1525a..ffb4e4c8a7 100644 --- a/common/src/sys/agent.rs +++ b/common/src/sys/agent.rs @@ -1,7 +1,7 @@ use crate::comp::{ Agent, CharacterState, Controller, MountState, MovementState::Glide, Pos, Stats, }; -use crate::{hierarchical::ChunkPath, path::Path, pathfinding::WorldPath, terrain::TerrainGrid}; +use crate::terrain::TerrainGrid; use rand::{seq::SliceRandom, thread_rng}; use specs::{Entities, Join, ReadExpect, ReadStorage, System, WriteStorage}; use vek::*; @@ -61,35 +61,6 @@ impl<'a> System<'a> for Sys { let mut inputs = &mut controller.inputs; match agent { - Agent::Traveler { path } => { - let mut new_path: Option = None; - let is_destination = |cur_pos: Vec3, dest: Vec3| { - Vec2::::from(cur_pos) == Vec2::::from(dest) - }; - - let found_destination = || { - const MAX_TRAVEL_DIST: f32 = 200.0; - let new_dest = Vec3::new(rand::random::(), rand::random::(), 0.0) - * MAX_TRAVEL_DIST; - new_path = Some( - ChunkPath::new(&*terrain, pos.0, pos.0 + new_dest) - .get_worldpath(&*terrain) - .unwrap(), - ); - }; - - path.move_along_path( - &*terrain, - pos, - &mut inputs, - is_destination, - found_destination, - ); - - if let Some(new_path) = new_path { - *path = new_path; - } - } Agent::Wanderer(bearing) => { *bearing += Vec2::new(rand::random::() - 0.5, rand::random::() - 0.5) * 0.1 @@ -103,8 +74,9 @@ impl<'a> System<'a> for Sys { Agent::Pet { target, chaser } => { // Run towards target. if let Some(tgt_pos) = positions.get(*target) { - if let Some(bearing) = chaser.chase(&*terrain, tgt_pos.0, pos.0) { - inputs.move_dir = Vec2::from(bearing).normalized(); + if let Some(bearing) = chaser.chase(&*terrain, pos.0, tgt_pos.0) { + inputs.move_dir = + Vec2::from(bearing).try_normalized().unwrap_or(Vec2::zero()); inputs.jump.set_state(bearing.z > 1.0); } } else { diff --git a/server/src/cmd.rs b/server/src/cmd.rs index d1a3504666..595bbb3d2a 100644 --- a/server/src/cmd.rs +++ b/server/src/cmd.rs @@ -7,10 +7,8 @@ use chrono::{NaiveTime, Timelike}; use common::{ assets, comp, event::{EventBus, ServerEvent}, - hierarchical::ChunkPath, msg::{PlayerListUpdate, ServerMsg}, npc::{get_npc_name, NpcKind}, - pathfinding::WorldPath, state::TimeOfDay, sync::{Uid, WorldSyncExt}, terrain::{Block, BlockKind, TerrainChunkSize}, @@ -249,13 +247,6 @@ lazy_static! { true, handle_debug, ), - ChatCommand::new( - "pathfind", - "{} {d} {d} {d}", - "/pathfind : Send a given entity with ID to the coordinates provided", - true, - handle_pathfind, - ), ]; } fn handle_give(server: &mut Server, entity: EcsEntity, args: String, _action: &ChatCommand) { @@ -524,46 +515,6 @@ fn handle_spawn(server: &mut Server, entity: EcsEntity, args: String, action: &C } } -fn handle_pathfind(server: &mut Server, player: EcsEntity, args: String, action: &ChatCommand) { - if let (Some(id), Some(x), Some(y), Some(z)) = - scan_fmt_some!(&args, action.arg_fmt, u64, f32, f32, f32) - { - let entity = server.state.ecs().entity_from_uid(id); - - if let Some(target_entity) = entity { - if let Some(start_pos) = server - .state - .read_component_cloned::(target_entity) - { - let target = start_pos.0 + Vec3::new(x, y, z); - let new_path = ChunkPath::new(&*server.state.terrain(), start_pos.0, target) - .get_worldpath(&*server.state.terrain()) - .unwrap(); - - server.state.write_component( - target_entity, - comp::Agent::Traveler { - path: new_path.clone(), - }, - ); - - if let Some(path_positions) = &new_path.path { - for pos in path_positions { - server - .state - .set_block(*pos, Block::new(BlockKind::Normal, Rgb::new(255, 255, 0))); - } - } - } - } else { - server.notify_client( - player, - ServerMsg::private(format!("Unable to find entity with ID: {:?}", id)), - ); - } - } -} - fn handle_players(server: &mut Server, entity: EcsEntity, _args: String, _action: &ChatCommand) { let ecs = server.state.ecs(); let players = ecs.read_storage::(); @@ -629,9 +580,6 @@ fn alignment_to_agent(alignment: &str, target: EcsEntity) -> Option match alignment { "hostile" => Some(comp::Agent::enemy()), "friendly" => Some(comp::Agent::pet(target)), - "traveler" => Some(comp::Agent::Traveler { - path: WorldPath::default(), - }), // passive? _ => None, } diff --git a/server/src/lib.rs b/server/src/lib.rs index 853689f7ee..d2cf54da7c 100644 --- a/server/src/lib.rs +++ b/server/src/lib.rs @@ -46,6 +46,8 @@ use std::{ sync::Arc, time::{Duration, Instant}, }; +#[cfg(not(feature = "worldgen"))] +use test_world::{World, WORLD_SIZE}; use uvth::{ThreadPool, ThreadPoolBuilder}; use vek::*; #[cfg(feature = "worldgen")] @@ -53,8 +55,6 @@ use world::{ sim::{FileOpts, WorldOpts, DEFAULT_WORLD_MAP, WORLD_SIZE}, World, }; -#[cfg(not(feature = "worldgen"))] -use test_world::World; const CLIENT_TIMEOUT: f64 = 20.0; // Seconds diff --git a/server/src/test_world.rs b/server/src/test_world.rs index 6eb4764b61..d8f804e07f 100644 --- a/server/src/test_world.rs +++ b/server/src/test_world.rs @@ -7,6 +7,11 @@ use rand::{prelude::*, rngs::SmallRng}; use std::time::Duration; use vek::*; +pub const WORLD_SIZE: Vec2 = Vec2 { + x: 1024 * 1, + y: 1024 * 1, +}; + pub struct World; impl World {