From d8fc7cb667e7c3db14b918fb76002baa01c1148b Mon Sep 17 00:00:00 2001 From: Forest Anderson Date: Wed, 11 Dec 2019 05:28:45 +0000 Subject: [PATCH] Add advanced path finding to new 'Traveler' enemy using A* algorithm --- common/src/astar.rs | 111 +++++++++++++++++++++++ common/src/comp/agent.rs | 6 +- common/src/lib.rs | 2 + common/src/pathfinding.rs | 182 ++++++++++++++++++++++++++++++++++++++ common/src/sys/agent.rs | 41 ++++++++- server/src/cmd.rs | 65 +++++++++++++- 6 files changed, 400 insertions(+), 7 deletions(-) create mode 100644 common/src/astar.rs create mode 100644 common/src/pathfinding.rs diff --git a/common/src/astar.rs b/common/src/astar.rs new file mode 100644 index 0000000000..22718cc857 --- /dev/null +++ b/common/src/astar.rs @@ -0,0 +1,111 @@ +use core::cmp::Ordering::Equal; +use hashbrown::{HashMap, HashSet}; +use std::cmp::Ordering; +use std::collections::BinaryHeap; +use std::f32; +use std::hash::Hash; + +#[derive(Copy, Clone)] +pub struct PathEntry { + cost: f32, + node: S, +} + +impl PartialEq for PathEntry { + fn eq(&self, other: &PathEntry) -> bool { + self.node.eq(&other.node) + } +} + +impl Eq for PathEntry {} + +impl Ord for PathEntry { + // This method implements reverse ordering, so that the lowest cost + // will be ordered first + fn cmp(&self, other: &PathEntry) -> Ordering { + other.cost.partial_cmp(&self.cost).unwrap_or(Equal) + } +} + +impl PartialOrd for PathEntry { + fn partial_cmp(&self, other: &PathEntry) -> Option { + Some(self.cmp(other)) + } +} + +fn reconstruct_path(came_from: &HashMap, target: &S) -> Vec +where + S: Clone + Eq + Hash, +{ + let mut path = Vec::new(); + path.push(target.to_owned()); + let mut cur_node = target; + while let Some(node) = came_from.get(cur_node) { + path.push(node.to_owned()); + cur_node = node; + } + path +} + +pub fn astar( + initial: S, + target: S, + mut heuristic: impl FnMut(&S, &S) -> f32, + mut neighbors: impl FnMut(&S) -> I, + mut transition_cost: impl FnMut(&S, &S) -> f32, +) -> Option> +where + S: Clone + Eq + Hash, + I: IntoIterator, +{ + // Set of discovered nodes so far + let mut potential_nodes = BinaryHeap::new(); + potential_nodes.push(PathEntry { + cost: 0.0f32, + node: initial.clone(), + }); + + // For entry e, contains the cheapest node preceding it on the known path from start to e + let mut came_from = HashMap::new(); + + // Contains cheapest cost from 'initial' to the current entry + let mut cheapest_scores = HashMap::new(); + cheapest_scores.insert(initial.clone(), 0.0f32); + + // Contains cheapest score to get to node + heuristic to the end, for an entry + let mut final_scores = HashMap::new(); + final_scores.insert(initial.clone(), heuristic(&initial, &target)); + + // Set of nodes we have already visited + let mut visited = HashSet::new(); + visited.insert(initial.clone()); + + while let Some(PathEntry { node: current, .. }) = potential_nodes.pop() { + if current == target { + return Some(reconstruct_path(&came_from, ¤t)); + } + + let current_neighbors = neighbors(¤t); + for neighbor in current_neighbors { + let current_cheapest_score = cheapest_scores.get(¤t).unwrap_or(&f32::MAX); + let neighbor_cheapest_score = cheapest_scores.get(&neighbor).unwrap_or(&f32::MAX); + let score = current_cheapest_score + transition_cost(¤t, &neighbor); + if score < *neighbor_cheapest_score { + // Path to the neighbor is better than anything yet recorded + came_from.insert(neighbor.to_owned(), current.to_owned()); + cheapest_scores.insert(neighbor.clone(), score); + let neighbor_score = score + heuristic(&neighbor, &target); + final_scores.insert(neighbor.clone(), neighbor_score); + + if visited.insert(neighbor.clone()) { + potential_nodes.push(PathEntry { + node: neighbor.clone(), + cost: neighbor_score, + }); + } + } + } + } + + None +} diff --git a/common/src/comp/agent.rs b/common/src/comp/agent.rs index 38190848f0..f63abb5e8a 100644 --- a/common/src/comp/agent.rs +++ b/common/src/comp/agent.rs @@ -1,8 +1,9 @@ +use crate::pathfinding::WorldPath; use specs::{Component, Entity as EcsEntity}; use specs_idvs::IDVStorage; use vek::*; -#[derive(Copy, Clone, Debug)] +#[derive(Clone, Debug)] pub enum Agent { Wanderer(Vec2), Pet { @@ -13,6 +14,9 @@ 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 4c7196f4ca..eab693848d 100644 --- a/common/src/lib.rs +++ b/common/src/lib.rs @@ -8,6 +8,7 @@ extern crate serde_derive; extern crate log; pub mod assets; +pub mod astar; pub mod clock; pub mod comp; pub mod effect; @@ -15,6 +16,7 @@ pub mod event; pub mod figure; pub mod msg; pub mod npc; +pub mod pathfinding; pub mod ray; pub mod region; pub mod state; diff --git a/common/src/pathfinding.rs b/common/src/pathfinding.rs new file mode 100644 index 0000000000..26bc6e5ef5 --- /dev/null +++ b/common/src/pathfinding.rs @@ -0,0 +1,182 @@ +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 new(vol: &V, 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 path = WorldPath::get_path(vol, ifrom, idest); + + Self { from, dest, path } + } + + pub fn get_path( + vol: &V, + from: Vec3, + dest: Vec3, + ) -> Option>> { + 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| WorldPath::get_neighbors(vol, pos), + transition_cost, + ) + } else { + None + } + } + + fn get_z_walkable_space(vol: &V, pos: Vec3) -> Option> { + if WorldPath::is_walkable_space(vol, pos) { + return Some(pos); + } + + let mut cur_pos_below = pos.clone(); + while !WorldPath::is_walkable_space(vol, cur_pos_below) && cur_pos_below.z > 0 { + cur_pos_below.z -= 1; + } + + let max_z = 1000; + let mut cur_pos_above = pos.clone(); + while !WorldPath::is_walkable_space(vol, cur_pos_above) && cur_pos_above.z <= max_z { + cur_pos_above.z += 1; + } + + if cur_pos_below.z > 0 { + Some(cur_pos_below) + } else if cur_pos_above.z < max_z { + Some(cur_pos_above) + } else { + None + } + } + + pub fn is_walkable_space(vol: &V, pos: Vec3) -> bool { + if let (Ok(voxel), Ok(upper_neighbor), Ok(upper_neighbor2)) = ( + vol.get(pos), + vol.get(pos + Vec3::new(0, 0, 1)), + vol.get(pos + Vec3::new(0, 0, 2)), + ) { + !voxel.is_empty() && upper_neighbor.is_empty() && upper_neighbor2.is_empty() + } else { + false + } + } + + pub fn get_neighbors( + vol: &V, + 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 + Vec3::new(0, 0, -1), // Downwards + ]; + + let neighbors: Vec> = directions + .into_iter() + .map(|dir| dir + pos) + .filter(|new_pos| Self::is_walkable_space(vol, *new_pos)) + .collect(); + + neighbors.into_iter() + } + + pub fn move_along_path( + &mut self, + vol: &V, + pos: &Pos, + inputs: &mut ControllerInputs, + is_destination: impl Fn(Vec3, Vec3) -> bool, + found_destination: impl FnOnce(), + ) { + // 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) + } + + if Vec2::::from(ipos) == Vec2::::from(*next_pos) { + block_path.pop(); + self.path = Some(block_path); + } + + let move_dir = Vec2::::from(next_pos - ipos); + + // Move the input towards the next area on the path + inputs.move_dir = Vec2::::new(move_dir.x as f32, move_dir.y as f32); + + // 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); + } + } + } + } + + 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.0f32 +} diff --git a/common/src/sys/agent.rs b/common/src/sys/agent.rs index c1603a6bdc..c2bcbe1c2a 100644 --- a/common/src/sys/agent.rs +++ b/common/src/sys/agent.rs @@ -2,8 +2,10 @@ use crate::comp::{ Agent, CharacterState, Controller, ControllerInputs, MountState, MovementState::Glide, Pos, Stats, }; +use crate::pathfinding::WorldPath; +use crate::terrain::TerrainGrid; use rand::{seq::SliceRandom, thread_rng}; -use specs::{Entities, Join, ReadStorage, System, WriteStorage}; +use specs::{Entities, Join, ReadExpect, ReadStorage, System, WriteStorage}; use vek::*; /// This system will allow NPCs to modify their controller @@ -14,6 +16,7 @@ impl<'a> System<'a> for Sys { ReadStorage<'a, Pos>, ReadStorage<'a, Stats>, ReadStorage<'a, CharacterState>, + ReadExpect<'a, TerrainGrid>, WriteStorage<'a, Agent>, WriteStorage<'a, Controller>, ReadStorage<'a, MountState>, @@ -21,7 +24,16 @@ impl<'a> System<'a> for Sys { fn run( &mut self, - (entities, positions, stats, character_states, mut agents, mut controllers, mount_states): Self::SystemData, + ( + entities, + positions, + stats, + character_states, + terrain, + mut agents, + mut controllers, + mount_states, + ): Self::SystemData, ) { for (entity, pos, agent, controller, mount_state) in ( &entities, @@ -51,6 +63,31 @@ impl<'a> System<'a> for Sys { let mut inputs = ControllerInputs::default(); 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(WorldPath::new(&*terrain, pos.0, pos.0 + new_dest)); + }; + + 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 diff --git a/server/src/cmd.rs b/server/src/cmd.rs index b978d658dd..a38a1126fe 100644 --- a/server/src/cmd.rs +++ b/server/src/cmd.rs @@ -9,8 +9,9 @@ use common::{ event::{EventBus, ServerEvent}, msg::ServerMsg, npc::{get_npc_name, NpcKind}, + pathfinding::WorldPath, state::TimeOfDay, - terrain::TerrainChunkSize, + terrain::{Block, BlockKind, TerrainChunkSize}, vol::RectVolSize, }; use rand::Rng; @@ -245,7 +246,13 @@ 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) { @@ -461,13 +468,22 @@ fn handle_spawn(server: &mut Server, entity: EcsEntity, args: String, action: &C ); let body = kind_to_body(id); - server + let new_entity = server .state .create_npc(pos, comp::Stats::new(get_npc_name(id), None), body) .with(comp::Vel(vel)) .with(comp::MountState::Unmounted) - .with(agent) + .with(agent.clone()) .build(); + + if let Some(uid) = server.state.ecs().uid_from_entity(new_entity) { + server.notify_client( + entity, + ServerMsg::private( + format!("Spawned entity with ID: {}", uid).to_owned(), + ), + ); + } } server.notify_client( entity, @@ -487,6 +503,44 @@ 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 = WorldPath::new(&*server.state.terrain(), start_pos.0, target); + + 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::(); @@ -555,6 +609,9 @@ fn alignment_to_agent(alignment: &str, target: EcsEntity) -> Option target, offset: Vec2::zero(), }), + "traveler" => Some(comp::Agent::Traveler { + path: WorldPath::default(), + }), // passive? _ => None, }