Add advanced path finding to new 'Traveler' enemy using A* algorithm

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<S> {
+    cost: f32,
+    node: S,
+}
+
+impl<S: Eq> PartialEq for PathEntry<S> {
+    fn eq(&self, other: &PathEntry<S>) -> bool {
+        self.node.eq(&other.node)
+    }
+}
+
+impl<S: Eq> Eq for PathEntry<S> {}
+
+impl<S: Eq> Ord for PathEntry<S> {
+    // This method implements reverse ordering, so that the lowest cost
+    // will be ordered first
+    fn cmp(&self, other: &PathEntry<S>) -> Ordering {
+        other.cost.partial_cmp(&self.cost).unwrap_or(Equal)
+    }
+}
+
+impl<S: Eq> PartialOrd for PathEntry<S> {
+    fn partial_cmp(&self, other: &PathEntry<S>) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+fn reconstruct_path<S>(came_from: &HashMap<S, S>, target: &S) -> Vec<S>
+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<S, I>(
+    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<Vec<S>>
+where
+    S: Clone + Eq + Hash,
+    I: IntoIterator<Item = S>,
+{
+    // 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, &current));
+        }
+
+        let current_neighbors = neighbors(&current);
+        for neighbor in current_neighbors {
+            let current_cheapest_score = cheapest_scores.get(&current).unwrap_or(&f32::MAX);
+            let neighbor_cheapest_score = cheapest_scores.get(&neighbor).unwrap_or(&f32::MAX);
+            let score = current_cheapest_score + transition_cost(&current, &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
+}

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<f32>),
     Pet {
@@ -13,6 +14,9 @@ pub enum Agent {
         bearing: Vec2<f32>,
         target: Option<EcsEntity>,
     },
+    Traveler {
+        path: WorldPath,
+    },
 }
 
 impl Agent {

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;

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<f32>,
+    pub dest: Vec3<f32>,
+    pub path: Option<Vec<Vec3<i32>>>,
+}
+
+impl WorldPath {
+    pub fn new<V: ReadVol>(vol: &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 path = WorldPath::get_path(vol, ifrom, idest);
+
+        Self { from, dest, path }
+    }
+
+    pub fn get_path<V: ReadVol>(
+        vol: &V,
+        from: Vec3<i32>,
+        dest: Vec3<i32>,
+    ) -> Option<Vec<Vec3<i32>>> {
+        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<V: ReadVol>(vol: &V, pos: Vec3<i32>) -> Option<Vec3<i32>> {
+        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<V: ReadVol>(vol: &V, pos: Vec3<i32>) -> 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<V: ReadVol>(
+        vol: &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
+            Vec3::new(0, 0, -1),  // Downwards
+        ];
+
+        let neighbors: Vec<Vec3<i32>> = 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<V: ReadVol>(
+        &mut self,
+        vol: &V,
+        pos: &Pos,
+        inputs: &mut ControllerInputs,
+        is_destination: impl Fn(Vec3<i32>, Vec3<i32>) -> 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::<i32>::from(ipos) == Vec2::<i32>::from(*next_pos) {
+                    block_path.pop();
+                    self.path = Some(block_path);
+                }
+
+                let move_dir = Vec2::<i32>::from(next_pos - ipos);
+
+                // Move the input towards the next area on the path
+                inputs.move_dir = Vec2::<f32>::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<V: ReadVol>(&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<i32>, end: &Vec3<i32>) -> f32 {
+    start.map(|e| e as f32).distance(end.map(|e| e as f32))
+}
+
+pub fn transition_cost(_start: &Vec3<i32>, _end: &Vec3<i32>) -> f32 {
+    1.0f32
+}

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<WorldPath> = None;
+                    let is_destination = |cur_pos: Vec3<i32>, dest: Vec3<i32>| {
+                        Vec2::<i32>::from(cur_pos) == Vec2::<i32>::from(dest)
+                    };
+
+                    let found_destination = || {
+                        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));
+                    };
+
+                    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::<f32>() - 0.5, rand::random::<f32>() - 0.5)
                         * 0.1

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::<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);
+
+                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::<comp::Player>();
@@ -555,6 +609,9 @@ fn alignment_to_agent(alignment: &str, target: EcsEntity) -> Option<comp::Agent>
             target,
             offset: Vec2::zero(),
         }),
+        "traveler" => Some(comp::Agent::Traveler {
+            path: WorldPath::default(),
+        }),
         // passive?
         _ => None,
     }