diff --git a/Cargo.lock b/Cargo.lock index b1b85a9f..6817db2d 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -687,18 +687,6 @@ dependencies = [ "syn", ] -[[package]] -name = "deprecate-until" -version = "0.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7a3767f826efbbe5a5ae093920b58b43b01734202be697e1354914e862e8e704" -dependencies = [ - "proc-macro2", - "quote", - "semver", - "syn", -] - [[package]] name = "derive-new" version = "0.7.0" @@ -1490,15 +1478,6 @@ dependencies = [ "hashbrown 0.15.2", ] -[[package]] -name = "integer-sqrt" -version = "0.1.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "276ec31bcb4a9ee45f58bec6f9ec700ae4cf4f4f8f2fa7e06cb406bd5ffdd770" -dependencies = [ - "num-traits", -] - [[package]] name = "interface_procedural" version = "0.1.0" @@ -1626,6 +1605,7 @@ dependencies = [ name = "korangar" version = "0.1.0" dependencies = [ + "arrayvec", "bumpalo", "bytemuck", "cgmath", @@ -1645,7 +1625,6 @@ dependencies = [ "mlua", "num", "option-ext", - "pathfinding", "pollster", "ragnarok_bytes", "ragnarok_formats", @@ -2528,20 +2507,6 @@ version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" -[[package]] -name = "pathfinding" -version = "4.11.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1cff69f3ba9d0346c1dbe1248fc2ed4523567b683d1b6ff4144a6b3583369082" -dependencies = [ - "deprecate-until", - "indexmap", - "integer-sqrt", - "num-traits", - "rustc-hash 2.0.0", - "thiserror", -] - [[package]] name = "pcap" version = "2.2.0" @@ -3187,12 +3152,6 @@ version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d369a96f978623eb3dc28807c4852d6cc617fed53da5d3c400feff1ef34a714a" -[[package]] -name = "semver" -version = "1.0.23" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" - [[package]] name = "send_wrapper" version = "0.6.0" diff --git a/Cargo.toml b/Cargo.toml index 6b1ca864..9f7611e5 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3,6 +3,7 @@ resolver = "2" members = ["korangar", "ragnarok_*", "korangar_*"] [workspace.dependencies] +arrayvec = "0.7" bitflags = "2.6" bumpalo = "3.16" bytemuck = "1.20" @@ -27,7 +28,6 @@ lunify = "1.1" mlua = "0.10" num = "0.4" option-ext = "0.2" -pathfinding = "4.11" pcap = "2.2" pollster = "0.4" proc-macro2 = "1.0" diff --git a/korangar/Cargo.toml b/korangar/Cargo.toml index 5689bc19..e23101c1 100644 --- a/korangar/Cargo.toml +++ b/korangar/Cargo.toml @@ -4,6 +4,7 @@ version = "0.1.0" edition = "2021" [dependencies] +arrayvec = { workspace = true } bumpalo = { workspace = true, features = ["allocator_api"] } bytemuck = { workspace = true, features = ["derive", "extern_crate_std", "min_const_generics"] } cgmath = { workspace = true, features = ["mint", "serde"] } @@ -23,7 +24,6 @@ lunify = { workspace = true } mlua = { workspace = true, features = ["lua51", "vendored"] } num = { workspace = true } option-ext = { workspace = true } -pathfinding = { workspace = true } pollster = { workspace = true } ragnarok_bytes = { workspace = true, features = ["derive", "cgmath"] } ragnarok_formats = { workspace = true, features = ["interface"] } diff --git a/korangar/src/main.rs b/korangar/src/main.rs index 6922bcd3..128445bb 100644 --- a/korangar/src/main.rs +++ b/korangar/src/main.rs @@ -62,6 +62,7 @@ use korangar_networking::{ DisconnectReason, HotkeyState, LoginServerLoginData, MessageColor, NetworkEvent, NetworkEventBuffer, NetworkingSystem, SellItem, ShopItem, }; +use korangar_util::pathing::PathFinder; #[cfg(feature = "debug")] use korangar_util::texture_atlas::AtlasAllocation; #[cfg(not(feature = "debug"))] @@ -252,6 +253,7 @@ struct Client { player_inventory: Inventory, player_skill_tree: SkillTree, hotbar: Hotbar, + path_finder: PathFinder, point_light_set_buffer: ResourceSetBuffer, directional_shadow_object_set_buffer: ResourceSetBuffer, @@ -510,6 +512,7 @@ impl Client { let player_inventory = Inventory::default(); let player_skill_tree = SkillTree::default(); let hotbar = Hotbar::default(); + let path_finder = PathFinder::default(); let point_light_set_buffer = ResourceSetBuffer::default(); let directional_shadow_object_set_buffer = ResourceSetBuffer::default(); @@ -658,6 +661,7 @@ impl Client { player_inventory, player_skill_tree, hotbar, + path_finder, point_light_set_buffer, directional_shadow_object_set_buffer, point_shadow_object_set_buffer, @@ -956,6 +960,7 @@ impl Client { &mut self.animation_loader, &self.script_loader, &self.map, + &mut self.path_finder, saved_login_data.account_id, character_information, WorldPosition { x: 0, y: 0 }, @@ -1023,6 +1028,7 @@ impl Client { &mut self.animation_loader, &self.script_loader, &self.map, + &mut self.path_finder, entity_appeared_data, client_tick, ); @@ -1060,7 +1066,7 @@ impl Client { let position_from = Vector2::new(position_from.x, position_from.y); let position_to = Vector2::new(position_to.x, position_to.y); - entity.move_from_to(&self.map, position_from, position_to, starting_timestamp); + entity.move_from_to(&self.map, &mut self.path_finder, position_from, position_to, starting_timestamp); #[cfg(feature = "debug")] entity.generate_pathing_mesh(&self.device, &self.queue, &self.map, &self.pathing_texture_mapping); } @@ -1068,7 +1074,7 @@ impl Client { NetworkEvent::PlayerMove(position_from, position_to, starting_timestamp) => { let position_from = Vector2::new(position_from.x, position_from.y); let position_to = Vector2::new(position_to.x, position_to.y); - self.entities[0].move_from_to(&self.map, position_from, position_to, starting_timestamp); + self.entities[0].move_from_to(&self.map, &mut self.path_finder, position_from, position_to, starting_timestamp); #[cfg(feature = "debug")] self.entities[0].generate_pathing_mesh(&self.device, &self.queue, &self.map, &self.pathing_texture_mapping); diff --git a/korangar/src/world/entity/mod.rs b/korangar/src/world/entity/mod.rs index 1f545dcc..77ad432c 100644 --- a/korangar/src/world/entity/mod.rs +++ b/korangar/src/world/entity/mod.rs @@ -1,14 +1,15 @@ use std::string::String; use std::sync::Arc; +use arrayvec::ArrayVec; use cgmath::{EuclideanSpace, Point3, Vector2, VectorSpace}; use derive_new::new; use korangar_interface::elements::PrototypeElement; use korangar_interface::windows::{PrototypeWindow, Window}; use korangar_networking::EntityData; +use korangar_util::pathing::{PathFinder, MAX_WALK_PATH_SIZE}; #[cfg(feature = "debug")] use korangar_util::texture_atlas::AtlasAllocation; -use ragnarok_formats::map::TileFlags; use ragnarok_packets::{AccountId, CharacterInformation, ClientTick, EntityId, Sex, StatusType, WorldPosition}; #[cfg(feature = "debug")] use wgpu::{BufferUsages, Device, Queue}; @@ -51,7 +52,7 @@ impl ResourceState { #[derive(new, PrototypeElement)] pub struct Movement { #[hidden_element] - steps: Vec<(Vector2, u32)>, + steps: ArrayVec, starting_timestamp: u32, #[cfg(feature = "debug")] #[new(default)] @@ -59,6 +60,12 @@ pub struct Movement { pub pathing_vertex_buffer: Option>>, } +#[derive(Copy, Clone)] +pub struct Step { + arrival_position: Vector2, + arrival_timestamp: u32, +} + #[derive(Copy, Clone, PartialEq, Eq)] pub enum EntityType { Warp, @@ -292,6 +299,7 @@ impl Common { animation_loader: &mut AnimationLoader, script_loader: &ScriptLoader, map: &Map, + path_finder: &mut PathFinder, entity_data: EntityData, client_tick: ClientTick, ) -> Self { @@ -347,7 +355,7 @@ impl Common { if let Some(destination) = entity_data.destination { let position_from = Vector2::new(entity_data.position.x, entity_data.position.y); let position_to = Vector2::new(destination.x, destination.y); - common.move_from_to(map, position_from, position_to, client_tick); + common.move_from_to(map, path_finder, position_from, position_to, client_tick); } common @@ -377,20 +385,20 @@ impl Common { if let Some(active_movement) = self.active_movement.take() { let last_step = active_movement.steps.last().unwrap(); - if client_tick.0 > last_step.1 { - let position = Vector2::new(last_step.0.x, last_step.0.y); + if client_tick.0 > last_step.arrival_timestamp { + let position = Vector2::new(last_step.arrival_position.x, last_step.arrival_position.y); self.set_position(map, position, client_tick); } else { let mut last_step_index = 0; - while active_movement.steps[last_step_index + 1].1 < client_tick.0 { + while active_movement.steps[last_step_index + 1].arrival_timestamp < client_tick.0 { last_step_index += 1; } let last_step = active_movement.steps[last_step_index]; let next_step = active_movement.steps[last_step_index + 1]; - let last_step_position = last_step.0.map(|value| value as isize); - let next_step_position = next_step.0.map(|value| value as isize); + let last_step_position = last_step.arrival_position.map(|value| value as isize); + let next_step_position = next_step.arrival_position.map(|value| value as isize); let array = last_step_position - next_step_position; let array: &[isize; 2] = array.as_ref(); @@ -406,12 +414,12 @@ impl Common { _ => panic!("impossible step"), }; - let last_step_position = map.get_world_position(last_step.0).to_vec(); - let next_step_position = map.get_world_position(next_step.0).to_vec(); + let last_step_position = map.get_world_position(last_step.arrival_position).to_vec(); + let next_step_position = map.get_world_position(next_step.arrival_position).to_vec(); - let clamped_tick = u32::max(last_step.1, client_tick.0); - let total = next_step.1 - last_step.1; - let offset = clamped_tick - last_step.1; + let clamped_tick = u32::max(last_step.arrival_timestamp, client_tick.0); + let total = next_step.arrival_timestamp - last_step.arrival_timestamp; + let offset = clamped_tick - last_step.arrival_timestamp; let movement_elapsed = (1.0 / total as f32) * offset as f32; let position = last_step_position.lerp(next_step_position, movement_elapsed); @@ -424,125 +432,52 @@ impl Common { self.animation_state.update(client_tick); } - pub fn move_from_to(&mut self, map: &Map, from: Vector2, to: Vector2, starting_timestamp: ClientTick) { - use pathfinding::prelude::astar; - - #[derive(Clone, Debug, Eq, Hash, Ord, PartialEq, PartialOrd)] - struct Pos(usize, usize); - - impl Pos { - fn successors(&self, map: &Map) -> Vec { - let &Pos(x, y) = self; - let mut successors = Vec::new(); - - if map.x_in_bounds(x + 1) { - successors.push(Pos(x + 1, y)); - } - - if x > 0 { - successors.push(Pos(x - 1, y)); - } - - if map.y_in_bounds(y + 1) { - successors.push(Pos(x, y + 1)); - } - - if y > 0 { - successors.push(Pos(x, y - 1)); - } - - if map.x_in_bounds(x + 1) - && map.y_in_bounds(y + 1) - && map.get_tile(Vector2::new(x + 1, y)).flags.contains(TileFlags::WALKABLE) - && map.get_tile(Vector2::new(x, y + 1)).flags.contains(TileFlags::WALKABLE) - { - successors.push(Pos(x + 1, y + 1)); - } - - if x > 0 - && map.y_in_bounds(y + 1) - && map.get_tile(Vector2::new(x - 1, y)).flags.contains(TileFlags::WALKABLE) - && map.get_tile(Vector2::new(x, y + 1)).flags.contains(TileFlags::WALKABLE) - { - successors.push(Pos(x - 1, y + 1)); - } - - if map.x_in_bounds(x + 1) - && y > 0 - && map.get_tile(Vector2::new(x + 1, y)).flags.contains(TileFlags::WALKABLE) - && map.get_tile(Vector2::new(x, y - 1)).flags.contains(TileFlags::WALKABLE) - { - successors.push(Pos(x + 1, y - 1)); - } - - if x > 0 - && y > 0 - && map.get_tile(Vector2::new(x - 1, y)).flags.contains(TileFlags::WALKABLE) - && map.get_tile(Vector2::new(x, y - 1)).flags.contains(TileFlags::WALKABLE) - { - successors.push(Pos(x - 1, y - 1)); - } - - let successors = successors - .drain(..) - .filter(|Pos(x, y)| map.get_tile(Vector2::new(*x, *y)).flags.contains(TileFlags::WALKABLE)) - .collect::>(); - - successors - } - - fn convert_to_vector(self) -> Vector2 { - Vector2::new(self.0, self.1) + pub fn move_from_to( + &mut self, + map: &Map, + path_finder: &mut PathFinder, + start: Vector2, + goal: Vector2, + starting_timestamp: ClientTick, + ) { + if let Some(path) = path_finder.find_walkable_path(map, start, goal) { + if path.len() <= 1 { + return; } - } - - let result = astar( - &Pos(from.x, from.y), - |position| position.successors(map).into_iter().map(|position| (position, 0)), - |position| -> usize { - // Values taken from rAthena. - const MOVE_COST: usize = 10; - const DIAGONAL_MOVE_COST: usize = 14; - - let distance_x = usize::abs_diff(position.0, to.x); - let distance_y = usize::abs_diff(position.1, to.y); - let straight_moves = usize::abs_diff(distance_x, distance_y); - let diagonal_moves = usize::min(distance_x, distance_y); - - DIAGONAL_MOVE_COST * diagonal_moves + MOVE_COST * straight_moves - }, - |position| *position == Pos(to.x, to.y), - ) - .map(|x| x.0); - - if let Some(path) = result { let mut last_timestamp = starting_timestamp.0; let mut last_position: Option> = None; - let steps: Vec<(Vector2, u32)> = path - .into_iter() - .map(|pos| { + let steps: ArrayVec = path + .iter() + .map(|&step| { if let Some(position) = last_position { const DIAGONAL_MULTIPLIER: f32 = 1.4; - let speed = match position.x == pos.0 || position.y == pos.1 { - // true means we are moving orthogonally + let speed = match position.x == step.x || position.y == step.y { + // `true` means we are moving orthogonally true => self.movement_speed as u32, - // false means we are moving diagonally + // `false` means we are moving diagonally false => (self.movement_speed as f32 * DIAGONAL_MULTIPLIER) as u32, }; - let arrival_position = pos.convert_to_vector(); + let arrival_position = step; let arrival_timestamp = last_timestamp + speed; last_timestamp = arrival_timestamp; last_position = Some(arrival_position); - (arrival_position, arrival_timestamp) + Step { + arrival_position, + arrival_timestamp, + } } else { - last_position = Some(from); - (from, last_timestamp) + last_position = Some(start); + + Step { + arrival_position: start, + arrival_timestamp: last_timestamp, + } } }) .collect(); @@ -559,7 +494,7 @@ impl Common { } #[cfg(feature = "debug")] - fn pathing_texture_coordinates(steps: &Vec<(Vector2, u32)>, step: Vector2, index: usize) -> ([Vector2; 4], i32) { + fn pathing_texture_coordinates(steps: &[Step], step: Vector2, index: usize) -> ([Vector2; 4], i32) { if steps.len() - 1 == index { return ( [ @@ -572,7 +507,7 @@ impl Common { ); } - let delta = steps[index + 1].0.map(|component| component as isize) - step.map(|component| component as isize); + let delta = steps[index + 1].arrival_position.map(|component| component as isize) - step.map(|component| component as isize); match delta { Vector2 { x: 1, y: 0 } => ( @@ -670,9 +605,12 @@ impl Common { _ => Color::WHITE, }; - for (index, (step, _)) in active_movement.steps.iter().cloned().enumerate() { - let tile = map.get_tile(step); - let offset = Vector2::new(step.x as f32 * HALF_TILE_SIZE, step.y as f32 * HALF_TILE_SIZE); + for (index, Step { arrival_position, .. }) in active_movement.steps.iter().cloned().enumerate() { + let tile = map.get_tile(arrival_position); + let offset = Vector2::new( + arrival_position.x as f32 * HALF_TILE_SIZE, + arrival_position.y as f32 * HALF_TILE_SIZE, + ); let first_position = Point3::new(offset.x, tile.upper_left_height + PATHING_MESH_OFFSET, offset.y); let second_position = Point3::new( @@ -694,7 +632,7 @@ impl Common { let first_normal = NativeModelVertex::calculate_normal(first_position, second_position, third_position); let second_normal = NativeModelVertex::calculate_normal(fourth_position, first_position, third_position); - let (texture_coordinates, texture_index) = Self::pathing_texture_coordinates(&active_movement.steps, step, index); + let (texture_coordinates, texture_index) = Self::pathing_texture_coordinates(&active_movement.steps, arrival_position, index); native_pathing_vertices.push(NativeModelVertex::new( first_position, @@ -818,6 +756,7 @@ impl Player { animation_loader: &mut AnimationLoader, script_loader: &ScriptLoader, map: &Map, + path_finder: &mut PathFinder, account_id: AccountId, character_information: CharacterInformation, player_position: WorldPosition, @@ -834,6 +773,7 @@ impl Player { animation_loader, script_loader, map, + path_finder, EntityData::from_character(account_id, character_information, player_position), client_tick, ); @@ -969,6 +909,7 @@ impl Npc { animation_loader: &mut AnimationLoader, script_loader: &ScriptLoader, map: &Map, + path_finder: &mut PathFinder, entity_data: EntityData, client_tick: ClientTick, ) -> Self { @@ -978,6 +919,7 @@ impl Npc { animation_loader, script_loader, map, + path_finder, entity_data, client_tick, ); @@ -1134,8 +1076,15 @@ impl Entity { self.get_common_mut().update(map, delta_time, client_tick); } - pub fn move_from_to(&mut self, map: &Map, from: Vector2, to: Vector2, starting_timestamp: ClientTick) { - self.get_common_mut().move_from_to(map, from, to, starting_timestamp); + pub fn move_from_to( + &mut self, + map: &Map, + path_finder: &mut PathFinder, + from: Vector2, + to: Vector2, + starting_timestamp: ClientTick, + ) { + self.get_common_mut().move_from_to(map, path_finder, from, to, starting_timestamp); } #[cfg(feature = "debug")] diff --git a/korangar/src/world/map/mod.rs b/korangar/src/world/map/mod.rs index fb772b56..4b91bc30 100644 --- a/korangar/src/world/map/mod.rs +++ b/korangar/src/world/map/mod.rs @@ -9,6 +9,7 @@ use korangar_audio::AudioEngine; use korangar_interface::windows::PrototypeWindow; use korangar_util::collision::{Frustum, KDTree, Sphere, AABB}; use korangar_util::container::{SimpleKey, SimpleSlab}; +use korangar_util::pathing::Traversable; use korangar_util::{create_simple_key, Rectangle}; #[cfg(feature = "debug")] use option_ext::OptionExt; @@ -146,14 +147,6 @@ impl Map { self.water_bounds } - pub fn x_in_bounds(&self, x: usize) -> bool { - x <= self.width - } - - pub fn y_in_bounds(&self, y: usize) -> bool { - y <= self.height - } - pub fn get_world_position(&self, position: Vector2) -> Point3 { let height = average_tile_height(self.get_tile(position)); Point3::new(position.x as f32 * 5.0 + 2.5, height, position.y as f32 * 5.0 + 2.5) @@ -740,3 +733,19 @@ impl Map { Some((screen_position, screen_size)) } } + +impl Traversable for Map { + fn is_walkable(&self, position: Vector2) -> bool { + self.tiles + .get(position.x + position.y * self.width) + .map(|tile| tile.flags.contains(TileFlags::WALKABLE)) + .unwrap_or(false) + } + + fn is_snipeable(&self, position: Vector2) -> bool { + self.tiles + .get(position.x + position.y * self.width) + .map(|tile| tile.flags.contains(TileFlags::SNIPABLE)) + .unwrap_or(false) + } +} diff --git a/korangar_util/src/lib.rs b/korangar_util/src/lib.rs index 6bedd244..2cf894b0 100644 --- a/korangar_util/src/lib.rs +++ b/korangar_util/src/lib.rs @@ -7,6 +7,7 @@ pub mod color; pub mod container; mod loader; pub mod math; +pub mod pathing; mod rectangle; pub mod texture_atlas; diff --git a/korangar_util/src/pathing.rs b/korangar_util/src/pathing.rs new file mode 100644 index 00000000..bf3d8ce0 --- /dev/null +++ b/korangar_util/src/pathing.rs @@ -0,0 +1,393 @@ +//! Implements pathfinding algorithms. + +use std::cmp::Ordering; +use std::collections::BinaryHeap; + +use cgmath::Vector2; +use hashbrown::{HashMap, HashSet}; + +const MOVE_DIAGONAL_COST: usize = 14; +const MOVE_ORTHOGONAL_COST: usize = 10; +/// The maximum size a walkable path can have. +pub const MAX_WALK_PATH_SIZE: usize = 32; + +/// Essential trait that is needed to be implements for pathfinding. +pub trait Traversable { + /// Must return `true` if the position can be walked on. + fn is_walkable(&self, position: Vector2) -> bool; + /// Must return `true` if the position can be shot through. + fn is_snipeable(&self, position: Vector2) -> bool; +} + +#[derive(Copy, Clone, Eq, PartialEq)] +struct PathNode { + position: Vector2, + f_score: usize, + g_score: usize, +} + +impl Ord for PathNode { + fn cmp(&self, other: &Self) -> Ordering { + other.f_score.cmp(&self.f_score) + } +} + +impl PartialOrd for PathNode { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} + +/// Pathfinding algorithm for entity map navigation. +#[derive(Default)] +pub struct PathFinder { + open_set: BinaryHeap, + closed_set: HashSet>, + came_from: HashMap, Vector2>, + g_scores: HashMap, usize>, + path: Vec>, + neighbors: Vec>, +} + +impl PathFinder { + /// Returns the shortest walkable path between start and goal. Uses a simple + /// A* search algorithm like the legacy client and alternative server + /// implementations. It must have the same behavior, or else we would + /// "desync" with our client movement prediction. + pub fn find_walkable_path(&mut self, map: &impl Traversable, start: Vector2, goal: Vector2) -> Option<&[Vector2]> { + self.open_set.clear(); + self.closed_set.clear(); + self.came_from.clear(); + self.g_scores.clear(); + self.path.clear(); + + self.open_set.push(PathNode { + position: start, + g_score: 0, + f_score: Self::heuristic(start, goal), + }); + self.g_scores.insert(start, 0); + + while let Some(current) = self.open_set.pop() { + if current.position == goal { + return match self.reconstruct_path(start, goal) { + true => Some(&self.path), + false => None, + }; + } + + if self.closed_set.contains(¤t.position) { + continue; + } + self.closed_set.insert(current.position); + + self.find_neighbors(map, current.position); + + for neighbor in self.neighbors.drain(..) { + if self.closed_set.contains(&neighbor) { + continue; + } + + let movement_cost = if neighbor.x != current.position.x && neighbor.y != current.position.y { + MOVE_DIAGONAL_COST + } else { + MOVE_ORTHOGONAL_COST + }; + + let tentative_g_score = current.g_score + movement_cost; + + if tentative_g_score < self.g_scores.get(&neighbor).copied().unwrap_or(usize::MAX) { + self.came_from.insert(neighbor, current.position); + self.g_scores.insert(neighbor, tentative_g_score); + + let h_score = Self::heuristic(neighbor, goal); + let f_score = tentative_g_score + h_score; + + self.open_set.push(PathNode { + position: neighbor, + g_score: tentative_g_score, + f_score, + }); + } + } + } + + None + } + + /// Returns the shortest path between start and goal that can be shot + /// through. + pub fn find_snipable_path(&mut self, map: &impl Traversable, start: Vector2, goal: Vector2) -> Option<&[Vector2]> { + self.path.clear(); + + let mut current_x = start.x as isize; + let mut current_y = start.y as isize; + let mut target_x = goal.x as isize; + let mut target_y = goal.y as isize; + + let mut delta_x = target_x - current_x; + if delta_x < 0 { + std::mem::swap(&mut current_x, &mut target_x); + std::mem::swap(&mut current_y, &mut target_y); + delta_x = -delta_x; + } + let delta_y = target_y - current_y; + + self.path.push(Vector2::new(current_x as usize, current_y as usize)); + + let weight = if delta_x > delta_y.abs() { delta_x } else { delta_y.abs() }; + + let mut weight_x = 0; + let mut weight_y = 0; + + while current_x != target_x || current_y != target_y { + weight_x += delta_x; + weight_y += delta_y; + + if weight_x >= weight { + weight_x -= weight; + current_x += 1; + } + if weight_y >= weight { + weight_y -= weight; + current_y += 1; + } else if weight_y < 0 { + weight_y += weight; + current_y -= 1; + } + + if self.path.len() < MAX_WALK_PATH_SIZE { + self.path.push(Vector2::new(current_x as usize, current_y as usize)); + } else { + return None; + } + + if (current_x != target_x || current_y != target_y) && !map.is_snipeable(Vector2::new(current_x as usize, current_y as usize)) { + return None; + } + } + + Some(&self.path) + } + + fn heuristic(start: Vector2, goal: Vector2) -> usize { + let dx = (start.x as isize - goal.x as isize).unsigned_abs(); + let dy = (start.y as isize - goal.y as isize).unsigned_abs(); + let manhattan_distance = dx + dy; + MOVE_ORTHOGONAL_COST * manhattan_distance + } + + fn find_neighbors(&mut self, map: &impl Traversable, position: Vector2) { + let orthogonal_neighbors = [(1, 0), (-1, 0), (0, 1), (0, -1)]; + + for (dx, dy) in orthogonal_neighbors { + let new_x = position.x.wrapping_add_signed(dx); + let new_y = position.y.wrapping_add_signed(dy); + let new_position = Vector2::new(new_x, new_y); + + if map.is_walkable(new_position) { + self.neighbors.push(new_position); + } + } + + let diagonal_neighbors = [(1, 1), (1, -1), (-1, 1), (-1, -1)]; + + for (dx, dy) in diagonal_neighbors { + let new_x = position.x.wrapping_add_signed(dx); + let new_y = position.y.wrapping_add_signed(dy); + let new_position = Vector2::new(new_x, new_y); + + // Only allow diagonal neighbors when both adjacent orthogonal neighbors are + // also walkable. + if map.is_walkable(new_position) + && map.is_walkable(Vector2::new(position.x, new_y)) + && map.is_walkable(Vector2::new(new_x, position.y)) + { + self.neighbors.push(new_position); + } + } + } + + fn reconstruct_path(&mut self, start: Vector2, goal: Vector2) -> bool { + let mut current = goal; + + while current != start { + self.path.push(current); + current = *self.came_from.get(¤t).unwrap(); + + if self.path.len() >= MAX_WALK_PATH_SIZE { + return false; + } + } + + self.path.push(start); + self.path.reverse(); + + true + } +} + +#[cfg(test)] +mod tests { + use super::*; + + struct TestMap { + width: usize, + height: usize, + not_walkable: HashSet>, + not_snipable: HashSet>, + } + + impl TestMap { + fn new(width: usize, height: usize) -> Self { + Self { + width, + height, + not_walkable: HashSet::new(), + not_snipable: HashSet::new(), + } + } + + fn set_unwalkable(&mut self, points: &[Vector2]) { + for point in points { + self.not_walkable.insert(*point); + } + } + + fn set_unsnipable(&mut self, points: &[Vector2]) { + for point in points { + self.not_snipable.insert(*point); + } + } + } + + impl Traversable for TestMap { + fn is_walkable(&self, position: Vector2) -> bool { + position.x < self.width && position.y < self.height && !self.not_walkable.contains(&position) + } + + fn is_snipeable(&self, position: Vector2) -> bool { + position.x < self.width && position.y < self.height && !self.not_snipable.contains(&position) + } + } + + #[test] + fn test_straight_path() { + let map = TestMap::new(10, 10); + let mut pathfinder = PathFinder::default(); + + let start = Vector2::new(0, 0); + let goal = Vector2::new(3, 0); + + let path = pathfinder.find_walkable_path(&map, start, goal).unwrap(); + assert_eq!(path, vec![ + Vector2::new(0, 0), + Vector2::new(1, 0), + Vector2::new(2, 0), + Vector2::new(3, 0), + ]); + } + + #[test] + fn test_diagonal_path() { + let map = TestMap::new(10, 10); + let mut pathfinder = PathFinder::default(); + + let start = Vector2::new(0, 0); + let goal = Vector2::new(3, 3); + + let path = pathfinder.find_walkable_path(&map, start, goal).unwrap(); + assert_eq!(path, vec![ + Vector2::new(0, 0), + Vector2::new(1, 1), + Vector2::new(2, 2), + Vector2::new(3, 3), + ]); + } + + #[test] + fn test_path_with_obstacle() { + let mut map = TestMap::new(5, 5); + map.set_unwalkable(&[Vector2::new(1, 1), Vector2::new(1, 2), Vector2::new(1, 3)]); + + let mut pathfinder = PathFinder::default(); + let start = Vector2::new(0, 0); + let goal = Vector2::new(2, 2); + + let path = pathfinder.find_walkable_path(&map, start, goal).unwrap(); + + assert_eq!(path, vec![ + Vector2::new(0, 0), + Vector2::new(1, 0), + Vector2::new(2, 0), + Vector2::new(2, 1), + Vector2::new(2, 2), + ]); + } + + #[test] + fn test_no_path_possible() { + let mut map = TestMap::new(5, 5); + + map.set_unwalkable(&[ + Vector2::new(1, 0), + Vector2::new(1, 1), + Vector2::new(1, 2), + Vector2::new(1, 3), + Vector2::new(1, 4), + ]); + + let mut pathfinder = PathFinder::default(); + + let start = Vector2::new(0, 2); + let goal = Vector2::new(2, 2); + + assert!(pathfinder.find_walkable_path(&map, start, goal).is_none()); + } + + #[test] + fn test_shoot_path_straight() { + let map = TestMap::new(10, 10); + let mut pathfinder = PathFinder::default(); + + let start = Vector2::new(0, 0); + let goal = Vector2::new(3, 0); + + let path = pathfinder.find_snipable_path(&map, start, goal).unwrap(); + assert_eq!(path.len(), 4); + + for (index, step) in path.iter().enumerate() { + assert_eq!(step.x, index); + assert_eq!(step.y, 0); + } + } + + #[test] + fn test_shoot_path_diagonal() { + let map = TestMap::new(10, 10); + let mut pathfinder = PathFinder::default(); + + let start = Vector2::new(0, 0); + let goal = Vector2::new(3, 3); + + let path = pathfinder.find_snipable_path(&map, start, goal).unwrap(); + assert_eq!(path.len(), 4); + + for (index, step) in path.iter().enumerate() { + assert_eq!(step.x, index); + assert_eq!(step.y, index); + } + } + + #[test] + fn test_shoot_path_blocked() { + let mut map = TestMap::new(5, 5); + map.set_unsnipable(&[Vector2::new(1, 1)]); + + let mut pathfinder = PathFinder::default(); + let start = Vector2::new(0, 0); + let goal = Vector2::new(2, 2); + + assert!(pathfinder.find_snipable_path(&map, start, goal).is_none()); + } +}