├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ ├── documentation-comment.md │ └── feature_request.md └── workflows │ └── test.yml ├── .gitignore ├── .vscode └── settings.json ├── Cargo.toml ├── LICENSE ├── benches └── my_benchmark.rs ├── img ├── a_star.png ├── hpa.png ├── hpa_solution.png ├── plot │ ├── empty1024.png │ ├── empty128.png │ ├── random1024.png │ ├── random128.png │ ├── snake1024.png │ └── snake128.png └── problem.png ├── readme.md ├── src ├── graph │ ├── a_star.rs │ ├── dijkstra.rs │ ├── mod.rs │ ├── node.rs │ └── node_list.rs ├── grid │ ├── a_star.rs │ ├── dijkstra.rs │ └── mod.rs ├── lib.rs ├── neighbors.rs ├── path │ ├── abstract_path.rs │ ├── generic_path.rs │ ├── mod.rs │ └── path_segment.rs ├── path_cache.rs ├── path_cache │ ├── cache_config.rs │ └── chunk.rs └── utils.rs ├── test.sh └── tests └── path_cache.rs /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help fix a bug 4 | title: "[Bug]" 5 | labels: Bug 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | _A clear and concise description of what the bug is._ 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. 16 | 2. 17 | 3. 18 | 4. 19 | .... 20 | 21 | **Expected behavior** 22 | _What was supposed to happen_ 23 | 24 | **Actual behavior** 25 | _What happened instead / Error message_ 26 | 27 | **Screenshots** 28 | _If applicable, add screenshots to help explain your problem._ 29 | 30 | **Additional info** 31 | _Add any other information about the problem here._ 32 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/documentation-comment.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Documentation comment 3 | about: Comments on missing/incorrect/unclear parts of the Documentation 4 | title: "[Doc]" 5 | labels: Documentation 6 | assignees: '' 7 | 8 | --- 9 | 10 | **What part is this issue about?** 11 | _A link or description of what part of the Documentation you are refering to_ 12 | 13 | **What is the issue?** 14 | _A description of what you are unhappy with_ 15 | 16 | **Proposed wording** 17 | _If you have an idea of what the Documentation should say instead, put it here_ 18 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: "[Request]" 5 | labels: Suggestion 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | _A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]_ 12 | 13 | **Describe the solution you'd like** 14 | _A clear and concise description of what you want to happen._ 15 | 16 | **Describe alternatives you've considered** 17 | _A clear and concise description of any alternative solutions or features you've considered._ 18 | 19 | **Additional info** 20 | _Add any other information or screenshots about the feature request here._ 21 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: Tests 2 | 3 | on: 4 | push: { branches: [ master ] } 5 | pull_request: { branches: [ master ] } 6 | 7 | env: 8 | CARGO_TERM_COLOR: always 9 | 10 | jobs: 11 | stable: 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v3 15 | - uses: Swatinem/rust-cache@v2 16 | - uses: actions-rs/toolchain@v1.0.6 17 | with: { toolchain: stable } 18 | - name: Run tests 19 | run: cargo test 20 | - name: Run tests without features 21 | run: cargo test --no-default-features 22 | - name: Run feature tests for `parallel` 23 | run: cargo test --no-default-features --features parallel 24 | - name: Run feature tests for `log` 25 | run: cargo test --no-default-features --features log 26 | 27 | nightly: 28 | runs-on: ubuntu-latest 29 | steps: 30 | - uses: actions/checkout@v3 31 | - uses: actions-rs/toolchain@v1.0.6 32 | with: { toolchain: nightly } 33 | - name: Run tests 34 | run: cargo test 35 | - name: Run tests without features 36 | run: cargo test --no-default-features 37 | - name: Run feature tests for `parallel` 38 | run: cargo test --no-default-features --features parallel 39 | - name: Run feature tests for `log` 40 | run: cargo test --no-default-features --features log 41 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | **/*.rs.bk 2 | /target 3 | /Cargo.lock 4 | /benchmark/target 5 | /benchmark/Cargo.lock 6 | /test_dirs/ 7 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "cSpell.words": [ 3 | "hashbrown", 4 | "Hasher", 5 | "Neumann" 6 | ], 7 | } -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "hierarchical_pathfinding" 3 | version = "0.5.1" 4 | authors = ["mich101mich "] 5 | edition = "2021" 6 | description = "Quickly approximate Paths on a Grid" 7 | repository = "https://github.com/mich101mich/hierarchical_pathfinding" 8 | readme = "readme.md" 9 | license = "MIT" 10 | keywords = ["pathfinding", "dijkstra", "a-star", "grid"] 11 | categories = ["algorithms"] 12 | exclude = [ 13 | "/.github/*", 14 | "/.vscode/*", 15 | "/img/*", 16 | "/.gitignore", 17 | ] 18 | 19 | [dependencies] 20 | hashbrown = "0.14.0" 21 | log = { version = "0.4.0", optional = true } # Feature used for measuring internal timings. Recommended to leave this off unless working on improvements to hierarchical_pathfinding. 22 | rayon = { version = "1.8.0", optional = true } # don't set this directly, use feature `parallel` instead. 23 | slab = "0.4.0" 24 | 25 | [dev-dependencies] 26 | criterion = "0.5.0" 27 | env_logger = "0.10.0" 28 | log = "0.4.0" 29 | nanorand = "0.7.0" 30 | 31 | # fixes for minimal-versions 32 | serde = "1.0.100" # dependency of criterion: 'serde = "1.0"', but 1.0.0 to 1.0.99 stopped working 33 | 34 | [features] 35 | default = ["parallel"] 36 | parallel = ["rayon", "hashbrown/rayon"] 37 | 38 | [[bench]] 39 | name = "my_benchmark" 40 | harness = false 41 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Malte Hillmann 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /benches/my_benchmark.rs: -------------------------------------------------------------------------------- 1 | extern crate hierarchical_pathfinding; 2 | use env_logger::Env; 3 | 4 | use criterion::{criterion_group, criterion_main, BatchSize, Criterion}; 5 | 6 | use hierarchical_pathfinding::prelude::*; 7 | use log::warn; 8 | 9 | #[derive(Copy, Clone, Debug)] 10 | pub struct Tile { 11 | cost: isize, 12 | } 13 | 14 | #[derive(Clone)] 15 | struct Map { 16 | tiles: Vec, 17 | width: usize, 18 | height: usize, 19 | } 20 | 21 | impl Map { 22 | pub fn new(width: usize, height: usize) -> Self { 23 | let tile_count = width * height; 24 | Map { 25 | tiles: vec![Tile { cost: 1 }; tile_count], 26 | width, 27 | height, 28 | } 29 | } 30 | 31 | pub fn new_random(width: usize, height: usize) -> Self { 32 | use nanorand::{Rng, WyRand}; 33 | 34 | let tile_count = width * height; 35 | let mut tiles = Vec::with_capacity(tile_count); 36 | let mut rng = WyRand::new_seed(4); 37 | for _ in 0..tile_count { 38 | tiles.push(Tile { 39 | cost: rng.generate_range(-1_isize..8), 40 | }); 41 | } 42 | Map { 43 | tiles, 44 | width, 45 | height, 46 | } 47 | } 48 | 49 | #[allow(unused)] 50 | pub fn set_cost(&mut self, x: usize, y: usize, cost: isize) { 51 | let pos = self.get_tile_index(x, y); 52 | if let Some(pos) = pos { 53 | self.tiles[pos].cost = cost; 54 | } 55 | } 56 | 57 | fn get_tile_cost(&self, x: usize, y: usize) -> isize { 58 | let index = self.get_tile_index(x, y).unwrap(); 59 | self.tiles[index].cost 60 | } 61 | 62 | fn get_tile_index(&self, x: usize, y: usize) -> Option { 63 | if x >= self.width || y >= self.height { 64 | // Index out of bounds 65 | return None; 66 | } 67 | 68 | Some(x + y * self.width) 69 | } 70 | 71 | fn cost_fn(&self) -> impl '_ + Fn((usize, usize)) -> isize { 72 | move |(x, y)| self.get_tile_cost(x, y) 73 | } 74 | } 75 | 76 | #[allow(unused)] 77 | // Setup logging output 78 | fn init() { 79 | let env = Env::default() 80 | .filter_or("MY_LOG_LEVEL", "trace") // Change this from debug to trace to enable more in-depth timings. 81 | .write_style_or("MY_LOG_STYLE", "auto"); 82 | 83 | env_logger::init_from_env(env); 84 | let _ = env_logger::builder().is_test(true).try_init(); 85 | } 86 | 87 | fn bench_create_pathcache(c: &mut Criterion) { 88 | let mut group = c.benchmark_group("Create PathCache"); 89 | group.sample_size(10); 90 | 91 | // Log to stdout 92 | init(); 93 | 94 | let chunk_sizes = [32]; 95 | let map_sizes = [128, 1024]; 96 | 97 | for map_size in map_sizes { 98 | let (width, height) = (map_size, map_size); 99 | let map = Map::new(width, height); 100 | 101 | for chunk_size in chunk_sizes { 102 | #[cfg(feature = "parallel")] 103 | { 104 | let id = format!( 105 | "Create cache, Uniform map, Parallel, Map Size: ({}, {}), Cache Size: {}", 106 | width, height, chunk_size 107 | ); 108 | group.bench_function(&id, |b| { 109 | b.iter(|| { 110 | PathCache::new( 111 | (width, height), 112 | map.cost_fn(), 113 | MooreNeighborhood::new(width, height), // 114 | PathCacheConfig::with_chunk_size(chunk_size), 115 | ) 116 | }) 117 | }); 118 | } 119 | 120 | let id = format!( 121 | "Create cache, Uniform map, Single Threaded, Map Size: ({}, {}), Cache Size: {}", 122 | width, height, chunk_size 123 | ); 124 | group.bench_function(&id, |b| { 125 | b.iter(|| { 126 | PathCache::new_with_fn_mut( 127 | (width, height), 128 | map.cost_fn(), 129 | MooreNeighborhood::new(width, height), 130 | PathCacheConfig::with_chunk_size(chunk_size), 131 | ) 132 | }) 133 | }); 134 | } 135 | } 136 | 137 | // Large random map 138 | let (width, height) = (1024, 1024); 139 | let map = Map::new_random(width, height); 140 | let chunk_size = 32; 141 | 142 | #[cfg(target_os = "windows")] 143 | warn!("For some reason, the Create PathCache/Large Random Map... benchmark runs significantly slower on Windows than when running the same code in it's own binary."); 144 | #[cfg(feature = "parallel")] 145 | { 146 | let id = format!( 147 | "Create cache, Large Random Map, Parallel, Map Size: ({}, {}), Cache Size: {}", 148 | width, height, chunk_size 149 | ); 150 | group.bench_function(&id, |b| { 151 | b.iter(|| { 152 | PathCache::new( 153 | (width, height), 154 | map.cost_fn(), 155 | MooreNeighborhood::new(width, height), 156 | PathCacheConfig::with_chunk_size(chunk_size), 157 | ) 158 | }) 159 | }); 160 | } 161 | let id = format!( 162 | "Create cache, Large Random Map, Single Threaded, Map Size: ({}, {}), Cache Size: {}", 163 | width, height, chunk_size 164 | ); 165 | group.bench_function(&id, |b| { 166 | b.iter(|| { 167 | PathCache::new_with_fn_mut( 168 | (width, height), 169 | map.cost_fn(), 170 | ManhattanNeighborhood::new(width, height), 171 | PathCacheConfig::with_chunk_size(chunk_size), 172 | ) 173 | }) 174 | }); 175 | } 176 | 177 | fn bench_update_pathcache(c: &mut Criterion) { 178 | let mut group = c.benchmark_group("Update PathCache"); 179 | group.measurement_time(std::time::Duration::from_secs(60)); 180 | // init(); 181 | 182 | // Create our map 183 | let (width, height) = (1024, 1024); 184 | let mut map = Map::new_random(width, height); 185 | let chunk_size = 32; 186 | let pathcache = PathCache::new( 187 | (width, height), 188 | map.cost_fn(), 189 | MooreNeighborhood::new(width, height), 190 | PathCacheConfig::with_chunk_size(chunk_size), 191 | ); 192 | 193 | // Put a solid wall across our map 194 | let mut changed = Vec::with_capacity(width); 195 | for x in 0..width { 196 | map.set_cost(x, 8, -1); 197 | changed.push((x, 8)); 198 | } 199 | #[cfg(feature = "parallel")] 200 | { 201 | let id = format!( 202 | "Update cache, Large Random Map, Parallel, Map Size: ({}, {}), Cache Size: {}", 203 | width, height, chunk_size 204 | ); 205 | group.bench_function(&id, |b| { 206 | // clone on every iteration, so we aren't updaing the same pathcache twice. 207 | b.iter_batched_ref( 208 | || pathcache.clone(), 209 | |cache| cache.tiles_changed(&changed, map.cost_fn()), 210 | BatchSize::SmallInput, 211 | ) 212 | }); 213 | } 214 | 215 | group.sample_size(40); 216 | 217 | let id = format!( 218 | "Update cache, Large Random Map, Single Threaded, Map Size: ({}, {}), Cache Size: {}", 219 | width, height, chunk_size 220 | ); 221 | group.bench_function(&id, |b| { 222 | b.iter_batched_ref( 223 | || pathcache.clone(), 224 | |cache| cache.tiles_changed_with_fn_mut(&changed, map.cost_fn()), 225 | BatchSize::SmallInput, 226 | ) 227 | }); 228 | } 229 | 230 | fn bench_get_path(c: &mut Criterion) { 231 | let mut group = c.benchmark_group("Get Path"); 232 | 233 | for (size, iterations, name, start, goal) in [ 234 | (128, 100, "Medium", (0, 0), (127, 127)), 235 | (256, 50, "Medium+", (1, 1), (200, 250)), 236 | (512, 20, "Medium++", (20, 30), (500, 400)), 237 | (1024, 10, "Large", (40, 90), (900, 600)), 238 | ] { 239 | group.sample_size(iterations); 240 | 241 | // uniform map 242 | let map = Map::new(size, size); 243 | let neighborhood = MooreNeighborhood::new(size, size); 244 | let chunk_size = 32; 245 | let pathcache = PathCache::new( 246 | (size, size), 247 | map.cost_fn(), 248 | neighborhood, 249 | PathCacheConfig::with_chunk_size(chunk_size), 250 | ); 251 | let id = format!( 252 | "Get Single Path, {} Uniform Map, Map Size: ({}, {}), Cache Size: {}", 253 | name, size, size, chunk_size 254 | ); 255 | #[cfg(feature = "log")] 256 | { 257 | log::trace!(""); 258 | log::trace!("{}", id); 259 | log::trace!(""); 260 | } 261 | group.bench_function(&id, |b| { 262 | b.iter(|| pathcache.find_path(start, goal, map.cost_fn())) 263 | }); 264 | 265 | // a_star comparison 266 | let id = format!( 267 | "Get Single Path A*, {} Uniform Map, Map Size: ({}, {})", 268 | name, size, size 269 | ); 270 | group.bench_function(&id, |b| { 271 | b.iter(|| a_star_search(&neighborhood, |_| true, map.cost_fn(), start, goal)) 272 | }); 273 | 274 | // random map 275 | let map = Map::new_random(size, size); 276 | let pathcache = PathCache::new( 277 | (size, size), 278 | map.cost_fn(), 279 | neighborhood, 280 | PathCacheConfig::with_chunk_size(chunk_size), 281 | ); 282 | let id = format!( 283 | "Get Single Path, {} Random Map, Map Size: ({}, {}), Cache Size: {}", 284 | name, size, size, chunk_size 285 | ); 286 | #[cfg(feature = "log")] 287 | { 288 | log::trace!(""); 289 | log::trace!("{}", id); 290 | log::trace!(""); 291 | } 292 | group.bench_function(&id, |b| { 293 | b.iter(|| pathcache.find_path(start, goal, map.cost_fn())) 294 | }); 295 | 296 | // a_star comparison 297 | let id = format!( 298 | "Get Single Path A*, {} Random Map, Map Size: ({}, {})", 299 | name, size, size 300 | ); 301 | group.bench_function(&id, |b| { 302 | b.iter(|| a_star_search(&neighborhood, |_| true, map.cost_fn(), start, goal)) 303 | }); 304 | } 305 | } 306 | 307 | criterion_group!( 308 | benches, 309 | bench_create_pathcache, 310 | bench_update_pathcache, 311 | bench_get_path 312 | ); 313 | criterion_main!(benches); 314 | 315 | use std::cmp::Ordering; 316 | use std::collections::BinaryHeap; 317 | type Point = (usize, usize); 318 | 319 | pub fn a_star_search( 320 | neighborhood: &N, 321 | mut valid: impl FnMut(Point) -> bool, 322 | mut get_cost: impl FnMut(Point) -> isize, 323 | start: Point, 324 | goal: Point, 325 | ) -> Option> { 326 | if get_cost(start) < 0 { 327 | return None; 328 | } 329 | if start == goal { 330 | return Some([start, start].into()); 331 | } 332 | let mut visited = hashbrown::HashMap::<_, _>::default(); 333 | let mut next = BinaryHeap::new(); 334 | next.push(HeuristicElement(start, 0, 0)); 335 | visited.insert(start, (0, start)); 336 | 337 | let mut all_neighbors = vec![]; 338 | 339 | while let Some(HeuristicElement(current_id, current_cost, _)) = next.pop() { 340 | if current_id == goal { 341 | break; 342 | } 343 | match current_cost.cmp(&visited[¤t_id].0) { 344 | Ordering::Greater => continue, 345 | Ordering::Equal => {} 346 | Ordering::Less => panic!("Binary Heap failed"), 347 | } 348 | 349 | let delta_cost = get_cost(current_id); 350 | if delta_cost < 0 { 351 | continue; 352 | } 353 | let other_cost = current_cost + delta_cost as usize; 354 | 355 | all_neighbors.clear(); 356 | neighborhood.get_all_neighbors(current_id, &mut all_neighbors); 357 | for &other_id in all_neighbors.iter() { 358 | if !valid(other_id) { 359 | continue; 360 | } 361 | if get_cost(other_id) < 0 && other_id != goal { 362 | continue; 363 | } 364 | 365 | let mut needs_visit = true; 366 | if let Some((prev_cost, prev_id)) = visited.get_mut(&other_id) { 367 | if *prev_cost > other_cost { 368 | *prev_cost = other_cost; 369 | *prev_id = current_id; 370 | } else { 371 | needs_visit = false; 372 | } 373 | } else { 374 | visited.insert(other_id, (other_cost, current_id)); 375 | } 376 | 377 | if needs_visit { 378 | let heuristic = neighborhood.heuristic(other_id, goal); 379 | next.push(HeuristicElement( 380 | other_id, 381 | other_cost, 382 | other_cost + heuristic, 383 | )); 384 | } 385 | } 386 | } 387 | 388 | if !visited.contains_key(&goal) { 389 | return None; 390 | } 391 | 392 | let steps = { 393 | let mut steps = vec![]; 394 | let mut current = goal; 395 | 396 | while current != start { 397 | steps.push(current); 398 | let (_, prev) = visited[¤t]; 399 | current = prev; 400 | } 401 | steps.push(start); 402 | steps.reverse(); 403 | steps 404 | }; 405 | 406 | Some(steps) 407 | } 408 | 409 | type Cost = usize; 410 | #[derive(PartialEq, Eq)] 411 | pub struct HeuristicElement(pub Id, pub Cost, pub Cost); 412 | impl PartialOrd for HeuristicElement { 413 | fn partial_cmp(&self, rhs: &Self) -> Option { 414 | Some(self.cmp(rhs)) 415 | } 416 | } 417 | impl Ord for HeuristicElement { 418 | fn cmp(&self, rhs: &Self) -> Ordering { 419 | rhs.2.cmp(&self.2) 420 | } 421 | } 422 | -------------------------------------------------------------------------------- /img/a_star.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mich101mich/hierarchical_pathfinding/51d1089d9fb0be99c6b97937b4b1dc80a9a667bd/img/a_star.png -------------------------------------------------------------------------------- /img/hpa.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mich101mich/hierarchical_pathfinding/51d1089d9fb0be99c6b97937b4b1dc80a9a667bd/img/hpa.png -------------------------------------------------------------------------------- /img/hpa_solution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mich101mich/hierarchical_pathfinding/51d1089d9fb0be99c6b97937b4b1dc80a9a667bd/img/hpa_solution.png -------------------------------------------------------------------------------- /img/plot/empty1024.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mich101mich/hierarchical_pathfinding/51d1089d9fb0be99c6b97937b4b1dc80a9a667bd/img/plot/empty1024.png -------------------------------------------------------------------------------- /img/plot/empty128.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mich101mich/hierarchical_pathfinding/51d1089d9fb0be99c6b97937b4b1dc80a9a667bd/img/plot/empty128.png -------------------------------------------------------------------------------- /img/plot/random1024.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mich101mich/hierarchical_pathfinding/51d1089d9fb0be99c6b97937b4b1dc80a9a667bd/img/plot/random1024.png -------------------------------------------------------------------------------- /img/plot/random128.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mich101mich/hierarchical_pathfinding/51d1089d9fb0be99c6b97937b4b1dc80a9a667bd/img/plot/random128.png -------------------------------------------------------------------------------- /img/plot/snake1024.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mich101mich/hierarchical_pathfinding/51d1089d9fb0be99c6b97937b4b1dc80a9a667bd/img/plot/snake1024.png -------------------------------------------------------------------------------- /img/plot/snake128.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mich101mich/hierarchical_pathfinding/51d1089d9fb0be99c6b97937b4b1dc80a9a667bd/img/plot/snake128.png -------------------------------------------------------------------------------- /img/problem.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mich101mich/hierarchical_pathfinding/51d1089d9fb0be99c6b97937b4b1dc80a9a667bd/img/problem.png -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | 2 | # Hierarchical Pathfinding 3 | 4 | A Rust crate to find Paths on a Grid using HPA* (Hierarchical Pathfinding A*) and Hierarchical Dijkstra. 5 | 6 | [![Tests](https://github.com/mich101mich/hierarchical_pathfinding/actions/workflows/test.yml/badge.svg)](https://github.com/mich101mich/hierarchical_pathfinding/actions/workflows/test.yml) 7 | 8 | ## Description 9 | Provides a fast algorithm for finding Paths on a Grid-like structure by caching segments of Paths to form a Node Graph. Finding a Path in that Graph is a lot faster than on the Grid itself, but results in Paths that are _slightly_ worse than the optimal Path. 10 | 11 | Implementation based on the Paper ["Near Optimal Hierarchical Path-Finding"](https://www.researchgate.net/profile/Adi-Botea/publication/228785110_Near_optimal_hierarchical_path-finding_HPA/links/09e41508fc2fed9a72000000/Near-optimal-hierarchical-path-finding-HPA.pdf). 12 | 13 | ### Use Case 14 | A 2D Grid with 15 | - Constant/maximum size 16 | - Every Tile has a cost of traversing that Tile (cost is per Tile and _not_ per Edge between Tiles) 17 | - Changes are rare or locally contained (or at least much rarer than Path calculations). 18 | 19 | ### Advantages 20 | - Finding a Path is a lot faster compared to regular algorithms (A*, Dijkstra) 21 | - It is always correct: A Path is found **if and only if** it exists 22 | - This means that Hierarchical Pathfinding can be used as Heuristic to check if a Path exists and how long it will roughly be (upper bound) 23 | 24 | ### Disadvantages 25 | - Paths are slightly worse than optimal (negligible in most cases) 26 | - Creating the cache takes time (only happens once at the start) 27 | - Changes to the Grid require updating the cache 28 | - Whenever a Tile within a Chunk changes, that entire Chunk needs to recalculate its Paths. Performance depends on Chunk size (configurable) and the number of Nodes in a Chunk 29 | 30 | ## Explanation 31 | 32 | Finding Paths on a Grid is an expensive Operation. Consider the following Setup: 33 | 34 | ![The Setup](https://github.com/mich101mich/hierarchical_pathfinding/blob/master/img/problem.png?raw=true) 35 | 36 | In order to calculate a Path from Start to End using regular A*, it is necessary to check a 37 | lot of Tiles: 38 | 39 | ![A*](https://github.com/mich101mich/hierarchical_pathfinding/blob/master/img/a_star.png?raw=true) 40 | 41 | (This is simply a small example, longer Paths require a quadratic increase in Tile checks, 42 | and unreachable Goals require the check of _**every single**_ Tile) 43 | 44 | The Solution that Hierarchical Pathfinding provides is to divide the Grid into Chunks and 45 | cache the Paths between Chunk entrances as a Graph of Nodes: 46 | 47 | ![The Graph](https://github.com/mich101mich/hierarchical_pathfinding/blob/master/img/hpa.png?raw=true) 48 | 49 | This allows Paths to be generated by connecting the Start and End to the Nodes within the 50 | Chunk and using the Graph for the rest: 51 | 52 | ![The Solution](https://github.com/mich101mich/hierarchical_pathfinding/blob/master/img/hpa_solution.png?raw=true) 53 | 54 | ## Example 55 | 56 | ```rust 57 | use hierarchical_pathfinding::prelude::*; 58 | 59 | let mut pathfinding = PathCache::new( 60 | (width, height), // the size of the Grid 61 | |(x, y)| walking_cost(x, y), // get the cost for walking over a Tile 62 | ManhattanNeighborhood::new(width, height), // the connection between Tiles 63 | PathCacheConfig::with_chunk_size(3), // additional config params 64 | ); 65 | 66 | let start = (0, 0); 67 | let goal = (4, 4); 68 | 69 | // find_path returns Some(Path) on success 70 | let path = pathfinding.find_path( 71 | start, 72 | goal, 73 | |(x, y)| walking_cost(x, y), 74 | ); 75 | 76 | if let Some(path) = path { 77 | println!("Number of steps: {}", path.length()); 78 | println!("Total Cost: {}", path.cost()); 79 | for (x, y) in path { 80 | println!("Go to {}, {}", x, y); 81 | } 82 | } 83 | ``` 84 | -------------------------------------------------------------------------------- /src/graph/a_star.rs: -------------------------------------------------------------------------------- 1 | use super::*; 2 | use crate::neighbors::Neighborhood; 3 | 4 | use std::cmp::Ordering; 5 | use std::collections::BinaryHeap; 6 | 7 | pub(crate) fn a_star_search( 8 | nodes: &NodeList, 9 | start: NodeID, 10 | goal: NodeID, 11 | neighborhood: &N, 12 | size_hint: usize, 13 | ) -> Option> { 14 | if start == goal { 15 | return Some(Path::from_slice(&[start, start], 0)); 16 | } 17 | let mut visited = NodeIDMap::with_capacity(size_hint); 18 | let mut next = BinaryHeap::with_capacity(size_hint / 2); 19 | next.push(HeuristicElement(start, 0, 0)); 20 | visited.insert(start, (0, start)); 21 | 22 | while let Some(HeuristicElement(current_id, current_cost, _)) = next.pop() { 23 | if current_id == goal { 24 | break; 25 | } 26 | match current_cost.cmp(&visited[¤t_id].0) { 27 | Ordering::Greater => continue, 28 | Ordering::Equal => {} 29 | Ordering::Less => panic!("Binary Heap failed"), 30 | } 31 | 32 | let current = &nodes[current_id]; 33 | 34 | for (&other_id, path) in current.edges.iter() { 35 | let other_cost = current_cost + path.cost(); 36 | let other = &nodes[other_id]; 37 | 38 | let mut needs_visit = true; 39 | if let Some((prev_cost, prev_id)) = visited.get_mut(&other_id) { 40 | if *prev_cost > other_cost { 41 | *prev_cost = other_cost; 42 | *prev_id = current_id; 43 | } else { 44 | needs_visit = false; 45 | } 46 | } else { 47 | visited.insert(other_id, (other_cost, current_id)); 48 | } 49 | 50 | if needs_visit { 51 | let heuristic = neighborhood.heuristic(current.pos, other.pos); 52 | next.push(HeuristicElement( 53 | other_id, 54 | other_cost, 55 | other_cost + heuristic, 56 | )); 57 | } 58 | } 59 | } 60 | 61 | if !visited.contains_key(&goal) { 62 | return None; 63 | } 64 | 65 | let steps = { 66 | let mut steps = vec![]; 67 | let mut current = goal; 68 | 69 | while current != start { 70 | steps.push(current); 71 | let (_, prev) = visited[¤t]; 72 | current = prev; 73 | } 74 | steps.push(start); 75 | steps.reverse(); 76 | steps 77 | }; 78 | 79 | Some(Path::new(steps, visited[&goal].0)) 80 | } 81 | -------------------------------------------------------------------------------- /src/graph/dijkstra.rs: -------------------------------------------------------------------------------- 1 | use super::*; 2 | 3 | use std::cmp::Ordering; 4 | use std::collections::BinaryHeap; 5 | 6 | pub(crate) fn dijkstra_search( 7 | nodes: &NodeList, 8 | start: NodeID, 9 | goals: &[NodeID], 10 | only_closest_goal: bool, 11 | size_hint: usize, 12 | ) -> NodeIDMap> { 13 | let mut visited = NodeIDMap::with_capacity(size_hint); 14 | let mut next = BinaryHeap::with_capacity(size_hint / 2); 15 | next.push(Element(start, 0)); 16 | visited.insert(start, (0, start)); 17 | 18 | let mut remaining_goals: NodeIDSet = goals.iter().copied().collect(); 19 | 20 | let mut goal_costs = NodeIDMap::with_capacity(goals.len()); 21 | 22 | while let Some(Element(current_id, current_cost)) = next.pop() { 23 | match current_cost.cmp(&visited[¤t_id].0) { 24 | Ordering::Greater => continue, 25 | Ordering::Equal => {} 26 | Ordering::Less => panic!("Binary Heap failed"), 27 | } 28 | 29 | if remaining_goals.remove(¤t_id) { 30 | goal_costs.insert(current_id, current_cost); 31 | if only_closest_goal || remaining_goals.is_empty() { 32 | break; 33 | } 34 | } 35 | 36 | let current = &nodes[current_id]; 37 | 38 | for (&other_id, path) in current.edges.iter() { 39 | let other_cost = current_cost + path.cost(); 40 | 41 | let mut needs_visit = true; 42 | if let Some((prev_cost, prev_id)) = visited.get_mut(&other_id) { 43 | if *prev_cost > other_cost { 44 | *prev_cost = other_cost; 45 | *prev_id = current_id; 46 | } else { 47 | needs_visit = false; 48 | } 49 | } else { 50 | visited.insert(other_id, (other_cost, current_id)); 51 | } 52 | 53 | if needs_visit { 54 | next.push(Element(other_id, other_cost)); 55 | } 56 | } 57 | } 58 | 59 | let mut goal_data = NodeIDMap::with_capacity_and_hasher(goal_costs.len(), Default::default()); 60 | 61 | for (&goal, &cost) in goal_costs.iter() { 62 | let steps = { 63 | let mut steps = vec![]; 64 | let mut current = goal; 65 | 66 | while current != start { 67 | steps.push(current); 68 | let (_, prev) = visited[¤t]; 69 | current = prev; 70 | } 71 | steps.push(start); 72 | steps.reverse(); 73 | steps 74 | }; 75 | goal_data.insert(goal, Path::new(steps, cost)); 76 | } 77 | 78 | goal_data 79 | } 80 | -------------------------------------------------------------------------------- /src/graph/mod.rs: -------------------------------------------------------------------------------- 1 | mod node_list; 2 | pub(crate) use node_list::NodeList; 3 | 4 | mod node; 5 | pub(crate) use node::Node; 6 | 7 | mod a_star; 8 | pub(crate) use a_star::a_star_search; 9 | 10 | mod dijkstra; 11 | pub(crate) use dijkstra::dijkstra_search; 12 | 13 | use crate::grid::{Element, HeuristicElement}; 14 | use crate::path::Path; 15 | use crate::{NodeID, NodeIDMap, NodeIDSet}; 16 | -------------------------------------------------------------------------------- /src/graph/node.rs: -------------------------------------------------------------------------------- 1 | use crate::{path::PathSegment, NodeIDMap, Point}; 2 | 3 | #[derive(Clone, Debug)] 4 | pub(crate) struct Node { 5 | pub pos: Point, 6 | pub walk_cost: usize, 7 | pub edges: NodeIDMap, 8 | } 9 | 10 | impl Node { 11 | pub fn new(pos: Point, walk_cost: usize) -> Node { 12 | Node { 13 | pos, 14 | walk_cost, 15 | edges: NodeIDMap::default(), 16 | } 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /src/graph/node_list.rs: -------------------------------------------------------------------------------- 1 | use super::{Node, NodeID, NodeIDMap, NodeIDSet}; 2 | use crate::{path::PathSegment, Point, PointMap}; 3 | 4 | #[derive(Clone, Debug)] 5 | pub(crate) struct NodeList { 6 | nodes: slab::Slab, 7 | pos_map: PointMap, 8 | } 9 | 10 | impl NodeList { 11 | pub fn new() -> Self { 12 | Self { 13 | nodes: slab::Slab::default(), 14 | pos_map: PointMap::default(), 15 | } 16 | } 17 | 18 | pub fn len(&self) -> usize { 19 | self.pos_map.len() 20 | } 21 | 22 | pub fn add_node(&mut self, pos: Point, walk_cost: usize) -> NodeID { 23 | let id = self.nodes.insert(Node::new(pos, walk_cost)); 24 | self.pos_map.insert(pos, id); 25 | id 26 | } 27 | 28 | pub fn add_edge(&mut self, src: NodeID, target: NodeID, path: PathSegment) { 29 | let src_node = &self[src]; 30 | if let Some(existing) = src_node.edges.get(&target) { 31 | if existing.cost() == path.cost() { 32 | return; 33 | } 34 | } 35 | let src_cost = src_node.walk_cost; 36 | 37 | let target_node = &mut self[target]; 38 | 39 | let target_cost = target_node.walk_cost; 40 | 41 | let other_path = path.reversed(src_cost, target_cost); 42 | target_node.edges.insert(src, other_path); 43 | 44 | let src_node = &mut self[src]; 45 | src_node.edges.insert(target, path); 46 | } 47 | 48 | #[track_caller] 49 | pub fn remove_node(&mut self, id: NodeID) { 50 | let node = self.nodes.remove(id); 51 | for (other_id, _) in node.edges { 52 | self[other_id].edges.remove(&id); 53 | } 54 | self.pos_map.remove(&node.pos); 55 | } 56 | 57 | pub fn iter(&self) -> slab::Iter { 58 | self.nodes.iter() 59 | } 60 | 61 | pub fn id_at(&self, pos: Point) -> Option { 62 | self.pos_map.get(&pos).copied() 63 | } 64 | 65 | pub fn absorb(&mut self, other: NodeList) -> NodeIDSet { 66 | let mut ret = NodeIDSet::default(); 67 | let mut map = NodeIDMap::default(); 68 | 69 | for (old, node) in other.nodes.iter() { 70 | let new = self.add_node(node.pos, node.walk_cost); 71 | map.insert(old, new); 72 | ret.insert(new); 73 | } 74 | 75 | for (id, old_node) in other.nodes { 76 | let new_node = &mut self[map[&id]]; 77 | new_node.edges = old_node 78 | .edges 79 | .into_iter() 80 | .map(|(other_id, path)| (map[&other_id], path)) 81 | .collect(); 82 | } 83 | 84 | ret 85 | } 86 | } 87 | 88 | use std::ops::{Index, IndexMut}; 89 | impl Index for NodeList { 90 | type Output = Node; 91 | #[track_caller] 92 | fn index(&self, index: NodeID) -> &Node { 93 | &self.nodes[index] 94 | } 95 | } 96 | impl IndexMut for NodeList { 97 | #[track_caller] 98 | fn index_mut(&mut self, index: NodeID) -> &mut Node { 99 | &mut self.nodes[index] 100 | } 101 | } 102 | 103 | #[test] 104 | fn absorb() { 105 | let mut nodes = NodeList::new(); 106 | let zero_id = nodes.add_node((0, 0), 0); 107 | let one_id = nodes.add_node((1, 1), 1); 108 | let two_id = nodes.add_node((2, 2), 2); 109 | nodes.add_edge( 110 | zero_id, 111 | one_id, 112 | PathSegment::new(super::Path::from_slice(&[], 0), true), 113 | ); 114 | nodes.add_edge( 115 | two_id, 116 | zero_id, 117 | PathSegment::new(super::Path::from_slice(&[], 2), true), 118 | ); 119 | 120 | let mut new_nodes = NodeList::new(); 121 | let ten_id = new_nodes.add_node((10, 10), 10); 122 | let eleven_id = new_nodes.add_node((11, 11), 11); 123 | new_nodes.add_edge( 124 | ten_id, 125 | eleven_id, 126 | PathSegment::new(super::Path::from_slice(&[], 10), true), 127 | ); 128 | 129 | nodes.absorb(new_nodes); 130 | 131 | let new_ten_id = nodes.id_at((10, 10)).unwrap(); 132 | let new_eleven_id = nodes.id_at((11, 11)).unwrap(); 133 | 134 | assert_eq!(nodes.nodes.len(), 5); 135 | assert_eq!(nodes.nodes[new_ten_id].pos, (10, 10)); 136 | assert_eq!(nodes.nodes[new_eleven_id].pos, (11, 11)); 137 | assert_eq!(nodes.nodes[new_ten_id].edges[&new_eleven_id].cost(), 10); 138 | } 139 | -------------------------------------------------------------------------------- /src/grid/a_star.rs: -------------------------------------------------------------------------------- 1 | use super::{HeuristicElement, Path}; 2 | use crate::{neighbors::Neighborhood, Point, PointMap}; 3 | 4 | use std::cmp::Ordering; 5 | use std::collections::BinaryHeap; 6 | 7 | pub fn a_star_search( 8 | neighborhood: &N, 9 | mut valid: impl FnMut(Point) -> bool, 10 | mut get_cost: impl FnMut(Point) -> isize, 11 | start: Point, 12 | goal: Point, 13 | size_hint: usize, 14 | ) -> Option> { 15 | if get_cost(start) < 0 { 16 | return None; 17 | } 18 | if start == goal { 19 | return Some(Path::from_slice(&[start, start], 0)); 20 | } 21 | let mut visited = PointMap::with_capacity(size_hint); 22 | let mut next = BinaryHeap::with_capacity(size_hint / 2); 23 | next.push(HeuristicElement(start, 0, 0)); 24 | visited.insert(start, (0, start)); 25 | 26 | let mut all_neighbors = vec![]; 27 | 28 | while let Some(HeuristicElement(current_id, current_cost, _)) = next.pop() { 29 | if current_id == goal { 30 | break; 31 | } 32 | match current_cost.cmp(&visited[¤t_id].0) { 33 | Ordering::Greater => continue, 34 | Ordering::Equal => {} 35 | Ordering::Less => panic!("Binary Heap failed"), 36 | } 37 | 38 | let delta_cost = get_cost(current_id); 39 | if delta_cost < 0 { 40 | continue; 41 | } 42 | let other_cost = current_cost + delta_cost as usize; 43 | 44 | all_neighbors.clear(); 45 | neighborhood.get_all_neighbors(current_id, &mut all_neighbors); 46 | for &other_id in all_neighbors.iter() { 47 | if !valid(other_id) { 48 | continue; 49 | } 50 | if get_cost(other_id) < 0 && other_id != goal { 51 | continue; 52 | } 53 | 54 | let mut needs_visit = true; 55 | if let Some((prev_cost, prev_id)) = visited.get_mut(&other_id) { 56 | if *prev_cost > other_cost { 57 | *prev_cost = other_cost; 58 | *prev_id = current_id; 59 | } else { 60 | needs_visit = false; 61 | } 62 | } else { 63 | visited.insert(other_id, (other_cost, current_id)); 64 | } 65 | 66 | if needs_visit { 67 | let heuristic = neighborhood.heuristic(other_id, goal); 68 | next.push(HeuristicElement( 69 | other_id, 70 | other_cost, 71 | other_cost + heuristic, 72 | )); 73 | } 74 | } 75 | } 76 | 77 | if !visited.contains_key(&goal) { 78 | return None; 79 | } 80 | 81 | let steps = { 82 | let mut steps = vec![]; 83 | let mut current = goal; 84 | 85 | while current != start { 86 | steps.push(current); 87 | let (_, prev) = visited[¤t]; 88 | current = prev; 89 | } 90 | steps.push(start); 91 | steps.reverse(); 92 | steps 93 | }; 94 | 95 | Some(Path::new(steps, visited[&goal].0)) 96 | } 97 | 98 | #[cfg(test)] 99 | mod tests { 100 | use super::*; 101 | 102 | #[test] 103 | fn unreachable_goal() { 104 | use crate::prelude::*; 105 | 106 | // create and initialize Grid 107 | // 0 = empty, 1 = swamp, 2 = wall 108 | let grid = [ 109 | [0, 2, 0, 0, 0], 110 | [0, 2, 2, 2, 2], 111 | [0, 1, 0, 0, 0], 112 | [0, 1, 0, 2, 0], 113 | [0, 0, 0, 2, 0], 114 | ]; 115 | let (width, height) = (grid[0].len(), grid.len()); 116 | 117 | let neighborhood = ManhattanNeighborhood::new(width, height); 118 | 119 | const COST_MAP: [isize; 3] = [1, 10, -1]; 120 | 121 | fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + FnMut(Point) -> isize { 122 | move |(x, y)| COST_MAP[grid[y][x]] 123 | } 124 | 125 | let start = (0, 0); 126 | let goal = (2, 0); 127 | 128 | let path = a_star_search(&neighborhood, |_| true, cost_fn(&grid), start, goal, 40); 129 | 130 | assert!(path.is_none()); 131 | } 132 | 133 | #[test] 134 | fn basic() { 135 | use crate::prelude::*; 136 | 137 | // create and initialize Grid 138 | // 0 = empty, 1 = swamp, 2 = wall 139 | let grid = [ 140 | [0, 2, 0, 0, 0], 141 | [0, 2, 2, 2, 2], 142 | [0, 1, 0, 0, 0], 143 | [0, 1, 0, 2, 0], 144 | [0, 0, 0, 2, 0], 145 | ]; 146 | let (width, height) = (grid[0].len(), grid.len()); 147 | 148 | let neighborhood = ManhattanNeighborhood::new(width, height); 149 | 150 | const COST_MAP: [isize; 3] = [1, 10, -1]; 151 | 152 | fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + FnMut(Point) -> isize { 153 | move |(x, y)| COST_MAP[grid[y][x]] 154 | } 155 | 156 | let start = (0, 0); 157 | let goal = (4, 4); 158 | let path = a_star_search(&neighborhood, |_| true, cost_fn(&grid), start, goal, 40); 159 | 160 | assert!(path.is_some()); 161 | let path = path.unwrap(); 162 | 163 | assert_eq!(path.cost(), 12); 164 | } 165 | } 166 | -------------------------------------------------------------------------------- /src/grid/dijkstra.rs: -------------------------------------------------------------------------------- 1 | use super::{Element, Path}; 2 | use crate::{neighbors::Neighborhood, Point, PointMap, PointSet}; 3 | 4 | use std::cmp::Ordering; 5 | use std::collections::BinaryHeap; 6 | 7 | pub fn dijkstra_search( 8 | neighborhood: &N, 9 | mut valid: impl FnMut(Point) -> bool, 10 | mut get_cost: impl FnMut(Point) -> isize, 11 | start: Point, 12 | goals: &[Point], 13 | only_closest_goal: bool, 14 | size_hint: usize, 15 | ) -> PointMap> { 16 | if get_cost(start) < 0 { 17 | return PointMap::default(); 18 | } 19 | let mut visited = PointMap::with_capacity(size_hint); 20 | let mut next = BinaryHeap::with_capacity(size_hint / 2); 21 | next.push(Element(start, 0)); 22 | visited.insert(start, (0, start)); 23 | 24 | let mut remaining_goals: PointSet = goals.iter().copied().collect(); 25 | 26 | let mut goal_costs = PointMap::with_capacity(goals.len()); 27 | 28 | let mut all_neighbors = vec![]; 29 | 30 | while let Some(Element(current_id, current_cost)) = next.pop() { 31 | match current_cost.cmp(&visited[¤t_id].0) { 32 | Ordering::Greater => continue, 33 | Ordering::Equal => {} 34 | Ordering::Less => panic!("Binary Heap failed"), 35 | } 36 | 37 | if remaining_goals.remove(¤t_id) { 38 | goal_costs.insert(current_id, current_cost); 39 | if only_closest_goal || remaining_goals.is_empty() { 40 | break; 41 | } 42 | } 43 | 44 | let delta_cost = get_cost(current_id); 45 | if delta_cost < 0 { 46 | continue; 47 | } 48 | let other_cost = current_cost + delta_cost as usize; 49 | 50 | all_neighbors.clear(); 51 | neighborhood.get_all_neighbors(current_id, &mut all_neighbors); 52 | for &other_id in all_neighbors.iter() { 53 | if !valid(other_id) { 54 | continue; 55 | } 56 | if get_cost(other_id) < 0 && !remaining_goals.contains(&other_id) { 57 | continue; 58 | } 59 | 60 | let mut needs_visit = true; 61 | if let Some((prev_cost, prev_id)) = visited.get_mut(&other_id) { 62 | if *prev_cost > other_cost { 63 | *prev_cost = other_cost; 64 | *prev_id = current_id; 65 | } else { 66 | needs_visit = false; 67 | } 68 | } else { 69 | visited.insert(other_id, (other_cost, current_id)); 70 | } 71 | 72 | if needs_visit { 73 | next.push(Element(other_id, other_cost)); 74 | } 75 | } 76 | } 77 | 78 | let mut goal_data = PointMap::with_capacity_and_hasher(goal_costs.len(), Default::default()); 79 | 80 | for (&goal, &cost) in goal_costs.iter() { 81 | let steps = { 82 | let mut steps = vec![]; 83 | let mut current = goal; 84 | 85 | while current != start { 86 | steps.push(current); 87 | let (_, prev) = visited[¤t]; 88 | current = prev; 89 | } 90 | steps.push(start); 91 | steps.reverse(); 92 | steps 93 | }; 94 | goal_data.insert(goal, Path::new(steps, cost)); 95 | } 96 | 97 | goal_data 98 | } 99 | 100 | #[cfg(test)] 101 | mod tests { 102 | use super::*; 103 | 104 | #[test] 105 | fn basic() { 106 | use crate::prelude::*; 107 | 108 | // create and initialize Grid 109 | // 0 = empty, 1 = swamp, 2 = wall 110 | let grid = [ 111 | [0, 2, 0, 0, 0], 112 | [0, 2, 2, 2, 2], 113 | [0, 1, 0, 0, 0], 114 | [0, 1, 0, 2, 0], 115 | [0, 0, 0, 2, 0], 116 | ]; 117 | let (width, height) = (grid[0].len(), grid.len()); 118 | 119 | let neighborhood = ManhattanNeighborhood::new(width, height); 120 | 121 | const COST_MAP: [isize; 3] = [1, 10, -1]; 122 | 123 | fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + FnMut(Point) -> isize { 124 | move |(x, y)| COST_MAP[grid[y][x]] 125 | } 126 | 127 | let start = (0, 0); 128 | let goals = [(4, 4), (2, 0)]; 129 | 130 | let paths = dijkstra_search( 131 | &neighborhood, 132 | |_| true, 133 | cost_fn(&grid), 134 | start, 135 | &goals, 136 | false, 137 | 40, 138 | ); 139 | 140 | // (4, 4) is reachable 141 | assert!(paths.contains_key(&goals[0])); 142 | 143 | // (2, 0) is not reachable 144 | assert!(!paths.contains_key(&goals[1])); 145 | } 146 | } 147 | -------------------------------------------------------------------------------- /src/grid/mod.rs: -------------------------------------------------------------------------------- 1 | mod a_star; 2 | pub(crate) use a_star::a_star_search; 3 | 4 | mod dijkstra; 5 | pub(crate) use dijkstra::dijkstra_search; 6 | 7 | use crate::path::{Cost, Path}; 8 | 9 | use std::cmp::Ordering; 10 | 11 | #[derive(PartialEq, Eq)] 12 | pub(crate) struct HeuristicElement(pub Id, pub Cost, pub Cost); 13 | impl PartialOrd for HeuristicElement { 14 | fn partial_cmp(&self, rhs: &Self) -> Option { 15 | Some(self.cmp(rhs)) 16 | } 17 | } 18 | impl Ord for HeuristicElement { 19 | fn cmp(&self, rhs: &Self) -> Ordering { 20 | rhs.2.cmp(&self.2) 21 | } 22 | } 23 | 24 | #[derive(PartialEq, Eq)] 25 | pub(crate) struct Element(pub Id, pub Cost); 26 | impl PartialOrd for Element { 27 | fn partial_cmp(&self, rhs: &Self) -> Option { 28 | Some(self.cmp(rhs)) 29 | } 30 | } 31 | impl Ord for Element { 32 | fn cmp(&self, rhs: &Self) -> Ordering { 33 | rhs.1.cmp(&self.1) 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | #![deny( 2 | missing_docs, 3 | // missing_doc_code_examples, 4 | missing_debug_implementations, 5 | missing_copy_implementations, 6 | trivial_casts, 7 | trivial_numeric_casts, 8 | unsafe_code, 9 | unstable_features, 10 | unused_import_braces, 11 | unused_qualifications 12 | )] 13 | #![warn(clippy::pedantic)] 14 | #![allow(clippy::upper_case_acronyms)] 15 | 16 | //! A crate to quickly approximate Paths on a Grid. 17 | //! 18 | //! # Use Case 19 | //! 20 | //! Finding Paths on a Grid is an expensive Operation. Consider the following Setup: 21 | //! 22 | //! ![The Setup](https://github.com/mich101mich/hierarchical_pathfinding/blob/master/img/problem.png?raw=true) 23 | //! 24 | //! In order to calculate a Path from Start to End using regular A*, it is necessary to check a 25 | //! lot of Tiles: 26 | //! 27 | //! ![A*](https://github.com/mich101mich/hierarchical_pathfinding/blob/master/img/a_star.png?raw=true) 28 | //! 29 | //! (This is simply a small example, longer Paths require a quadratic increase in Tile checks, 30 | //! and unreachable Goals require the check of _**every single**_ Tile) 31 | //! 32 | //! The Solution that Hierarchical Pathfinding provides is to divide the Grid into Chunks and 33 | //! cache the Paths between Chunk entrances as a Graph of Nodes: 34 | //! 35 | //! ![The Graph](https://github.com/mich101mich/hierarchical_pathfinding/blob/master/img/hpa.png?raw=true) 36 | //! 37 | //! This allows Paths to be generated by connecting the Start and End to the Nodes within the 38 | //! Chunk and using the Graph for the rest: 39 | //! 40 | //! ![The Solution](https://github.com/mich101mich/hierarchical_pathfinding/blob/master/img/hpa_solution.png?raw=true) 41 | //! 42 | //! Since the Graph is not an exact representation of the Grid, **the resulting Paths will 43 | //! be slightly worse than the actual best Path** (unless [`config.perfect_paths`](PathCacheConfig::perfect_paths) 44 | //! is set to `true`). This is usually not a problem, since the purpose of Hierarchical Pathfinding 45 | //! is to quickly find the next direction to go in or a Heuristic for the Cost and existence 46 | //! of a Path. 47 | //! 48 | //! The only time where the actual best Path would noticeably differ is in the case of short Paths. 49 | //! That is why this crate calls the regular A* search after HPA* confirmed the length and 50 | //! existence. (This behavior can be turned of using the Config). 51 | //! 52 | //! This crate provides an implementation of a Hierarchical Pathfinding Algorithm for any generic Grid. 53 | //! Paths can be searched using either A* for a Path to a single Tile, or Dijkstra for searching 54 | //! multiple Targets. It handles solid walls in the Grid and actually finding a Path to a wall. 55 | //! 56 | //! # Examples 57 | //! ##### Creating the Cache 58 | //! First is the Grid itself. **How it is stored doesn't matter**, but lookup has to be fast. 59 | //! 60 | //! For this example, we shall use a 2D-Array: 61 | //! ```ignore 62 | //! // 0 = empty, 1 = swamp, 2 = wall 63 | //! let mut grid = [ 64 | //! [0, 2, 0, 0, 0], 65 | //! [0, 2, 2, 2, 0], 66 | //! [0, 1, 0, 0, 0], 67 | //! [0, 1, 0, 2, 0], 68 | //! [0, 0, 0, 2, 0], 69 | //! ]; 70 | //! let (width, height) = (grid[0].len(), grid.len()); 71 | //! 72 | //! let cost_map = [ 73 | //! 1, // empty 74 | //! 10, // swamp 75 | //! -1, // wall = solid 76 | //! ]; 77 | //! ``` 78 | //! Now for creating the [`PathCache`]: 79 | //! ``` 80 | //! # let mut grid = [ 81 | //! # [0, 2, 0, 0, 0], 82 | //! # [0, 2, 2, 2, 0], 83 | //! # [0, 1, 0, 0, 0], 84 | //! # [0, 1, 0, 2, 0], 85 | //! # [0, 0, 0, 2, 0], 86 | //! # ]; 87 | //! # let (width, height) = (grid[0].len(), grid.len()); 88 | //! # let cost_map = [ 89 | //! # 1, // empty 90 | //! # 10, // swamp 91 | //! # -1, // wall. Negative number == solid 92 | //! # ]; 93 | //! use hierarchical_pathfinding::prelude::*; 94 | //! 95 | //! let mut pathfinding = PathCache::new( 96 | //! (width, height), // the size of the Grid 97 | //! |(x, y)| cost_map[grid[y][x]], // get the cost for walking over a Tile 98 | //! ManhattanNeighborhood::new(width, height), // the Neighborhood 99 | //! PathCacheConfig::with_chunk_size(3), // config 100 | //! ); 101 | //! ``` 102 | //! The [`PathCache`] never takes the actual Grid, to allow for any storage format to be used 103 | //! (`Array`, `Vec`, `HashMap`, `kd-tree`, ...). Instead, it takes a callback function that 104 | //! indicates, how "expensive" walking across a Tile is (negative numbers for solid obstacles). 105 | //! 106 | //! Unfortunately, it is necessary to provide this function to every method of `PathCache`, since 107 | //! storing it would make the Grid immutable. See also [Updating the `PathCache`](#updating-the-pathcache). 108 | //! 109 | //! [Currying](https://en.wikipedia.org/wiki/Currying) can be used to reduce duplication: 110 | //! ``` 111 | //! # use hierarchical_pathfinding::prelude::*; 112 | //! # // 0 = empty, 1 = swamp, 2 = wall 113 | //! # let mut grid = [ 114 | //! # [0, 2, 0, 0, 0], 115 | //! # [0, 2, 2, 2, 0], 116 | //! # [0, 1, 0, 0, 0], 117 | //! # [0, 1, 0, 2, 0], 118 | //! # [0, 0, 0, 2, 0], 119 | //! # ]; 120 | //! # let (width, height) = (grid[0].len(), grid.len()); 121 | //! # type Grid = [[usize; 5]; 5]; 122 | //! const COST_MAP: [isize; 3] = [1, 10, -1]; // now const for ownership reasons 123 | //! 124 | //! // only borrows the Grid when called 125 | //! fn cost_fn(grid: &Grid) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 126 | //! move |(x, y)| COST_MAP[grid[y][x]] 127 | //! } 128 | //! 129 | //! let mut pathfinding = PathCache::new( 130 | //! (width, height), // the size of the Grid 131 | //! 132 | //! // simply call the creator function to take a reference of the Grid 133 | //! cost_fn(&grid), 134 | //! // ... 135 | //! # ManhattanNeighborhood::new(width, height), // the Neighborhood 136 | //! # PathCacheConfig::with_chunk_size(3), // config 137 | //! # ); 138 | //! ``` 139 | //! 140 | //! ##### Pathfinding 141 | //! Finding the Path to a single Goal: 142 | //! ``` 143 | //! # use hierarchical_pathfinding::prelude::*; 144 | //! # let mut grid = [ 145 | //! # [0, 2, 0, 0, 0], 146 | //! # [0, 2, 2, 2, 2], 147 | //! # [0, 1, 0, 0, 0], 148 | //! # [0, 1, 0, 2, 0], 149 | //! # [0, 0, 0, 2, 0], 150 | //! # ]; 151 | //! # let (width, height) = (grid[0].len(), grid.len()); 152 | //! # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 153 | //! # move |(x, y)| [1, 10, -1][grid[y][x]] 154 | //! # } 155 | //! # let mut pathfinding = PathCache::new( 156 | //! # (width, height), 157 | //! # cost_fn(&grid), 158 | //! # ManhattanNeighborhood::new(width, height), 159 | //! # PathCacheConfig::with_chunk_size(3), 160 | //! # ); 161 | //! # 162 | //! let start = (0, 0); 163 | //! let goal = (4, 4); 164 | //! 165 | //! // find_path returns Some(Path) on success 166 | //! let path = pathfinding.find_path( 167 | //! start, 168 | //! goal, 169 | //! cost_fn(&grid), 170 | //! ); 171 | //! 172 | //! assert!(path.is_some()); 173 | //! let path = path.unwrap(); 174 | //! 175 | //! assert_eq!(path.cost(), 12); 176 | //! ``` 177 | //! For more information, see [`find_path`](PathCache::find_path). 178 | //! 179 | //! Finding multiple Goals: 180 | //! ``` 181 | //! # use hierarchical_pathfinding::prelude::*; 182 | //! # let mut grid = [ 183 | //! # [0, 2, 0, 0, 0], 184 | //! # [0, 2, 2, 2, 2], 185 | //! # [0, 1, 0, 0, 0], 186 | //! # [0, 1, 0, 2, 0], 187 | //! # [0, 0, 0, 2, 0], 188 | //! # ]; 189 | //! # let (width, height) = (grid[0].len(), grid.len()); 190 | //! # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 191 | //! # move |(x, y)| [1, 10, -1][grid[y][x]] 192 | //! # } 193 | //! # let mut pathfinding = PathCache::new( 194 | //! # (width, height), 195 | //! # cost_fn(&grid), 196 | //! # ManhattanNeighborhood::new(width, height), 197 | //! # PathCacheConfig::with_chunk_size(3), 198 | //! # ); 199 | //! # 200 | //! let start = (0, 0); 201 | //! let goals = [(4, 4), (2, 0)]; 202 | //! 203 | //! // find_paths returns a HashMap for all successes 204 | //! let paths = pathfinding.find_paths( 205 | //! start, 206 | //! &goals, 207 | //! cost_fn(&grid), 208 | //! ); 209 | //! 210 | //! // (4, 4) is reachable 211 | //! assert!(paths.contains_key(&goals[0])); 212 | //! 213 | //! // (2, 0) is not reachable 214 | //! assert!(!paths.contains_key(&goals[1])); 215 | //! ``` 216 | //! For more information, see [`find_paths`](PathCache::find_paths). 217 | //! 218 | //! ##### Using a Path 219 | //! - Path exists: `path.is_some()` | `paths.contains_key()` 220 | //! - Useful as a Heuristic for other Algorithms 221 | //! - **100% correct** (`true` if and only if path can be found) 222 | //! - Total Cost of the Path: [`path.cost()`](internals::AbstractPath::cost) 223 | //! - Correct for this Path, may be slightly larger than for optimal Path 224 | //! - The cost is simply returned; `cost()` does no calculations 225 | //! - Total Length of the Path: [`path.length()`](internals::AbstractPath::length) 226 | //! - Correct for this Path, may be slightly longer than the optimal Path 227 | //! - The length is simply returned; `length()` does no calculations 228 | //! - Next Position: [`path.next()`](internals::AbstractPath::next) | [`path.safe_next(cost_fn)`](internals::AbstractPath::safe_next) 229 | //! - [`safe_next`](internals::AbstractPath::safe_next) is needed if [`config.cache_paths`](crate::PathCacheConfig::cache_paths) is set to `false` 230 | //! - can be called several times to iterate Path 231 | //! - path implements `Iterator` 232 | //! - Entire Path: `path.collect::>()` | [`path.resolve(cost_fn)`](internals::AbstractPath::resolve) 233 | //! - [`resolve`](internals::AbstractPath::resolve) is needed if [`config.cache_paths`](crate::PathCacheConfig::cache_paths) is set to `false` 234 | //! - Returns a `Vec<(usize, usize)>` 235 | //! 236 | //! Note that [`resolve`](internals::AbstractPath::resolve) calculates any missing segments (if [`config.cache_paths`](crate::PathCacheConfig::cache_paths) ` == false`) 237 | //! and allocates a [`Vec`] with the resulting Points. Not recommended if only the 238 | //! beginning of the Path is needed. 239 | //! ``` 240 | //! # use hierarchical_pathfinding::prelude::*; 241 | //! # let mut grid = [ 242 | //! # [0, 2, 0, 0, 0], 243 | //! # [0, 2, 2, 2, 2], 244 | //! # [0, 1, 0, 0, 0], 245 | //! # [0, 1, 0, 2, 0], 246 | //! # [0, 0, 0, 2, 0], 247 | //! # ]; 248 | //! # let (width, height) = (grid[0].len(), grid.len()); 249 | //! # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 250 | //! # move |(x, y)| [1, 10, -1][grid[y][x]] 251 | //! # } 252 | //! # let mut pathfinding = PathCache::new( 253 | //! # (width, height), 254 | //! # cost_fn(&grid), 255 | //! # ManhattanNeighborhood::new(width, height), 256 | //! # PathCacheConfig::with_chunk_size(3), 257 | //! # ); 258 | //! # struct Player{ pos: (usize, usize) } 259 | //! # impl Player { 260 | //! # pub fn move_to(&mut self, pos: (usize, usize)) { 261 | //! # self.pos = pos; 262 | //! # } 263 | //! # } 264 | //! # 265 | //! let mut player = Player { 266 | //! pos: (0, 0), 267 | //! //... 268 | //! }; 269 | //! let goal = (4, 4); 270 | //! 271 | //! let mut path = pathfinding.find_path( 272 | //! player.pos, 273 | //! goal, 274 | //! cost_fn(&grid), 275 | //! ).unwrap(); 276 | //! 277 | //! player.move_to(path.next().unwrap()); 278 | //! assert_eq!(player.pos, (0, 1)); 279 | //! 280 | //! // wait for next turn or whatever 281 | //! 282 | //! player.move_to(path.next().unwrap()); 283 | //! assert_eq!(player.pos, (0, 2)); 284 | //! 285 | //! // iterating is possible 286 | //! for new_pos in path { 287 | //! player.move_to(new_pos); 288 | //! } 289 | //! assert_eq!(player.pos, goal); 290 | //! ``` 291 | //! 292 | //! ##### Updating the `PathCache` 293 | //! The `PathCache` does not contain a copy or reference of the Grid for mutability and Ownership reasons. 294 | //! This means however, that the user is responsible for storing and maintaining both the Grid and the `PathCache`. 295 | //! It is also necessary to update the `PathCache` when the Grid has changed to keep it consistent: 296 | //! ```should_panic 297 | //! # use hierarchical_pathfinding::prelude::*; 298 | //! # let mut grid = [ 299 | //! # [0, 2, 0, 0, 0], 300 | //! # [0, 2, 2, 2, 2], 301 | //! # [0, 1, 0, 0, 0], 302 | //! # [0, 1, 0, 2, 0], 303 | //! # [0, 0, 0, 2, 0], 304 | //! # ]; 305 | //! # let (width, height) = (grid[0].len(), grid.len()); 306 | //! # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 307 | //! # move |(x, y)| [1, 10, -1][grid[y][x]] 308 | //! # } 309 | //! # let mut pathfinding = PathCache::new( 310 | //! # (width, height), 311 | //! # cost_fn(&grid), 312 | //! # ManhattanNeighborhood::new(width, height), 313 | //! # PathCacheConfig::with_chunk_size(3), 314 | //! # ); 315 | //! # 316 | //! let (start, goal) = ((0, 0), (2, 0)); 317 | //! 318 | //! let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 319 | //! assert!(path.is_none()); // from previous example 320 | //! 321 | //! // Clear a way to the goal 322 | //! grid[1][2] = 0; // at (2, 1): the wall below the goal 323 | //! 324 | //! let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 325 | //! assert!(path.is_some()); // there should be a Path now! 326 | //! ``` 327 | //! [`tiles_changed`](PathCache::tiles_changed) must be called with all changed Tiles: 328 | //! ``` 329 | //! # use hierarchical_pathfinding::prelude::*; 330 | //! # let mut grid = [ 331 | //! # [0, 2, 0, 0, 0], 332 | //! # [0, 2, 2, 2, 2], 333 | //! # [0, 1, 0, 0, 0], 334 | //! # [0, 1, 0, 2, 0], 335 | //! # [0, 0, 0, 2, 0], 336 | //! # ]; 337 | //! # let (width, height) = (grid[0].len(), grid.len()); 338 | //! # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 339 | //! # move |(x, y)| [1, 10, -1][grid[y][x]] 340 | //! # } 341 | //! # let mut pathfinding = PathCache::new( 342 | //! # (width, height), 343 | //! # cost_fn(&grid), 344 | //! # ManhattanNeighborhood::new(width, height), 345 | //! # PathCacheConfig::with_chunk_size(3), 346 | //! # ); 347 | //! # 348 | //! let (start, goal) = ((0, 0), (2, 0)); 349 | //! 350 | //! let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 351 | //! assert!(path.is_none()); 352 | //! 353 | //! // Clear a way to the goal 354 | //! grid[1][2] = 0; // at (2, 1): the wall below the goal 355 | //! 356 | //! pathfinding.tiles_changed( 357 | //! &[(2, 1)], 358 | //! cost_fn(&grid), 359 | //! ); 360 | //! 361 | //! let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 362 | //! assert!(path.is_some()); 363 | //! ``` 364 | //! `tiles_changed` takes a slice of Points, and it is recommended to bundle changes together for 365 | //! performance reasons. 366 | //! 367 | //! ##### Configuration 368 | //! The last parameter for [`PathCache::new`] is a [`PathCacheConfig`] object with different options to have more control over the generated `PathCache`. 369 | //! These options are mostly used to adjust the balance between Performance and Memory Usage, with the default values aiming more at Performance. 370 | //! ``` 371 | //! # use hierarchical_pathfinding::prelude::*; 372 | //! # let mut grid = [ 373 | //! # [0, 2, 0, 0, 0], 374 | //! # [0, 2, 2, 2, 2], 375 | //! # [0, 1, 0, 0, 0], 376 | //! # [0, 1, 0, 2, 0], 377 | //! # [0, 0, 0, 2, 0], 378 | //! # ]; 379 | //! # let (width, height) = (grid[0].len(), grid.len()); 380 | //! # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 381 | //! # move |(x, y)| [1, 10, -1][grid[y][x]] 382 | //! # } 383 | //! 384 | //! let mut pathfinding = PathCache::new( 385 | //! (width, height), // the size of the Grid 386 | //! cost_fn(&grid), // get the cost for walking over a Tile 387 | //! ManhattanNeighborhood::new(width, height), // the Neighborhood 388 | //! PathCacheConfig { 389 | //! chunk_size: 3, 390 | //! ..PathCacheConfig::LOW_MEM 391 | //! } 392 | //! ); 393 | //! 394 | //! assert_eq!(pathfinding.config().chunk_size, 3); 395 | //! ``` 396 | //! # Cargo Features 397 | //! ##### parallel 398 | //! Enabled by default. 399 | //! 400 | //! The parallel feature causes [`PathCache`] creation and updates to be multithreaded using [Rayon](https://crates.io/crates/rayon), making them significantly faster. 401 | //! This feature has no effect on the speed of finding paths (_yet_). 402 | //! 403 | //! ##### log 404 | //! Disabled by default. 405 | //! 406 | //! The log feature is used to enable internal timings on some functions. 407 | //! 408 | //! You probably shouldn't enable this feature unless you are working on improvements to `hierarchical_pathfinding`. 409 | //! In order to consume the logs, you need a logger setup to show trace! level logs. 410 | //! See the [log](https://crates.io/crates/log) crate for more details. 411 | //! 412 | 413 | /// Shorthand for a 2D Point 414 | type Point = (usize, usize); 415 | 416 | /// A convenience type for a [`HashMap`](hashbrown::HashMap) using Points as the key 417 | type PointMap = hashbrown::HashMap; 418 | /// A convenience type for a [`HashSet`](hashbrown::HashSet) with Points 419 | type PointSet = hashbrown::HashSet; 420 | 421 | /// The Type used to reference a Node in the abstracted Graph 422 | type NodeID = usize; 423 | 424 | /// A convenience type for a [`HashMap`](hashbrown::HashMap) using [`NodeID`]s as the key 425 | type NodeIDMap = hashbrown::HashMap; 426 | /// A convenience type for a [`HashSet`](hashbrown::HashSet) with [`NodeID`]s 427 | type NodeIDSet = hashbrown::HashSet; 428 | 429 | mod path_cache; 430 | pub use self::path_cache::{PathCache, PathCacheConfig}; 431 | 432 | mod path; 433 | 434 | mod utils; 435 | pub(crate) use utils::*; 436 | 437 | pub mod neighbors; 438 | 439 | mod graph; 440 | mod grid; 441 | 442 | /// Internal stuff that is returned by other function 443 | pub mod internals { 444 | pub use crate::path::AbstractPath; 445 | pub use crate::path_cache::{CacheInspector, NodeInspector}; 446 | } 447 | 448 | /// The prelude for this crate. 449 | pub mod prelude { 450 | pub use crate::{ 451 | neighbors::{ManhattanNeighborhood, MooreNeighborhood, Neighborhood}, 452 | PathCache, PathCacheConfig, 453 | }; 454 | } 455 | -------------------------------------------------------------------------------- /src/neighbors.rs: -------------------------------------------------------------------------------- 1 | //! A crate with the most common Neighborhoods 2 | 3 | use crate::Point; 4 | use std::fmt::Debug; 5 | 6 | /// Defines how a Path can move along the Grid. 7 | /// 8 | /// Different use cases may have different conditions for Paths. 9 | /// For example if movement is restricted to the 4 cardinal directions, any Paths generated should 10 | /// reflect that by only containing those steps. 11 | /// 12 | /// This Trait is a generalized solution to that problem. It provides a function to query all 13 | /// neighboring Points of an existing Point and a Heuristic for how long it might take to reach 14 | /// a goal from a Point. 15 | /// 16 | /// The most common implementations of this Trait are already provided by this Module: 17 | /// - [`ManhattanNeighborhood`] for Agents that can move 18 | /// up, down, left or right 19 | /// - [`MooreNeighborhood`] for Agents that can move 20 | /// up, down, left, right, as well as the 4 diagonals (up-right, ...) 21 | pub trait Neighborhood: Clone + Debug { 22 | /// Provides all the Neighbors of a Point. 23 | /// 24 | /// The Neighbors should be written into `target` 25 | /// 26 | /// Note that it is not necessary to check weather the Tile at a Point is solid or not. 27 | /// That check is done later. 28 | fn get_all_neighbors(&self, point: Point, target: &mut Vec); 29 | /// Gives a Heuristic for how long it takes to reach `goal` from `point`. 30 | /// 31 | /// This is usually the Distance between the two Points in the Metric of your Neighborhood. 32 | /// 33 | /// The returned value _**must be**_ less than or equal to the real cost of walking from 34 | /// `point` to `goal`. See [A*](https://en.wikipedia.org/wiki/A*_search_algorithm) for details. 35 | /// 36 | /// If there is no proper way of calculation how long it takes, simply return 0. This will 37 | /// increase the time it takes to calculate the Path, but at least it will always be correct. 38 | fn heuristic(&self, point: Point, goal: Point) -> usize; 39 | } 40 | 41 | /// A Neighborhood for Agents moving along the 4 cardinal directions. 42 | /// 43 | /// Also known as [Von Neumann Neighborhood](https://en.wikipedia.org/wiki/Von_Neumann_neighborhood), 44 | /// Manhattan Metric or [Taxicab Geometry](https://en.wikipedia.org/wiki/Taxicab_geometry). 45 | /// 46 | /// ```no_code 47 | /// A: Agent, o: reachable in one step 48 | /// o 49 | /// | 50 | /// o-A-o 51 | /// | 52 | /// o 53 | /// ``` 54 | #[derive(Clone, Copy, Debug)] 55 | pub struct ManhattanNeighborhood { 56 | width: usize, 57 | height: usize, 58 | } 59 | 60 | impl ManhattanNeighborhood { 61 | /// Creates a new `ManhattanNeighborhood`. 62 | /// 63 | /// `width` and `height` are the size of the Grid to move on. 64 | pub fn new(width: usize, height: usize) -> ManhattanNeighborhood { 65 | ManhattanNeighborhood { width, height } 66 | } 67 | } 68 | 69 | impl Neighborhood for ManhattanNeighborhood { 70 | fn get_all_neighbors(&self, point: Point, target: &mut Vec) { 71 | let (width, height) = (self.width, self.height); 72 | 73 | #[rustfmt::skip] 74 | static ALL_DELTAS: [(isize, isize); 4] = [(0, -1), (1, 0), (0, 1), (-1, 0)]; 75 | 76 | for (dx, dy) in &ALL_DELTAS { 77 | let x = point.0 as isize + dx; 78 | let y = point.1 as isize + dy; 79 | if x >= 0 && x < width as isize && y >= 0 && y < height as isize { 80 | target.push((x as usize, y as usize)) 81 | } 82 | } 83 | } 84 | fn heuristic(&self, point: Point, goal: Point) -> usize { 85 | let diff_0 = if goal.0 > point.0 { 86 | goal.0 - point.0 87 | } else { 88 | point.0 - goal.0 89 | }; 90 | let diff_1 = if goal.1 > point.1 { 91 | goal.1 - point.1 92 | } else { 93 | point.1 - goal.1 94 | }; 95 | diff_0 + diff_1 96 | } 97 | } 98 | 99 | /// A Neighborhood for Agents moving along the 4 cardinal directions and the 4 diagonals. 100 | /// 101 | /// Also known as [Moore Neighborhood](https://en.wikipedia.org/wiki/Moore_neighborhood), 102 | /// [Maximum Metric](https://en.wikipedia.org/wiki/Chebyshev_distance) or Chebyshev Metric. 103 | /// 104 | /// ```no_code 105 | /// A: Agent, o: reachable in one step 106 | /// o o o 107 | /// \|/ 108 | /// o-A-o 109 | /// /|\ 110 | /// o o o 111 | /// ``` 112 | #[derive(Clone, Copy, Debug)] 113 | pub struct MooreNeighborhood { 114 | width: usize, 115 | height: usize, 116 | } 117 | 118 | impl MooreNeighborhood { 119 | /// Creates a new `MooreNeighborhood`. 120 | /// 121 | /// `width` and `height` are the size of the Grid to move on. 122 | pub fn new(width: usize, height: usize) -> MooreNeighborhood { 123 | MooreNeighborhood { width, height } 124 | } 125 | } 126 | 127 | impl Neighborhood for MooreNeighborhood { 128 | fn get_all_neighbors(&self, point: Point, target: &mut Vec) { 129 | let (width, height) = (self.width, self.height); 130 | 131 | #[rustfmt::skip] 132 | static ALL_DELTAS: [(isize, isize); 8] = [(0, -1), (1, -1), (1, 0), (1, 1), (0, 1), (-1, 1), (-1, 0), (-1, -1)]; 133 | 134 | for (dx, dy) in ALL_DELTAS.iter() { 135 | let x = point.0 as isize + dx; 136 | let y = point.1 as isize + dy; 137 | if x >= 0 && x < width as isize && y >= 0 && y < height as isize { 138 | target.push((x as usize, y as usize)) 139 | } 140 | } 141 | } 142 | fn heuristic(&self, point: Point, goal: Point) -> usize { 143 | let diff_0 = if goal.0 > point.0 { 144 | goal.0 - point.0 145 | } else { 146 | point.0 - goal.0 147 | }; 148 | let diff_1 = if goal.1 > point.1 { 149 | goal.1 - point.1 150 | } else { 151 | point.1 - goal.1 152 | }; 153 | diff_0.max(diff_1) 154 | } 155 | } 156 | 157 | #[cfg(test)] 158 | mod tests { 159 | use super::*; 160 | mod manhattan { 161 | use super::*; 162 | #[test] 163 | fn get_all_neighbors() { 164 | let neighborhood = ManhattanNeighborhood::new(5, 5); 165 | let mut target = vec![]; 166 | neighborhood.get_all_neighbors((0, 2), &mut target); 167 | assert_eq!(target, vec![(0, 1), (1, 2), (0, 3)],); 168 | } 169 | 170 | #[test] 171 | fn heuristic() { 172 | let neighborhood = ManhattanNeighborhood::new(5, 5); 173 | assert_eq!(neighborhood.heuristic((3, 1), (0, 0)), 3 + 1); 174 | } 175 | } 176 | mod moore { 177 | use super::*; 178 | 179 | #[test] 180 | fn get_all_neighbors() { 181 | let neighborhood = MooreNeighborhood::new(5, 5); 182 | let mut target = vec![]; 183 | neighborhood.get_all_neighbors((0, 2), &mut target); 184 | assert_eq!(target, vec![(0, 1), (1, 1), (1, 2), (1, 3), (0, 3)],); 185 | } 186 | 187 | #[test] 188 | fn heuristic() { 189 | let neighborhood = MooreNeighborhood::new(5, 5); 190 | assert_eq!(neighborhood.heuristic((3, 1), (0, 0)), 3); 191 | } 192 | } 193 | } 194 | -------------------------------------------------------------------------------- /src/path/abstract_path.rs: -------------------------------------------------------------------------------- 1 | use super::{Cost, Path, PathSegment}; 2 | use crate::{grid, neighbors::Neighborhood, Point}; 3 | 4 | /// A Path that may not be fully calculated yet. 5 | /// 6 | /// This struct represents a Path that was generated by the `PathCache`. Since 7 | /// [`config.cache_paths`](crate::PathCacheConfig::cache_paths) may be set to false, there might by 8 | /// segments of this Path that are not yet calculated. In those cases, since the cost function is 9 | /// required to resolve those sections, it is necessary to call 10 | /// [`safe_next()`](AbstractPath::safe_next) or [`resolve()`](AbstractPath::resolve). 11 | /// Otherwise it is possible to treat this as an `Iterator`. 12 | /// 13 | /// **Warning: Calling `next()` on an `AbstractPath` with unknown segments will panic as soon as those 14 | /// segments are reached.** 15 | /// 16 | /// **Warning: Keeping an `AbstractPath` after changing the Grid, or using a different cost function, 17 | /// leads to panics and wrong results.** 18 | /// 19 | /// **You have been warned** 20 | #[derive(Debug, Clone)] 21 | pub struct AbstractPath { 22 | neighborhood: N, 23 | total_cost: Cost, 24 | total_length: usize, 25 | path: Vec, 26 | end: Point, 27 | current_index: (usize, usize), 28 | steps_taken: usize, 29 | } 30 | 31 | impl AbstractPath { 32 | /// Returns the total cost of this Path. 33 | /// This value is always known and requires no further calculations. 34 | pub fn cost(&self) -> Cost { 35 | self.total_cost 36 | } 37 | 38 | /// Returns the total length of this Path. 39 | /// 40 | /// This value is always known and requires no further calculations. 41 | /// 42 | /// Note that the [`len`](AbstractPath::len) method is provided by [`ExactSizeIterator`] and tells 43 | /// the remaining elements, whereas the one stays constant. 44 | pub fn length(&self) -> usize { 45 | self.total_length 46 | } 47 | 48 | /// A variant of [`Iterator::next()`](#impl-Iterator) that can resolve unknown segments 49 | /// of the Path. Use this method instead of `next()` when 50 | /// [`config.cache_paths`](crate::PathCacheConfig::cache_paths) is set to `false`. 51 | pub fn safe_next(&mut self, get_cost: impl FnMut(Point) -> isize) -> Option { 52 | self.internal_next(Some(get_cost)) 53 | } 54 | fn internal_next isize>(&mut self, get_cost: Option) -> Option { 55 | if self.current_index.0 >= self.path.len() { 56 | return None; 57 | } 58 | let mut current = &self.path[self.current_index.0]; 59 | if let PathSegment::Unknown { start, end, .. } = *current { 60 | let path = grid::a_star_search( 61 | &self.neighborhood, 62 | |_| true, 63 | get_cost.expect("Tried calling next() on a Path that is not fully known. Use safe_next() instead."), 64 | start, 65 | end, 66 | self.neighborhood.heuristic(start, end) * 2 67 | ) 68 | .unwrap_or_else(|| { 69 | panic!( 70 | "Impossible Path marked as Possible: {:?} -> {:?}", 71 | start, end 72 | ) 73 | }); 74 | 75 | self.path[self.current_index.0] = PathSegment::Known(path); 76 | current = &self.path[self.current_index.0]; 77 | 78 | self.current_index.1 = 1; // paths include start and end, but we are already at start 79 | } 80 | 81 | let path = match current { 82 | PathSegment::Known(path) => path, 83 | PathSegment::Unknown { .. } => { 84 | unreachable!() 85 | } 86 | }; 87 | 88 | let ret = Some(path[self.current_index.1]); 89 | self.current_index.1 += 1; 90 | if self.current_index.1 >= path.len() { 91 | self.current_index.0 += 1; 92 | self.current_index.1 = 1; 93 | } 94 | 95 | self.steps_taken += 1; 96 | 97 | ret 98 | } 99 | 100 | /// Resolves all unknown sections of the Path. 101 | /// 102 | /// if [`config.cache_paths`](crate::PathCacheConfig::cache_paths) is set to true, 103 | /// then calling this method is similar to calling `path.collect::>()`. 104 | pub fn resolve(mut self, mut get_cost: impl FnMut(Point) -> isize) -> Vec { 105 | let mut result = Vec::with_capacity(self.len()); 106 | 107 | while let Some(pos) = self.safe_next(&mut get_cost) { 108 | result.push(pos); 109 | } 110 | result 111 | } 112 | 113 | pub(crate) fn new(neighborhood: N, end: Point) -> AbstractPath { 114 | AbstractPath { 115 | neighborhood, 116 | total_cost: 0, 117 | total_length: 0, 118 | path: vec![], 119 | end, 120 | current_index: (0, 1), 121 | steps_taken: 0, 122 | } 123 | } 124 | 125 | pub(crate) fn from_known_path(neighborhood: N, path: Path) -> AbstractPath { 126 | let end = path[path.len() - 1]; 127 | AbstractPath { 128 | total_cost: path.cost(), 129 | total_length: path.len() - 1, 130 | path: vec![PathSegment::Known(path)], 131 | ..AbstractPath::new(neighborhood, end) 132 | } 133 | } 134 | 135 | pub(crate) fn add_path_segment(&mut self, path: PathSegment) -> &mut Self { 136 | assert!( 137 | self.end == path.start(), 138 | "Added disconnected PathSegment: expected {:?}, got {:?}", 139 | self.end, 140 | path.start() 141 | ); 142 | self.total_cost += path.cost(); 143 | self.total_length += path.len() - 1; 144 | self.end = path.end(); 145 | self.path.push(path); 146 | self 147 | } 148 | 149 | pub(crate) fn add_path(&mut self, path: Path) -> &mut Self { 150 | assert!( 151 | self.end == path[0], 152 | "Added disconnected Path: expected {:?}, got {:?}", 153 | self.end, 154 | path[0] 155 | ); 156 | self.total_cost += path.cost(); 157 | self.total_length += path.len() - 1; 158 | self.end = path[path.len() - 1]; 159 | self.path.push(PathSegment::Known(path)); 160 | self 161 | } 162 | 163 | #[allow(dead_code)] 164 | pub(crate) fn add_node(&mut self, node: Point, cost: Cost, len: usize) -> &mut Self { 165 | self.path.push(PathSegment::Unknown { 166 | start: self.end, 167 | end: node, 168 | cost, 169 | len, 170 | }); 171 | self.total_cost += cost; 172 | self.total_length += len; 173 | self.end = node; 174 | self 175 | } 176 | } 177 | 178 | impl Iterator for AbstractPath { 179 | type Item = Point; 180 | /// See [`Iterator::next`] 181 | /// 182 | /// ## Panics 183 | /// Panics if a segment of the Path is not known because [`config.cache_paths`](crate::PathCacheConfig::cache_paths) 184 | /// is set to `false`. Use [`safe_next`](AbstractPath::safe_next) in those cases. 185 | fn next(&mut self) -> Option { 186 | self.internal_next:: isize>(None) 187 | } 188 | fn size_hint(&self) -> (usize, Option) { 189 | let len = self.total_length - self.steps_taken; 190 | (len, Some(len)) 191 | } 192 | fn count(self) -> usize { 193 | self.len() 194 | } 195 | fn last(self) -> Option { 196 | Some(self.end) 197 | } 198 | fn nth(&mut self, step: usize) -> Option { 199 | self.current_index.1 += step; 200 | while self.current_index.0 < self.path.len() { 201 | let current_len = self.path[self.current_index.0].len(); 202 | if self.current_index.1 < current_len { 203 | break; 204 | } 205 | self.current_index.1 -= current_len - 1; 206 | self.current_index.0 += 1; 207 | } 208 | self.next() 209 | } 210 | } 211 | impl ExactSizeIterator for AbstractPath {} 212 | impl std::iter::FusedIterator for AbstractPath {} 213 | 214 | #[cfg(test)] 215 | mod tests { 216 | use super::*; 217 | 218 | #[test] 219 | fn next() { 220 | let neigh = crate::neighbors::ManhattanNeighborhood::new(100, 100); 221 | let mut path = AbstractPath::from_known_path( 222 | neigh, 223 | Path::from_slice(&[(99, 99), (0, 0), (1, 1), (2, 2), (3, 3)], 3), 224 | ); 225 | path.add_path(Path::from_slice(&[(3, 3), (4, 4), (5, 5)], 5)); 226 | assert_eq!(path.next(), Some((0, 0))); 227 | assert_eq!(path.next(), Some((1, 1))); 228 | assert_eq!(path.next(), Some((2, 2))); 229 | assert_eq!(path.next(), Some((3, 3))); 230 | assert_eq!(path.next(), Some((4, 4))); 231 | assert_eq!(path.next(), Some((5, 5))); 232 | assert_eq!(path.next(), None); 233 | assert_eq!(path.next(), None); 234 | assert_eq!(path.next(), None); 235 | } 236 | 237 | #[test] 238 | fn nth() { 239 | let neigh = crate::neighbors::ManhattanNeighborhood::new(100, 100); 240 | let mut path = AbstractPath::from_known_path( 241 | neigh, 242 | Path::from_slice( 243 | &[(99, 99), (0, 0), (1, 1), (2, 2), (3, 3), (4, 4), (5, 5)], 244 | 3, 245 | ), 246 | ); 247 | { 248 | let mut path = path.clone(); 249 | assert_eq!(path.nth(3), Some((3, 3))); 250 | assert_eq!(path.next(), Some((4, 4))); 251 | assert_eq!(path.nth(1), None); 252 | } 253 | 254 | path.add_path(Path::from_slice(&[(5, 5), (6, 6), (7, 7)], 5)); 255 | 256 | { 257 | let mut path = path.clone(); 258 | assert_eq!(path.nth(3), Some((3, 3))); 259 | assert_eq!(path.nth(3), Some((7, 7))); 260 | assert_eq!(path.nth(3), None); 261 | } 262 | { 263 | let mut path = path.clone(); 264 | assert_eq!(path.nth(7), Some((7, 7))); 265 | } 266 | assert_eq!(path.nth(100), None); 267 | } 268 | 269 | #[test] 270 | fn size_hint_and_len() { 271 | let neigh = crate::neighbors::ManhattanNeighborhood::new(100, 100); 272 | let mut path = AbstractPath::from_known_path( 273 | neigh, 274 | Path::from_slice(&[(99, 99), (0, 0), (1, 1), (2, 2), (3, 3)], 3), 275 | ); 276 | path.add_path(Path::from_slice(&[(3, 3), (4, 4), (5, 5)], 5)); 277 | assert_eq!(path.size_hint(), (6, Some(6))); 278 | assert_eq!(path.len(), 6); 279 | assert_eq!(path.len(), path.length()); 280 | assert_eq!(path.next(), Some((0, 0))); 281 | 282 | assert_eq!(path.size_hint(), (5, Some(5))); 283 | assert_eq!(path.len(), 5); 284 | assert_eq!(path.next(), Some((1, 1))); 285 | 286 | assert_eq!(path.size_hint(), (4, Some(4))); 287 | assert_eq!(path.len(), 4); 288 | assert_eq!(path.next(), Some((2, 2))); 289 | 290 | assert_eq!(path.size_hint(), (3, Some(3))); 291 | assert_eq!(path.len(), 3); 292 | assert_eq!(path.next(), Some((3, 3))); 293 | 294 | assert_eq!(path.size_hint(), (2, Some(2))); 295 | assert_eq!(path.len(), 2); 296 | assert_eq!(path.next(), Some((4, 4))); 297 | 298 | assert_eq!(path.size_hint(), (1, Some(1))); 299 | assert_eq!(path.len(), 1); 300 | assert_eq!(path.next(), Some((5, 5))); 301 | 302 | assert_eq!(path.size_hint(), (0, Some(0))); 303 | assert_eq!(path.len(), 0); 304 | assert_eq!(path.next(), None); 305 | 306 | assert_eq!(path.len(), 0); 307 | assert_eq!(path.size_hint(), (0, Some(0))); 308 | } 309 | 310 | #[test] 311 | fn count_and_last() { 312 | let neigh = crate::neighbors::ManhattanNeighborhood::new(100, 100); 313 | let mut path = AbstractPath::from_known_path( 314 | neigh, 315 | Path::from_slice(&[(99, 99), (0, 0), (1, 1), (2, 2), (3, 3)], 3), 316 | ); 317 | path.add_path(Path::from_slice(&[(3, 3), (4, 4), (5, 5)], 5)); 318 | 319 | let mut cnt = 0; 320 | let mut last = (99, 99); 321 | for p in path.clone() { 322 | cnt += 1; 323 | last = p; 324 | } 325 | assert_eq!(path.clone().count(), cnt); 326 | assert_eq!(path.last(), Some(last)); 327 | } 328 | 329 | #[test] 330 | fn disconnected_segment_on_long_paths() { 331 | use crate::prelude::*; 332 | for w in (1..5).map(|w| w * 23) { 333 | println!("w: {:?}", w); 334 | let pathfinding = PathCache::new( 335 | (w, w), 336 | |_| 1, 337 | MooreNeighborhood::new(w, w), 338 | PathCacheConfig::default(), 339 | ); 340 | let path = pathfinding.find_path((0, 0), (w - 1, w - 1), |_| 1); 341 | assert!(path.is_some()); 342 | 343 | let pathfinding = crate::PathCache::new( 344 | (w, w), 345 | |_| 1, 346 | ManhattanNeighborhood::new(w, w), 347 | PathCacheConfig::default(), 348 | ); 349 | let path = pathfinding.find_path((0, 0), (w - 1, w - 1), |_| 1); 350 | assert!(path.is_some()); 351 | } 352 | } 353 | } 354 | -------------------------------------------------------------------------------- /src/path/generic_path.rs: -------------------------------------------------------------------------------- 1 | use super::Cost; 2 | 3 | use std::sync::Arc; 4 | 5 | #[derive(Debug, Clone, PartialEq, Eq)] 6 | pub struct Path

{ 7 | path: Arc<[P]>, 8 | cost: Cost, 9 | is_reversed: bool, 10 | } 11 | 12 | #[allow(dead_code)] 13 | impl

Path

{ 14 | pub fn new(path: Vec

, cost: Cost) -> Path

{ 15 | Path { 16 | path: path.into(), 17 | cost, 18 | is_reversed: false, 19 | } 20 | } 21 | 22 | pub fn from_slice(path: &[P], cost: Cost) -> Path

23 | where 24 | P: Clone, 25 | { 26 | Path { 27 | path: path.into(), 28 | cost, 29 | is_reversed: false, 30 | } 31 | } 32 | 33 | pub fn cost(&self) -> Cost { 34 | self.cost 35 | } 36 | 37 | pub fn len(&self) -> usize { 38 | self.path.len() 39 | } 40 | 41 | pub fn is_empty(&self) -> bool { 42 | self.path.is_empty() 43 | } 44 | 45 | pub fn reversed(&self, start_cost: Cost, end_cost: Cost) -> Path

{ 46 | Path { 47 | path: self.path.clone(), 48 | cost: self.cost - start_cost + end_cost, 49 | is_reversed: !self.is_reversed, 50 | } 51 | } 52 | 53 | /// Returns an Iterator over the Path 54 | pub fn iter(&self) -> Iter

{ 55 | Iter { 56 | iter: self.path.iter(), 57 | reversed: self.is_reversed, 58 | } 59 | } 60 | } 61 | 62 | use std::ops::Index; 63 | 64 | impl

Index for Path

{ 65 | type Output = P; 66 | fn index(&self, index: usize) -> &P { 67 | let index = if self.is_reversed { 68 | self.path.len() - index - 1 69 | } else { 70 | index 71 | }; 72 | &self.path[index] 73 | } 74 | } 75 | 76 | #[derive(Debug)] 77 | pub struct Iter<'a, P> { 78 | iter: std::slice::Iter<'a, P>, 79 | reversed: bool, 80 | } 81 | 82 | impl<'a, P> Iterator for Iter<'a, P> { 83 | type Item = &'a P; 84 | fn next(&mut self) -> Option { 85 | if self.reversed { 86 | self.iter.next_back() 87 | } else { 88 | self.iter.next() 89 | } 90 | } 91 | fn size_hint(&self) -> (usize, Option) { 92 | self.iter.size_hint() 93 | } 94 | } 95 | 96 | impl

DoubleEndedIterator for Iter<'_, P> { 97 | fn next_back(&mut self) -> Option { 98 | if self.reversed { 99 | self.iter.next() 100 | } else { 101 | self.iter.next_back() 102 | } 103 | } 104 | } 105 | impl

ExactSizeIterator for Iter<'_, P> {} 106 | impl

std::iter::FusedIterator for Iter<'_, P> {} 107 | 108 | impl PartialEq> for Path

{ 109 | fn eq(&self, rhs: &Vec

) -> bool { 110 | // we can't just use slice's eq because self might be reversed 111 | self.len() == rhs.len() && self.iter().zip(rhs.iter()).all(|(a, b)| a == b) 112 | } 113 | } 114 | 115 | impl<'a, P: PartialEq> PartialEq<&'a [P]> for Path

{ 116 | fn eq(&self, rhs: &&'a [P]) -> bool { 117 | // we can't just use slice's eq because self might be reversed 118 | self.len() == rhs.len() && self.iter().zip(rhs.iter()).all(|(a, b)| a == b) 119 | } 120 | } 121 | 122 | use std::cmp::Ordering; 123 | 124 | impl Ord for Path

{ 125 | fn cmp(&self, other: &Path

) -> Ordering { 126 | self.cost.cmp(&other.cost) 127 | } 128 | } 129 | 130 | impl PartialOrd for Path

{ 131 | fn partial_cmp(&self, other: &Path

) -> Option { 132 | Some(self.cost.cmp(&other.cost)) 133 | } 134 | } 135 | 136 | use std::fmt; 137 | impl fmt::Display for Path

{ 138 | fn fmt(&self, fmt: &mut fmt::Formatter) -> fmt::Result { 139 | write!(fmt, "Path[Cost = {}]: ", self.cost)?; 140 | if self.path.is_empty() { 141 | write!(fmt, "") 142 | } else { 143 | write!(fmt, "{}", self.path[0])?; 144 | for p in self.path.iter().skip(1) { 145 | write!(fmt, " -> {}", p)?; 146 | } 147 | Ok(()) 148 | } 149 | } 150 | } 151 | 152 | #[cfg(test)] 153 | mod tests { 154 | 155 | use super::Path; 156 | #[test] 157 | fn index() { 158 | let path = Path::new(vec![4, 2, 0], 42); 159 | 160 | assert_eq!(path[0], 4); 161 | assert_eq!(path[1], 2); 162 | assert_eq!(path[2], 0); 163 | } 164 | 165 | #[test] 166 | fn display() { 167 | let path = Path::new(vec![4, 2, 0], 42); 168 | 169 | assert_eq!(&format!("{}", path), "Path[Cost = 42]: 4 -> 2 -> 0"); 170 | } 171 | 172 | #[test] 173 | fn display_empty() { 174 | let path = Path::new(Vec::::new(), 0); 175 | 176 | assert_eq!(&format!("{}", path), "Path[Cost = 0]: "); 177 | } 178 | } 179 | -------------------------------------------------------------------------------- /src/path/mod.rs: -------------------------------------------------------------------------------- 1 | mod abstract_path; 2 | pub use abstract_path::AbstractPath; 3 | 4 | mod generic_path; 5 | pub use generic_path::*; 6 | 7 | mod path_segment; 8 | pub use path_segment::PathSegment; 9 | 10 | pub type Cost = usize; 11 | -------------------------------------------------------------------------------- /src/path/path_segment.rs: -------------------------------------------------------------------------------- 1 | use super::{Cost, Path}; 2 | use crate::Point; 3 | 4 | #[derive(Clone, Debug)] 5 | pub enum PathSegment { 6 | Known(Path), 7 | Unknown { 8 | start: Point, 9 | end: Point, 10 | cost: Cost, 11 | len: usize, 12 | }, 13 | } 14 | 15 | use self::PathSegment::*; 16 | 17 | impl PathSegment { 18 | pub fn new(path: Path, known: bool) -> PathSegment { 19 | if known { 20 | Known(path) 21 | } else { 22 | Unknown { 23 | start: path[0], 24 | end: path[path.len() - 1], 25 | cost: path.cost(), 26 | len: path.len(), 27 | } 28 | } 29 | } 30 | 31 | pub fn cost(&self) -> Cost { 32 | match *self { 33 | Known(ref path) => path.cost(), 34 | Unknown { cost, .. } => cost, 35 | } 36 | } 37 | 38 | pub fn len(&self) -> usize { 39 | match *self { 40 | Known(ref path) => path.len(), 41 | Unknown { len, .. } => len, 42 | } 43 | } 44 | 45 | pub fn start(&self) -> Point { 46 | match *self { 47 | Known(ref path) => path[0], 48 | Unknown { start, .. } => start, 49 | } 50 | } 51 | 52 | pub fn end(&self) -> Point { 53 | match *self { 54 | Known(ref path) => path[path.len() - 1], 55 | Unknown { end, .. } => end, 56 | } 57 | } 58 | 59 | pub fn reversed(&self, start_cost: Cost, end_cost: Cost) -> PathSegment { 60 | match *self { 61 | Known(ref path) => Known(path.reversed(start_cost, end_cost)), 62 | Unknown { 63 | start, 64 | end, 65 | len, 66 | cost, 67 | } => Unknown { 68 | start: end, 69 | end: start, 70 | cost: cost + end_cost - start_cost, 71 | len, 72 | }, 73 | } 74 | } 75 | } 76 | -------------------------------------------------------------------------------- /src/path_cache.rs: -------------------------------------------------------------------------------- 1 | use crate::{ 2 | graph::{self, Node, NodeList}, 3 | neighbors::Neighborhood, 4 | path::{AbstractPath, Cost, Path, PathSegment}, 5 | *, 6 | }; 7 | 8 | use std::marker::PhantomData; 9 | 10 | // a Macro to log::trace the time since $timer, and restart $timer 11 | #[cfg(feature = "log")] 12 | macro_rules! re_trace { 13 | ($msg: literal, $timer: ident) => { 14 | let now = std::time::Instant::now(); 15 | log::trace!(concat!("time to ", $msg, ": {:?}"), now - $timer); 16 | #[allow(unused)] 17 | let $timer = now; 18 | }; 19 | } 20 | #[cfg(not(feature = "log"))] 21 | macro_rules! re_trace { 22 | // does nothing without log feature 23 | ($msg: literal, $timer: ident) => {}; 24 | } 25 | 26 | mod cache_config; 27 | pub use cache_config::PathCacheConfig; 28 | 29 | mod chunk; 30 | use chunk::Chunk; 31 | 32 | enum CostFnWrapper 33 | where 34 | F1: Sync + Fn(Point) -> isize, 35 | F2: FnMut(Point) -> isize, 36 | { 37 | Sequential(F2, PhantomData), // F1 has to appear in the enum even if `parallel` is disabled 38 | #[cfg(feature = "parallel")] 39 | Parallel(F1), 40 | } 41 | 42 | /// A struct to store the Hierarchical Pathfinding information. 43 | #[derive(Clone, Debug)] 44 | pub struct PathCache { 45 | width: usize, 46 | height: usize, 47 | chunks: Vec, 48 | num_chunks: (usize, usize), 49 | nodes: NodeList, 50 | neighborhood: N, 51 | config: PathCacheConfig, 52 | } 53 | 54 | impl PathCache { 55 | /// Creates a new `PathCache` 56 | /// 57 | /// ## Arguments 58 | /// - `(width, height)` - the size of the Grid 59 | /// - `get_cost` - get the cost for walking over a Tile. (Cost < 0 means solid Tile) 60 | /// - `neighborhood` - the Neighborhood to use. (See [`Neighborhood`]) 61 | /// - `config` - optional config for creating the cache. (See [`PathCacheConfig`]) 62 | /// 63 | /// `get_cost((x, y))` should return the cost for walking over the Tile at (x, y). 64 | /// Costs below 0 are solid Tiles. 65 | /// 66 | /// ## Examples 67 | /// Basic usage: 68 | /// ``` 69 | /// use hierarchical_pathfinding::prelude::*; 70 | /// 71 | /// // create and initialize Grid 72 | /// // 0 = empty, 1 = swamp, 2 = wall 73 | /// let mut grid = [ 74 | /// [0, 2, 0, 0, 0], 75 | /// [0, 2, 2, 2, 0], 76 | /// [0, 1, 0, 0, 0], 77 | /// [0, 1, 0, 2, 0], 78 | /// [0, 0, 0, 2, 0], 79 | /// ]; 80 | /// let (width, height) = (grid[0].len(), grid.len()); 81 | /// type Grid = [[usize; 5]; 5]; 82 | /// 83 | /// const COST_MAP: [isize; 3] = [1, 10, -1]; 84 | /// 85 | /// fn cost_fn(grid: &Grid) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 86 | /// move |(x, y)| COST_MAP[grid[y][x]] 87 | /// } 88 | /// 89 | /// let mut pathfinding = PathCache::new( 90 | /// (width, height), // the size of the Grid 91 | /// cost_fn(&grid), // get the cost for walking over a Tile 92 | /// ManhattanNeighborhood::new(width, height), // the Neighborhood 93 | /// PathCacheConfig::with_chunk_size(3), // config 94 | /// ); 95 | /// ``` 96 | pub fn new isize>( 97 | (width, height): (usize, usize), 98 | get_cost: F, 99 | neighborhood: N, 100 | config: PathCacheConfig, 101 | ) -> PathCache { 102 | #[cfg(feature = "parallel")] 103 | { 104 | PathCache::new_internal:: isize>( 105 | (width, height), 106 | CostFnWrapper::Parallel(get_cost), 107 | neighborhood, 108 | config, 109 | ) 110 | } 111 | #[cfg(not(feature = "parallel"))] 112 | { 113 | PathCache::new_internal:: isize, F>( 114 | (width, height), 115 | CostFnWrapper::Sequential(get_cost, PhantomData), 116 | neighborhood, 117 | config, 118 | ) 119 | } 120 | } 121 | 122 | /// Same as [`new`](PathCache::new), but doesn't use threads to allow [`FnMut`]. 123 | /// 124 | /// Equivalent to `new` if `parallel` feature is disabled. 125 | /// 126 | /// Note that this is _**way**_ slower than `new` with `parallel`. 127 | pub fn new_with_fn_mut isize>( 128 | (width, height): (usize, usize), 129 | get_cost: F, 130 | neighborhood: N, 131 | config: PathCacheConfig, 132 | ) -> PathCache { 133 | PathCache::new_internal:: isize, F>( 134 | (width, height), 135 | CostFnWrapper::Sequential(get_cost, PhantomData::default()), 136 | neighborhood, 137 | config, 138 | ) 139 | } 140 | 141 | /// Same as [`new`](PathCache::new), ~~but uses multiple threads.~~ 142 | /// 143 | /// Note that `get_cost` has to be `Fn` instead of `FnMut`. 144 | #[cfg(feature = "parallel")] 145 | #[deprecated(since = "0.5.0", note = "`new` is automatically parallel")] 146 | pub fn new_parallel isize>( 147 | (width, height): (usize, usize), 148 | get_cost: F, 149 | neighborhood: N, 150 | config: PathCacheConfig, 151 | ) -> PathCache { 152 | PathCache::new_internal:: isize>( 153 | (width, height), 154 | CostFnWrapper::Parallel(get_cost), 155 | neighborhood, 156 | config, 157 | ) 158 | } 159 | 160 | fn new_internal( 161 | (width, height): (usize, usize), 162 | get_cost: CostFnWrapper, 163 | neighborhood: N, 164 | config: PathCacheConfig, 165 | ) -> PathCache 166 | where 167 | F1: Sync + Fn(Point) -> isize, 168 | F2: FnMut(Point) -> isize, 169 | { 170 | #[cfg(feature = "log")] 171 | let (outer_timer, timer) = (std::time::Instant::now(), std::time::Instant::now()); 172 | 173 | // calculate chunk size 174 | let (num_chunks_w, last_width) = { 175 | let w = width / config.chunk_size; 176 | let remain = width - w * config.chunk_size; 177 | if remain > 0 { 178 | (w + 1, remain) 179 | } else { 180 | (w, config.chunk_size) 181 | } 182 | }; 183 | let (num_chunks_h, last_height) = { 184 | let h = height / config.chunk_size; 185 | let remain = height - h * config.chunk_size; 186 | if remain > 0 { 187 | (h + 1, remain) 188 | } else { 189 | (h, config.chunk_size) 190 | } 191 | }; 192 | 193 | let mut nodes = NodeList::new(); 194 | 195 | // create chunks 196 | let chunks = match get_cost { 197 | CostFnWrapper::Sequential(mut get_cost, _) => { 198 | let mut chunks: Vec = Vec::with_capacity(num_chunks_w * num_chunks_h); 199 | for y in 0..num_chunks_h { 200 | let h = if y == num_chunks_h - 1 { 201 | last_height 202 | } else { 203 | config.chunk_size 204 | }; 205 | 206 | for x in 0..num_chunks_w { 207 | let w = if x == num_chunks_w - 1 { 208 | last_width 209 | } else { 210 | config.chunk_size 211 | }; 212 | 213 | chunks.push(Chunk::new( 214 | (x * config.chunk_size, y * config.chunk_size), 215 | (w, h), 216 | (width, height), 217 | &mut get_cost, 218 | &neighborhood, 219 | &mut nodes, 220 | config, 221 | )); 222 | } 223 | } 224 | 225 | re_trace!("create chunks", timer); 226 | 227 | chunks 228 | } 229 | #[cfg(feature = "parallel")] 230 | CostFnWrapper::Parallel(get_cost) => { 231 | use rayon::prelude::*; 232 | 233 | let (mut chunks, node_lists): (Vec<_>, Vec<_>) = (0..num_chunks_h * num_chunks_w) 234 | .into_par_iter() 235 | .map(|index| { 236 | let x = index % num_chunks_w; 237 | let y = index / num_chunks_w; 238 | 239 | let w = if x == num_chunks_w - 1 { 240 | last_width 241 | } else { 242 | config.chunk_size 243 | }; 244 | 245 | let h = if y == num_chunks_h - 1 { 246 | last_height 247 | } else { 248 | config.chunk_size 249 | }; 250 | 251 | let mut node_list = NodeList::new(); 252 | 253 | let chunk = Chunk::new( 254 | (x * config.chunk_size, y * config.chunk_size), 255 | (w, h), 256 | (width, height), 257 | &get_cost, 258 | &neighborhood, 259 | &mut node_list, 260 | config, 261 | ); 262 | 263 | (chunk, node_list) 264 | }) 265 | .collect(); 266 | 267 | re_trace!("create raw chunks", timer); 268 | 269 | chunks 270 | .iter_mut() 271 | .zip(node_lists) 272 | .map(|(chunk, new_nodes)| { 273 | chunk.nodes = nodes.absorb(new_nodes); 274 | chunk 275 | }) 276 | .to_vec(); 277 | 278 | re_trace!("absorb nodes", timer); 279 | 280 | chunks 281 | } 282 | }; 283 | 284 | let mut cache = PathCache { 285 | width, 286 | height, 287 | chunks, 288 | num_chunks: (num_chunks_w, num_chunks_h), 289 | nodes, 290 | neighborhood, 291 | config, 292 | }; 293 | 294 | // connect neighboring Nodes across Chunk borders 295 | cache.connect_nodes(None); 296 | 297 | re_trace!("connect nodes", timer); 298 | re_trace!("total time", outer_timer); 299 | 300 | cache 301 | } 302 | 303 | /// Calculates the Path from `start` to `goal` on the Grid. 304 | /// 305 | /// If no Path could be found, `None` is returned. 306 | /// 307 | /// `get_cost((x, y))` should return the cost for walking over the Tile at (x, y). 308 | /// Costs below 0 are solid Tiles. 309 | /// 310 | /// ## Examples 311 | /// Basic usage: 312 | /// ``` 313 | /// # use hierarchical_pathfinding::prelude::*; 314 | /// # let mut grid = [ 315 | /// # [0, 2, 0, 0, 0], 316 | /// # [0, 2, 2, 2, 2], 317 | /// # [0, 1, 0, 0, 0], 318 | /// # [0, 1, 0, 2, 0], 319 | /// # [0, 0, 0, 2, 0], 320 | /// # ]; 321 | /// # let (width, height) = (grid[0].len(), grid.len()); 322 | /// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 323 | /// # move |(x, y)| [1, 10, -1][grid[y][x]] 324 | /// # } 325 | /// let pathfinding: PathCache<_> = // ... 326 | /// # PathCache::new( 327 | /// # (width, height), 328 | /// # cost_fn(&grid), 329 | /// # ManhattanNeighborhood::new(width, height), 330 | /// # PathCacheConfig::with_chunk_size(3), 331 | /// # ); 332 | /// 333 | /// let start = (0, 0); 334 | /// let goal = (4, 4); 335 | /// 336 | /// // find_path returns Some(Path) on success 337 | /// let path = pathfinding.find_path( 338 | /// start, 339 | /// goal, 340 | /// cost_fn(&grid), 341 | /// ); 342 | /// 343 | /// assert!(path.is_some()); 344 | /// let path = path.unwrap(); 345 | /// 346 | /// assert_eq!(path.cost(), 12); 347 | /// ``` 348 | /// 349 | /// The return Value gives the total Cost of the Path using `cost()` and allows to iterate over 350 | /// the Points in the Path. 351 | /// 352 | /// **Note**: Setting [`config.cache_paths`](PathCacheConfig::cache_paths) to `false` means 353 | /// that the Paths need to be recalculated as needed. This means that for any sections of the 354 | /// Path that are not present, [`safe_next`](AbstractPath::safe_next) needs to be called to supply the Cost function. 355 | /// Calling `next` in that scenario would lead to a Panic. 356 | /// 357 | /// Using the Path: 358 | /// ``` 359 | /// # use hierarchical_pathfinding::prelude::*; 360 | /// # let mut grid = [ 361 | /// # [0, 2, 0, 0, 0], 362 | /// # [0, 2, 2, 2, 2], 363 | /// # [0, 1, 0, 0, 0], 364 | /// # [0, 1, 0, 2, 0], 365 | /// # [0, 0, 0, 2, 0], 366 | /// # ]; 367 | /// # let (width, height) = (grid[0].len(), grid.len()); 368 | /// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 369 | /// # move |(x, y)| [1, 10, -1][grid[y][x]] 370 | /// # } 371 | /// # let pathfinding = PathCache::new( 372 | /// # (width, height), 373 | /// # cost_fn(&grid), 374 | /// # ManhattanNeighborhood::new(width, height), 375 | /// # PathCacheConfig::with_chunk_size(3), 376 | /// # ); 377 | /// # struct Player{ pos: (usize, usize) } 378 | /// # impl Player { 379 | /// # pub fn move_to(&mut self, pos: (usize, usize)) { 380 | /// # self.pos = pos; 381 | /// # } 382 | /// # } 383 | /// # 384 | /// let mut player = Player { 385 | /// pos: (0, 0), 386 | /// //... 387 | /// }; 388 | /// let goal = (4, 4); 389 | /// 390 | /// let mut path = pathfinding.find_path( 391 | /// player.pos, 392 | /// goal, 393 | /// cost_fn(&grid), 394 | /// ).unwrap(); 395 | /// 396 | /// player.move_to(path.next().unwrap()); 397 | /// assert_eq!(player.pos, (0, 1)); 398 | /// 399 | /// // wait for next turn or whatever 400 | /// 401 | /// player.move_to(path.next().unwrap()); 402 | /// assert_eq!(player.pos, (0, 2)); 403 | /// ``` 404 | /// If the Grid changes, any Path objects still in use may become invalid. You can still 405 | /// use them if you are certain that nothing in relation to that Path changed, but it is 406 | /// discouraged and can lead to undefined behavior or panics. 407 | /// 408 | /// Obtaining the entire Path: 409 | /// ``` 410 | /// # use hierarchical_pathfinding::prelude::*; 411 | /// # let mut grid = [ 412 | /// # [0, 2, 0, 0, 0], 413 | /// # [0, 2, 2, 2, 2], 414 | /// # [0, 1, 0, 0, 0], 415 | /// # [0, 1, 0, 2, 0], 416 | /// # [0, 0, 0, 2, 0], 417 | /// # ]; 418 | /// # let (width, height) = (grid[0].len(), grid.len()); 419 | /// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 420 | /// # move |(x, y)| [1, 10, -1][grid[y][x]] 421 | /// # } 422 | /// # let pathfinding = PathCache::new( 423 | /// # (width, height), 424 | /// # cost_fn(&grid), 425 | /// # ManhattanNeighborhood::new(width, height), 426 | /// # PathCacheConfig::with_chunk_size(3), 427 | /// # ); 428 | /// # let start = (0, 0); 429 | /// # let goal = (4, 4); 430 | /// # let path = pathfinding.find_path( 431 | /// # start, 432 | /// # goal, 433 | /// # cost_fn(&grid), 434 | /// # ); 435 | /// // ... 436 | /// let path = path.unwrap(); 437 | /// 438 | /// let points: Vec<(usize, usize)> = path.collect(); 439 | /// assert_eq!( 440 | /// points, 441 | /// vec![(0, 1), (0, 2), (0, 3), (0, 4), (1, 4), (2, 4), 442 | /// (2, 3), (2, 2), (3, 2), (4, 2), (4, 3), (4, 4)], 443 | /// ); 444 | /// ``` 445 | pub fn find_path( 446 | &self, 447 | start: Point, 448 | goal: Point, 449 | mut get_cost: impl FnMut(Point) -> isize, 450 | ) -> Option> { 451 | #[cfg(feature = "log")] 452 | let (outer_timer, timer) = (std::time::Instant::now(), std::time::Instant::now()); 453 | 454 | if !self.in_bounds(start) { 455 | panic!( 456 | "start {:?} is out of bounds of a grid of size {}x{}", 457 | start, self.width, self.height 458 | ); 459 | } 460 | if !self.in_bounds(goal) { 461 | return None; 462 | } 463 | 464 | if get_cost(start) < 0 { 465 | // cannot start on a wall 466 | return None; 467 | } 468 | 469 | let neighborhood = self.neighborhood.clone(); 470 | 471 | if start == goal { 472 | return Some(AbstractPath::from_known_path( 473 | neighborhood, 474 | Path::from_slice(&[start, start], 0), 475 | )); 476 | } 477 | 478 | let (start_id, start_path) = 479 | if let Some(s) = self.find_nearest_node(start, &mut get_cost, false) { 480 | s 481 | } else { 482 | // no path from start to any Node => start is in cave within chunk 483 | // => hope that goal is in the same cave 484 | return self 485 | .get_chunk(start) 486 | .find_path(start, goal, get_cost, &neighborhood) 487 | .map(|path| AbstractPath::from_known_path(neighborhood, path)); 488 | }; 489 | 490 | // try-operator: see above, but we know that start is not in a cave 491 | let (goal_id, goal_path) = self.find_nearest_node(goal, &mut get_cost, true)?; 492 | 493 | re_trace!("find nodes", timer); 494 | 495 | // size hint for number of visited nodes in graph::a_star_search: 496 | // percentage of total area visited (heuristic / max_heuristic) 497 | // as the percentage of nodes visited ( * self.nodes.len()) 498 | let heuristic = neighborhood.heuristic(start, goal); 499 | let max_heuristic = neighborhood.heuristic((0, 0), (self.width - 1, self.height - 1)); 500 | let max_size = self.nodes.len(); 501 | let size_hint = heuristic as f32 / max_heuristic as f32 * max_size as f32; 502 | 503 | let path = graph::a_star_search( 504 | &self.nodes, 505 | start_id, 506 | goal_id, 507 | &neighborhood, 508 | size_hint as usize, 509 | )?; 510 | 511 | re_trace!("graph::a_star_search", timer); 512 | 513 | let mut paths = NodeIDMap::default(); 514 | paths.insert(goal_id, path); 515 | 516 | let mut ret_map = PointMap::default(); 517 | self.resolve_paths( 518 | start, 519 | start_path, 520 | &mut [(goal, goal_id, goal_path)], 521 | &paths, 522 | get_cost, 523 | &mut ret_map, 524 | ); 525 | 526 | re_trace!("resolve_paths", timer); 527 | re_trace!("total time", outer_timer); 528 | 529 | ret_map.remove(&goal) 530 | } 531 | 532 | /// Calculates the Paths from one `start` to several `goals` on the Grid. 533 | /// 534 | /// This is equivalent to [`find_path`](PathCache::find_path), except that it is optimized to handle multiple Goals 535 | /// at once. However, it is slower for very few goals, since it does not use a heuristic like 536 | /// [`find_path`](PathCache::find_path) does. 537 | /// 538 | /// Instead of returning a single Option, it returns a Hashmap, where the position of the Goal 539 | /// is the key, and the Value is a Tuple of the Path and the Cost of that Path. 540 | /// 541 | /// `get_cost((x, y))` should return the cost for walking over the Tile at (x, y). 542 | /// Costs below 0 are solid Tiles. 543 | /// 544 | /// See [`find_path`](PathCache::find_path) for more details on how to use the returned Paths. 545 | /// 546 | /// ## Examples 547 | /// Basic usage: 548 | /// ``` 549 | /// # use hierarchical_pathfinding::prelude::*; 550 | /// # let mut grid = [ 551 | /// # [0, 2, 0, 0, 0], 552 | /// # [0, 2, 2, 2, 2], 553 | /// # [0, 1, 0, 0, 0], 554 | /// # [0, 1, 0, 2, 0], 555 | /// # [0, 0, 0, 2, 0], 556 | /// # ]; 557 | /// # let (width, height) = (grid[0].len(), grid.len()); 558 | /// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 559 | /// # move |(x, y)| [1, 10, -1][grid[y][x]] 560 | /// # } 561 | /// let pathfinding: PathCache<_> = // ... 562 | /// # PathCache::new( 563 | /// # (width, height), 564 | /// # cost_fn(&grid), 565 | /// # ManhattanNeighborhood::new(width, height), 566 | /// # PathCacheConfig::with_chunk_size(3), 567 | /// # ); 568 | /// 569 | /// let start = (0, 0); 570 | /// let goals = [(4, 4), (2, 0)]; 571 | /// 572 | /// // find_paths returns a HashMap for all successes 573 | /// let paths = pathfinding.find_paths( 574 | /// start, 575 | /// &goals, 576 | /// cost_fn(&grid), 577 | /// ); 578 | /// 579 | /// // (4, 4) is reachable 580 | /// assert!(paths.contains_key(&goals[0])); 581 | /// 582 | /// // (2, 0) is not reachable 583 | /// assert!(!paths.contains_key(&goals[1])); 584 | /// ``` 585 | /// 586 | /// The returned Path is always equivalent to the one returned by [`find_path`](PathCache::find_path): 587 | /// ``` 588 | /// # use hierarchical_pathfinding::prelude::*; 589 | /// # let mut grid = [ 590 | /// # [0, 2, 0, 0, 0], 591 | /// # [0, 2, 2, 2, 2], 592 | /// # [0, 1, 0, 0, 0], 593 | /// # [0, 1, 0, 2, 0], 594 | /// # [0, 0, 0, 2, 0], 595 | /// # ]; 596 | /// # let (width, height) = (grid[0].len(), grid.len()); 597 | /// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 598 | /// # move |(x, y)| [1, 10, -1][grid[y][x]] 599 | /// # } 600 | /// # let pathfinding = PathCache::new( 601 | /// # (width, height), 602 | /// # cost_fn(&grid), 603 | /// # ManhattanNeighborhood::new(width, height), 604 | /// # PathCacheConfig::with_chunk_size(3), 605 | /// # ); 606 | /// let start = (0, 0); 607 | /// let goal = (4, 4); 608 | /// 609 | /// let paths = pathfinding.find_paths( 610 | /// start, 611 | /// &[goal], 612 | /// cost_fn(&grid), 613 | /// ); 614 | /// let dijkstra_path: Vec<_> = paths[&goal].clone().collect(); 615 | /// 616 | /// let a_star_path: Vec<_> = pathfinding.find_path( 617 | /// start, 618 | /// goal, 619 | /// cost_fn(&grid), 620 | /// ).unwrap().collect(); 621 | /// 622 | /// assert_eq!(dijkstra_path, a_star_path); 623 | /// ``` 624 | pub fn find_paths( 625 | &self, 626 | start: Point, 627 | goals: &[Point], 628 | get_cost: impl FnMut(Point) -> isize, 629 | ) -> PointMap> { 630 | self.find_paths_internal(start, goals, get_cost, false) 631 | } 632 | 633 | /// Finds the closest from a list of goals. 634 | /// 635 | /// Returns a tuple of the goal and the Path to that goal, or `None` if none of the goals are 636 | /// reachable. 637 | /// 638 | /// Similar to [`find_paths`](PathCache::find_paths) in performance and search strategy, but 639 | /// stops after the first goal is found. 640 | /// 641 | /// ## Examples 642 | /// Basic usage: 643 | /// ``` 644 | /// # use hierarchical_pathfinding::prelude::*; 645 | /// # let mut grid = [ 646 | /// # [0, 2, 0, 0, 0], 647 | /// # [0, 2, 2, 2, 2], 648 | /// # [0, 1, 0, 0, 0], 649 | /// # [0, 1, 0, 2, 0], 650 | /// # [0, 0, 0, 2, 0], 651 | /// # ]; 652 | /// # let (width, height) = (grid[0].len(), grid.len()); 653 | /// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 654 | /// # move |(x, y)| [1, 10, -1][grid[y][x]] 655 | /// # } 656 | /// let pathfinding: PathCache<_> = // ... 657 | /// # PathCache::new( 658 | /// # (width, height), 659 | /// # cost_fn(&grid), 660 | /// # ManhattanNeighborhood::new(width, height), 661 | /// # PathCacheConfig::with_chunk_size(3), 662 | /// # ); 663 | /// 664 | /// let start = (0, 0); 665 | /// let goals = [(4, 4), (2, 0), (2, 2)]; 666 | /// 667 | /// // find_closest_goal returns Some((goal, Path)) on success 668 | /// let (goal, path) = pathfinding.find_closest_goal( 669 | /// start, 670 | /// &goals, 671 | /// cost_fn(&grid), 672 | /// ).unwrap(); 673 | /// 674 | /// assert_eq!(goal, goals[2]); 675 | /// 676 | /// let naive_closest = pathfinding 677 | /// .find_paths(start, &goals, cost_fn(&grid)) 678 | /// .into_iter() 679 | /// .min_by_key(|(_, path)| path.cost()) 680 | /// .unwrap(); 681 | /// 682 | /// assert_eq!(goal, naive_closest.0); 683 | /// 684 | /// let path: Vec<_> = path.collect(); 685 | /// let naive_path: Vec<_> = naive_closest.1.collect(); 686 | /// assert_eq!(path, naive_path); 687 | /// ``` 688 | /// Comparison with [`find_paths`](PathCache::find_paths): 689 | /// ``` 690 | /// # use hierarchical_pathfinding::prelude::*; 691 | /// # let mut grid = [ 692 | /// # [0, 2, 0, 0, 0], 693 | /// # [0, 2, 2, 2, 2], 694 | /// # [0, 1, 0, 0, 0], 695 | /// # [0, 1, 0, 2, 0], 696 | /// # [0, 0, 0, 2, 0], 697 | /// # ]; 698 | /// # let (width, height) = (grid[0].len(), grid.len()); 699 | /// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 700 | /// # move |(x, y)| [1, 10, -1][grid[y][x]] 701 | /// # } 702 | /// # let pathfinding = PathCache::new( 703 | /// # (width, height), 704 | /// # cost_fn(&grid), 705 | /// # ManhattanNeighborhood::new(width, height), 706 | /// # PathCacheConfig::with_chunk_size(3), 707 | /// # ); 708 | /// # let start = (0, 0); 709 | /// # let goals = [(4, 4), (2, 0), (2, 2)]; 710 | /// let (goal, path) = pathfinding.find_closest_goal( 711 | /// start, 712 | /// &goals, 713 | /// cost_fn(&grid), 714 | /// ).unwrap(); 715 | /// 716 | /// let naive_closest = pathfinding 717 | /// .find_paths(start, &goals, cost_fn(&grid)) 718 | /// .into_iter() 719 | /// .min_by_key(|(_, path)| path.cost()) 720 | /// .unwrap(); 721 | /// 722 | /// assert_eq!(goal, naive_closest.0); 723 | /// 724 | /// let path: Vec<_> = path.collect(); 725 | /// let naive_path: Vec<_> = naive_closest.1.collect(); 726 | /// assert_eq!(path, naive_path); 727 | /// ``` 728 | pub fn find_closest_goal( 729 | &self, 730 | start: Point, 731 | goals: &[Point], 732 | get_cost: impl FnMut(Point) -> isize, 733 | ) -> Option<(Point, AbstractPath)> { 734 | self.find_paths_internal(start, goals, get_cost, true) 735 | .into_iter() 736 | .next() 737 | } 738 | 739 | fn find_paths_internal( 740 | &self, 741 | start: Point, 742 | goals: &[Point], 743 | mut get_cost: impl FnMut(Point) -> isize, 744 | only_closest_goal: bool, 745 | ) -> PointMap> { 746 | if !self.in_bounds(start) { 747 | panic!( 748 | "start {:?} is out of bounds of a grid of size {}x{}", 749 | start, self.width, self.height 750 | ); 751 | } 752 | if get_cost(start) < 0 || goals.is_empty() { 753 | return PointMap::default(); 754 | } 755 | 756 | if goals.len() == 1 { 757 | let goal = goals[0]; 758 | return self 759 | .find_path(start, goal, get_cost) 760 | .map(|path| (goal, path)) 761 | .into_iter() 762 | .collect(); 763 | } 764 | 765 | let neighborhood = self.neighborhood.clone(); 766 | 767 | let (start_id, start_path) = 768 | if let Some(s) = self.find_nearest_node(start, &mut get_cost, false) { 769 | s 770 | } else { 771 | // no path from start to any Node => start is in cave within chunk 772 | // => find all goals in the same cave 773 | return self 774 | .get_chunk(start) 775 | .find_paths(start, goals, get_cost, &neighborhood) 776 | .into_iter() 777 | .map(|(goal, path)| { 778 | ( 779 | goal, 780 | AbstractPath::from_known_path(neighborhood.clone(), path), 781 | ) 782 | }) 783 | .collect(); 784 | }; 785 | 786 | let mut goal_data = Vec::with_capacity(goals.len()); 787 | let mut goal_ids = Vec::with_capacity(goals.len()); 788 | 789 | let mut ret = PointMap::default(); 790 | let mut heuristic = 0; 791 | 792 | for goal in goals.iter().copied() { 793 | if goal == start { 794 | let path = AbstractPath::from_known_path( 795 | self.neighborhood.clone(), 796 | Path::from_slice(&[start, start], 0), 797 | ); 798 | ret.insert(goal, path); 799 | continue; 800 | } 801 | 802 | if !self.in_bounds(goal) { 803 | continue; 804 | } 805 | 806 | let (goal_id, goal_path) = 807 | if let Some(g) = self.find_nearest_node(goal, &mut get_cost, true) { 808 | g 809 | } else { 810 | // goal is in a cave within a chunk. If it was the same cave as start, 811 | // then we would have already stopped at the `find_nearest_node` for start. 812 | // Since we didn't, we know that goal is in a different cave that is not 813 | // reachable from the node network. 814 | continue; 815 | }; 816 | 817 | goal_data.push((goal, goal_id, goal_path)); 818 | goal_ids.push(goal_id); 819 | if only_closest_goal { 820 | heuristic = heuristic.min(self.neighborhood.heuristic(start, goal)); 821 | } else { 822 | heuristic = heuristic.max(self.neighborhood.heuristic(start, goal)); 823 | } 824 | } 825 | 826 | let max_heuristic = neighborhood.heuristic((0, 0), (self.width - 1, self.height - 1)); 827 | let max_size = self.nodes.len(); 828 | let size_hint = heuristic as f32 / max_heuristic as f32 * max_size as f32; 829 | 830 | let paths = graph::dijkstra_search( 831 | &self.nodes, 832 | start_id, 833 | &goal_ids, 834 | only_closest_goal, 835 | size_hint as usize, 836 | ); 837 | 838 | self.resolve_paths( 839 | start, 840 | start_path, 841 | &mut goal_data, 842 | &paths, 843 | get_cost, 844 | &mut ret, 845 | ); 846 | ret 847 | } 848 | 849 | /// Notifies the `PathCache` that the Grid changed. 850 | /// 851 | /// This Method updates any internal Paths that might have changed when the Grid changed. This 852 | /// is an expensive operation and should only be performed if the change affected the walking 853 | /// cost of a tile and the `PathCache` is needed again. If possible, try to bundle as many 854 | /// changes as possible into a single call to `tiles_changed` to avoid unnecessary 855 | /// recalculations. 856 | /// 857 | /// Side note: if anybody has a way to improve this method, open a GitHub Issue / Pull Request. 858 | /// 859 | /// ## Examples 860 | /// Basic usage: 861 | /// ``` 862 | /// # use hierarchical_pathfinding::prelude::*; 863 | /// # let mut grid = [ 864 | /// # [0, 2, 0, 0, 0], 865 | /// # [0, 2, 2, 2, 2], 866 | /// # [0, 1, 0, 0, 0], 867 | /// # [0, 1, 0, 2, 0], 868 | /// # [0, 0, 0, 2, 0], 869 | /// # ]; 870 | /// # let (width, height) = (grid[0].len(), grid.len()); 871 | /// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 872 | /// # move |(x, y)| [1, 10, -1][grid[y][x]] 873 | /// # } 874 | /// let mut pathfinding: PathCache<_> = // ... 875 | /// # PathCache::new( 876 | /// # (width, height), 877 | /// # cost_fn(&grid), 878 | /// # ManhattanNeighborhood::new(width, height), 879 | /// # PathCacheConfig::with_chunk_size(3), 880 | /// # ); 881 | /// 882 | /// let (start, goal) = ((0, 0), (2, 0)); 883 | /// 884 | /// let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 885 | /// assert!(path.is_none()); 886 | /// 887 | /// grid[1][2] = 0; 888 | /// grid[3][2] = 2; 889 | /// 890 | /// assert_eq!(grid, [ 891 | /// [0, 2, 0, 0, 0], 892 | /// [0, 2, 0, 2, 2], 893 | /// [0, 1, 0, 0, 0], 894 | /// [0, 1, 2, 2, 0], 895 | /// [0, 0, 0, 2, 0], 896 | /// ]); 897 | /// 898 | /// pathfinding.tiles_changed( 899 | /// &[(2, 1), (2, 3)], 900 | /// cost_fn(&grid), 901 | /// ); 902 | /// 903 | /// let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 904 | /// assert!(path.is_some()); 905 | /// ``` 906 | pub fn tiles_changed isize>(&mut self, tiles: &[Point], get_cost: F) { 907 | #[cfg(feature = "parallel")] 908 | { 909 | self.tiles_changed_internal:: isize>( 910 | tiles, 911 | CostFnWrapper::Parallel(get_cost), 912 | ); 913 | } 914 | #[cfg(not(feature = "parallel"))] 915 | { 916 | self.tiles_changed_internal:: isize, F>( 917 | tiles, 918 | CostFnWrapper::Sequential(get_cost, PhantomData), 919 | ); 920 | } 921 | } 922 | 923 | /// Same as [`tiles_changed`](PathCache::tiles_changed), but doesn't use threads to allow [`FnMut`]. 924 | /// 925 | /// Equivalent to `tiles_changed` if `parallel` feature is disabled. 926 | /// 927 | /// Note that this is _**way**_ slower than `tiles_changed` with `parallel`. 928 | pub fn tiles_changed_with_fn_mut isize>( 929 | &mut self, 930 | tiles: &[Point], 931 | get_cost: F, 932 | ) { 933 | self.tiles_changed_internal:: isize, F>( 934 | tiles, 935 | CostFnWrapper::Sequential(get_cost, PhantomData), 936 | ); 937 | } 938 | 939 | fn tiles_changed_internal( 940 | &mut self, 941 | tiles: &[Point], 942 | mut get_cost: CostFnWrapper, 943 | ) where 944 | F1: Sync + Fn(Point) -> isize, 945 | F2: FnMut(Point) -> isize, 946 | { 947 | let size = self.config.chunk_size; 948 | 949 | #[cfg(feature = "log")] 950 | let (outer_timer, timer) = (std::time::Instant::now(), std::time::Instant::now()); 951 | 952 | let mut dirty = PointMap::default(); 953 | for &p in tiles { 954 | let chunk_pos = self.get_chunk_pos(p); 955 | dirty.entry(chunk_pos).or_insert_with(Vec::new).push(p); 956 | } 957 | 958 | #[derive(Debug, Clone, Copy, PartialEq, Eq)] 959 | enum Renew { 960 | No, 961 | Inner, 962 | Corner(Point), 963 | All, 964 | } 965 | 966 | // map of chunk_pos => array: [Renew; 4] where array[side] says if chunk[side] needs to be renewed 967 | let mut renew = PointMap::default(); 968 | 969 | for (&cp, positions) in dirty.iter() { 970 | let chunk = self.get_chunk(cp); 971 | // for every changed tile in the chunk 972 | for &p in positions { 973 | // check every side that this tile is on 974 | for dir in Dir::all().filter(|dir| chunk.sides[dir.num()] && chunk.at_side(p, *dir)) 975 | { 976 | // if there is a chunk in that direction 977 | let other_pos = jump_in_dir(cp, dir, size, (0, 0), (self.width, self.height)) 978 | .expect("Internal Error #2 in PathCache. Please report this"); 979 | 980 | // mark the current and other side 981 | let own = &mut renew.entry(cp).or_insert([Renew::No; 4])[dir.num()]; 982 | let old = *own; 983 | if chunk.is_corner(p) { 984 | if old == Renew::No || old == Renew::Inner { 985 | *own = Renew::Corner(p); 986 | } else if let Renew::Corner(p2) = old { 987 | if p2 != p { 988 | *own = Renew::All; 989 | } 990 | } else if old != Renew::All { 991 | *own = Renew::Corner(p) 992 | } 993 | } else { 994 | // All > Corner > Inner > No, and we don't want to override anything greater than Inner 995 | if *own == Renew::No { 996 | *own = Renew::Inner; 997 | } 998 | } 999 | let other = 1000 | &mut renew.entry(other_pos).or_insert([Renew::No; 4])[dir.opposite().num()]; 1001 | if *other == Renew::No { 1002 | *other = Renew::Inner; 1003 | } 1004 | } 1005 | } 1006 | } 1007 | 1008 | re_trace!("establish renew", timer); 1009 | 1010 | // remove all nodes of sides in renew 1011 | 1012 | for (&cp, sides) in renew.iter() { 1013 | let chunk_index = self.get_chunk_index(cp); 1014 | let chunk = &self.chunks[chunk_index]; 1015 | let removed = chunk 1016 | .nodes 1017 | .iter() 1018 | .filter(|id| { 1019 | let pos = self.nodes[**id].pos; 1020 | let corner = chunk.is_corner(pos); 1021 | Dir::all().any(|dir| match sides[dir.num()] { 1022 | Renew::No => false, 1023 | Renew::Inner => !corner, 1024 | Renew::Corner(c) => !corner || c == pos, 1025 | Renew::All => true, 1026 | } && chunk.at_side(pos, dir)) 1027 | }) 1028 | .copied() 1029 | .to_vec(); 1030 | 1031 | let chunk = &mut self.chunks[chunk_index]; 1032 | 1033 | for id in removed { 1034 | chunk.nodes.remove(&id); 1035 | self.nodes.remove_node(id); 1036 | } 1037 | } 1038 | 1039 | re_trace!("remove nodes of sides in renew", timer); 1040 | 1041 | let mut changed_nodes = NodeIDSet::default(); 1042 | 1043 | // remove all Paths in changed chunks 1044 | for cp in dirty.keys() { 1045 | let chunk_index = self.get_chunk_index(*cp); 1046 | let chunk = &self.chunks[chunk_index]; 1047 | for id in chunk.nodes.iter() { 1048 | self.nodes[*id].edges.clear(); 1049 | } 1050 | } 1051 | 1052 | { 1053 | let mut get_cost: &mut dyn FnMut(Point) -> isize = match &mut get_cost { 1054 | CostFnWrapper::Sequential(get_cost, _) => get_cost, 1055 | #[cfg(feature = "parallel")] 1056 | CostFnWrapper::Parallel(get_cost) => get_cost, 1057 | }; 1058 | 1059 | // recreate sides in renew 1060 | for (&cp, sides) in renew.iter() { 1061 | let mut candidates = PointSet::default(); 1062 | let chunk_index = self.get_chunk_index(cp); 1063 | let chunk = &self.chunks[chunk_index]; 1064 | 1065 | for dir in Dir::all() { 1066 | if sides[dir.num()] != Renew::No { 1067 | chunk.calculate_side_nodes( 1068 | dir, 1069 | (self.width, self.height), 1070 | &mut get_cost, 1071 | self.config, 1072 | &mut candidates, 1073 | ); 1074 | } 1075 | } 1076 | 1077 | // Only include nodes that aren't already part of the map 1078 | candidates.retain(|&pos| self.nodes.id_at(pos).is_none()); 1079 | 1080 | if candidates.is_empty() { 1081 | continue; 1082 | } 1083 | 1084 | let all_nodes = &mut self.nodes; 1085 | let nodes = candidates 1086 | .into_iter() 1087 | .map(|p| all_nodes.add_node(p, get_cost(p) as usize)) 1088 | .to_vec(); 1089 | 1090 | let chunk = &mut self.chunks[chunk_index]; 1091 | if !dirty.contains_key(&cp) { 1092 | for node in nodes.iter() { 1093 | changed_nodes.insert(*node); 1094 | } 1095 | chunk.add_nodes( 1096 | &nodes, 1097 | &mut get_cost, 1098 | &self.neighborhood, 1099 | &mut self.nodes, 1100 | &self.config, 1101 | ); 1102 | } else { 1103 | for id in nodes { 1104 | chunk.nodes.insert(id); 1105 | } 1106 | } 1107 | } 1108 | } 1109 | 1110 | re_trace!("recreates sides in renew", timer); 1111 | 1112 | match get_cost { 1113 | CostFnWrapper::Sequential(mut get_cost, _) => { 1114 | for cp in dirty.keys() { 1115 | let chunk_index = self.get_chunk_index(*cp); 1116 | let chunk = &mut self.chunks[chunk_index]; 1117 | let nodes = chunk.nodes.iter().copied().to_vec(); 1118 | 1119 | for node in nodes.iter() { 1120 | changed_nodes.insert(*node); 1121 | } 1122 | 1123 | chunk.nodes.clear(); 1124 | chunk.add_nodes( 1125 | &nodes, 1126 | &mut get_cost, 1127 | &self.neighborhood, 1128 | &mut self.nodes, 1129 | &self.config, 1130 | ); 1131 | } 1132 | re_trace!("recreate Paths", timer); 1133 | } 1134 | #[cfg(feature = "parallel")] 1135 | CostFnWrapper::Parallel(get_cost) => { 1136 | use rayon::prelude::*; 1137 | let dirty_indices: hashbrown::HashSet = dirty 1138 | .keys() 1139 | .map(|(x, y)| self.get_chunk_index((*x, *y))) 1140 | .collect(); 1141 | 1142 | let paths: Vec<_> = { 1143 | let neighborhood = &self.neighborhood; 1144 | let all_nodes = &self.nodes; 1145 | let cache_paths = self.config.cache_paths; 1146 | 1147 | self.chunks 1148 | .par_iter() 1149 | .enumerate() 1150 | .filter(|(chunk_index, _)| dirty_indices.contains(chunk_index)) 1151 | .map(|(_, chunk)| { 1152 | chunk.connect_nodes_parallel( 1153 | &get_cost, 1154 | neighborhood, 1155 | all_nodes, 1156 | cache_paths, 1157 | ) 1158 | }) 1159 | .collect() 1160 | }; 1161 | 1162 | re_trace!("get paths", timer); 1163 | 1164 | for (id, other_id, path) in paths.into_iter().flatten() { 1165 | self.nodes.add_edge(id, other_id, path); 1166 | } 1167 | 1168 | for chunk_index in dirty_indices.iter() { 1169 | for node in self.chunks[*chunk_index].nodes.iter() { 1170 | changed_nodes.insert(*node); 1171 | } 1172 | } 1173 | 1174 | re_trace!("update edges", timer); 1175 | } 1176 | } 1177 | 1178 | // re-establish cross-chunk connections 1179 | self.connect_nodes(Some(changed_nodes)); 1180 | 1181 | re_trace!("connect nodes", timer); 1182 | re_trace!("total time", outer_timer); 1183 | } 1184 | 1185 | /// Allows for debugging and visualizing the `PathCache` 1186 | /// 1187 | /// The returned object gives read-only access to the current state of the `PathCache`, mainly the 1188 | /// Nodes and how they are connected to each other 1189 | /// 1190 | /// ## Examples 1191 | /// Basic usage: 1192 | /// ``` 1193 | /// # use hierarchical_pathfinding::prelude::*; 1194 | /// # let mut grid = [ 1195 | /// # [0, 2, 0, 0, 0], 1196 | /// # [0, 2, 2, 2, 2], 1197 | /// # [0, 1, 0, 0, 0], 1198 | /// # [0, 1, 0, 2, 0], 1199 | /// # [0, 0, 0, 2, 0], 1200 | /// # ]; 1201 | /// # let (width, height) = (grid[0].len(), grid.len()); 1202 | /// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 1203 | /// # move |(x, y)| [1, 10, -1][grid[y][x]] 1204 | /// # } 1205 | /// let pathfinding: PathCache<_> = // ... 1206 | /// # PathCache::new( 1207 | /// # (width, height), 1208 | /// # cost_fn(&grid), 1209 | /// # ManhattanNeighborhood::new(width, height), 1210 | /// # PathCacheConfig::with_chunk_size(3), 1211 | /// # ); 1212 | /// 1213 | /// // only draw the connections between Nodes once 1214 | /// # use std::collections::HashSet; 1215 | /// let mut visited = HashSet::new(); 1216 | /// 1217 | /// for node in pathfinding.inspect_nodes() { 1218 | /// let pos = node.pos(); 1219 | /// // draw Node at x: pos.0, y: pos.1 1220 | /// 1221 | /// visited.insert(node.id()); 1222 | /// 1223 | /// for (neighbor, cost) in node.connected().filter(|(n, _)| !visited.contains(&n.id())) { 1224 | /// let other_pos = neighbor.pos(); 1225 | /// // draw Line from pos to other_pos, colored by cost 1226 | /// } 1227 | /// } 1228 | /// ``` 1229 | pub fn inspect_nodes(&self) -> CacheInspector { 1230 | CacheInspector::new(self) 1231 | } 1232 | 1233 | /// Prints all Nodes 1234 | #[allow(dead_code)] 1235 | fn print_nodes(&self) { 1236 | for node in self.inspect_nodes() { 1237 | print!("{} at {:?}: ", node.id(), node.pos()); 1238 | 1239 | for (neighbor, cost) in node.connected() { 1240 | print!("{:?}({}), ", neighbor.pos(), cost); 1241 | } 1242 | 1243 | println!(); 1244 | } 1245 | } 1246 | 1247 | fn in_bounds(&self, point: Point) -> bool { 1248 | point.0 < self.width && point.1 < self.height 1249 | } 1250 | 1251 | fn get_chunk_pos(&self, point: Point) -> Point { 1252 | let size = self.config.chunk_size; 1253 | ((point.0 / size) * size, (point.1 / size) * size) 1254 | } 1255 | 1256 | fn get_chunk(&self, point: Point) -> &Chunk { 1257 | let index = self.get_chunk_index(point); 1258 | &self.chunks[index] 1259 | } 1260 | 1261 | fn get_chunk_index(&self, point: Point) -> usize { 1262 | let size = self.config.chunk_size; 1263 | let (x, y) = ((point.0 / size), (point.1 / size)); 1264 | y * self.num_chunks.0 + x 1265 | } 1266 | 1267 | fn same_chunk(&self, a: Point, b: Point) -> bool { 1268 | let size = self.config.chunk_size; 1269 | a.0 / size == b.0 / size && a.1 / size == b.1 / size 1270 | } 1271 | 1272 | fn node_at(&self, pos: Point) -> Option { 1273 | self.nodes.id_at(pos) 1274 | } 1275 | 1276 | /// Returns the config used to create this `PathCache` 1277 | pub fn config(&self) -> &PathCacheConfig { 1278 | &self.config 1279 | } 1280 | 1281 | fn find_nearest_node( 1282 | &self, 1283 | pos: Point, 1284 | get_cost: impl FnMut(Point) -> isize, 1285 | reverse: bool, 1286 | ) -> Option<(NodeID, Option>)> { 1287 | if let Some(id) = self.node_at(pos) { 1288 | return Some((id, None)); 1289 | } 1290 | self.get_chunk(pos) 1291 | .nearest_node(&self.nodes, pos, get_cost, &self.neighborhood, reverse) 1292 | .map(|(id, path)| (id, Some(path))) 1293 | } 1294 | 1295 | fn grid_a_star( 1296 | &self, 1297 | start: Point, 1298 | goal: Point, 1299 | get_cost: impl FnMut(Point) -> isize, 1300 | ) -> Option> { 1301 | let heuristic = self.neighborhood.heuristic(start, goal); 1302 | let max_heuristic = self 1303 | .neighborhood 1304 | .heuristic((0, 0), (self.width - 1, self.height - 1)); 1305 | let max_size = self.width * self.height; 1306 | let size_hint = heuristic as f32 / max_heuristic as f32 * max_size as f32; 1307 | 1308 | grid::a_star_search( 1309 | &self.neighborhood, 1310 | |_| true, 1311 | get_cost, 1312 | start, 1313 | goal, 1314 | size_hint as usize, 1315 | ) 1316 | } 1317 | 1318 | fn resolve_paths( 1319 | &self, 1320 | start: Point, 1321 | start_path: Option>, 1322 | goal_data: &mut [(Point, NodeID, Option>)], 1323 | paths: &NodeIDMap>, 1324 | mut get_cost: impl FnMut(Point) -> isize, 1325 | out: &mut PointMap>, 1326 | ) { 1327 | // a map for direct paths from the start to other nodes in the same chunk as start. 1328 | // see `start_path` calculation below 1329 | let mut start_path_map = PointMap::default(); 1330 | 1331 | for (goal, goal_id, goal_path) in goal_data { 1332 | let path = if let Some(path) = paths.get(goal_id) { 1333 | path 1334 | } else { 1335 | continue; 1336 | }; 1337 | 1338 | if path.len() == 1 1339 | || (self.config.a_star_fallback && path.cost() < 2 * self.config.chunk_size) 1340 | { 1341 | // len == 1: start_id == goal_id 1342 | let res = self 1343 | .grid_a_star(start, *goal, &mut get_cost) 1344 | .map(|path| AbstractPath::from_known_path(self.neighborhood.clone(), path)) 1345 | .expect("Inconsistency in Pathfinding"); 1346 | 1347 | out.insert(*goal, res); 1348 | continue; 1349 | } 1350 | 1351 | let path = path.iter().copied().to_vec(); 1352 | let mut path = path.as_slice(); 1353 | 1354 | let mut start_path = start_path.as_ref(); 1355 | if start_path.is_none() { 1356 | // start is itself a node, so leave the start as is 1357 | } else { 1358 | // start_path connects start to *some* node in the chunk, which is not necessarily 1359 | // the best node to start from. 1360 | // => find a direct path to the node furthest into the path that is still in the 1361 | // same chunk as start 1362 | let candidate = path 1363 | .iter() 1364 | .map(|&id| self.nodes[id].pos) 1365 | .chain(std::iter::once(*goal)) 1366 | .enumerate() 1367 | .skip(1) // skip the current candidate 1368 | .take_while(|(_, pos)| self.same_chunk(start, *pos)) 1369 | .last(); 1370 | 1371 | if let Some((index, next_pos)) = candidate { 1372 | let new_start_path = start_path_map.entry(next_pos).or_insert_with(|| { 1373 | // this path is guaranteed to be within this chunk, because all nodes 1374 | // between start and candidate are in the same chunk as start 1375 | // and paths between nodes are either fully within a chunk or the 1376 | // nodes are in different chunks 1377 | self.get_chunk(start) 1378 | .find_path(start, next_pos, &mut get_cost, &self.neighborhood) 1379 | .expect("Inconsistency in Pathfinding") 1380 | }); 1381 | 1382 | if next_pos == *goal { 1383 | out.insert( 1384 | *goal, 1385 | AbstractPath::from_known_path( 1386 | self.neighborhood.clone(), 1387 | new_start_path.clone(), 1388 | ), 1389 | ); 1390 | continue; 1391 | } 1392 | 1393 | start_path = Some(new_start_path); 1394 | path = &path[index..]; 1395 | } 1396 | } 1397 | 1398 | let mut goal_path = goal_path.take(); 1399 | if goal_path.is_none() { 1400 | // goal is itself a node, so leave the goal as is 1401 | } else { 1402 | // same as with start_path, but for the goal 1403 | let candidate = path 1404 | .iter() 1405 | .enumerate() 1406 | .rev() 1407 | .skip(1) // skip the current candidate 1408 | .take_while(|(_, &id)| self.same_chunk(*goal, self.nodes[id].pos)) 1409 | .last(); 1410 | 1411 | if let Some((index, id)) = candidate { 1412 | let previous_pos = self.nodes[*id].pos; 1413 | let new_goal_path = self 1414 | .get_chunk(*goal) 1415 | .find_path(previous_pos, *goal, &mut get_cost, &self.neighborhood) 1416 | .expect("Inconsistency in Pathfinding"); 1417 | 1418 | goal_path = Some(new_goal_path); 1419 | path = &path[..=index]; 1420 | } 1421 | } 1422 | 1423 | let mut final_path = AbstractPath::new(self.neighborhood.clone(), start); 1424 | 1425 | if let Some(path) = start_path { 1426 | final_path.add_path(path.clone()); 1427 | } 1428 | 1429 | for (a, b) in path.windows(2).map(|w| (w[0], w[1])) { 1430 | final_path.add_path_segment(self.nodes[a].edges[&b].clone()); 1431 | } 1432 | 1433 | if let Some(path) = goal_path { 1434 | final_path.add_path(path); 1435 | } 1436 | 1437 | out.insert(*goal, final_path); 1438 | } 1439 | } 1440 | 1441 | fn connect_nodes(&mut self, ids: Option) { 1442 | let mut target = vec![]; 1443 | let mut new_paths = vec![]; 1444 | let mut seen = NodeIDSet::default(); 1445 | 1446 | // we iterate over ids if they exist or self.nodes otherwise, which cannot be unified 1447 | // without allocations, so we extract the body of the loop as a function instead 1448 | let convert = |(id, node): (NodeID, &Node)| { 1449 | seen.insert(id); 1450 | 1451 | target.clear(); 1452 | self.neighborhood.get_all_neighbors(node.pos, &mut target); 1453 | for &other_pos in target.iter() { 1454 | if let Some(other_id) = self.node_at(other_pos) { 1455 | if node.edges.contains_key(&other_id) || seen.contains(&other_id) { 1456 | continue; 1457 | } 1458 | let path = PathSegment::new( 1459 | Path::from_slice(&[node.pos, other_pos], node.walk_cost), 1460 | self.config.cache_paths, 1461 | ); 1462 | new_paths.push((id, other_id, path)); 1463 | } 1464 | } 1465 | }; 1466 | match ids { 1467 | Some(ids) => ids 1468 | .iter() 1469 | .map(|&id| (id, &self.nodes[id])) 1470 | .for_each(convert), 1471 | None => self.nodes.iter().for_each(convert), 1472 | }; 1473 | 1474 | for (id, other_id, path) in new_paths { 1475 | self.nodes.add_edge(id, other_id, path); 1476 | } 1477 | } 1478 | } 1479 | 1480 | /// Allows for debugging and visualizing a PathCache. 1481 | /// 1482 | /// See [`inspect_nodes`](PathCache::inspect_nodes) for details and an example. 1483 | /// 1484 | /// Allows iteration over all Nodes and specific lookup with [`get_node`](CacheInspector::get_node) 1485 | #[derive(Debug)] 1486 | pub struct CacheInspector<'a, N: Neighborhood> { 1487 | src: &'a PathCache, 1488 | inner: slab::Iter<'a, Node>, 1489 | } 1490 | 1491 | impl<'a, N: Neighborhood> CacheInspector<'a, N> { 1492 | /// Creates a new CacheInspector 1493 | /// 1494 | /// Same as calling [`.inspect_nodes()`](PathCache::inspect_nodes) on the cache 1495 | pub fn new(src: &'a PathCache) -> Self { 1496 | CacheInspector { 1497 | src, 1498 | inner: src.nodes.iter(), 1499 | } 1500 | } 1501 | 1502 | /// Provides the handle to a specific Node. 1503 | /// 1504 | /// It is recommended to use the `Iterator` implementation instead 1505 | pub fn get_node(&self, id: u32) -> NodeInspector { 1506 | NodeInspector::new(self.src, id as NodeID) 1507 | } 1508 | } 1509 | 1510 | impl<'a, N: Neighborhood> Iterator for CacheInspector<'a, N> { 1511 | type Item = NodeInspector<'a, N>; 1512 | fn next(&mut self) -> Option { 1513 | self.inner 1514 | .next() 1515 | .map(|id| NodeInspector::new(self.src, id.0)) 1516 | } 1517 | } 1518 | 1519 | /// Allows for debugging and visualizing a Node 1520 | /// 1521 | /// See [`inspect_nodes`](PathCache::inspect_nodes) for details and an example. 1522 | /// 1523 | /// Can be obtained by iterating over a [`CacheInspector`] or from [`get_node`](CacheInspector::get_node). 1524 | /// 1525 | /// Gives basic info about the Node and an Iterator over all connected Nodes 1526 | #[derive(Debug, Clone, Copy)] 1527 | pub struct NodeInspector<'a, N: Neighborhood> { 1528 | src: &'a PathCache, 1529 | node: &'a Node, 1530 | id: NodeID, 1531 | } 1532 | 1533 | impl<'a, N: Neighborhood> NodeInspector<'a, N> { 1534 | fn new(src: &'a PathCache, id: NodeID) -> Self { 1535 | NodeInspector { 1536 | src, 1537 | node: &src.nodes[id], 1538 | id, 1539 | } 1540 | } 1541 | 1542 | /// The position of the Node on the Grid 1543 | pub fn pos(&self) -> (usize, usize) { 1544 | self.node.pos 1545 | } 1546 | 1547 | /// The internal unique ID 1548 | /// 1549 | /// IDs are unique at any point in time, but may be reused if Nodes are deleted. 1550 | pub fn id(&self) -> u32 { 1551 | self.id as u32 1552 | } 1553 | 1554 | /// Provides an iterator over all connected Nodes with the Cost of the Path to that Node 1555 | pub fn connected(&'a self) -> impl Iterator, Cost)> + 'a { 1556 | self.node 1557 | .edges 1558 | .iter() 1559 | .map(move |(id, path)| (NodeInspector::new(self.src, *id), path.cost())) 1560 | } 1561 | } 1562 | 1563 | #[cfg(test)] 1564 | mod tests { 1565 | use crate::prelude::*; 1566 | #[test] 1567 | fn get_chunk_index() { 1568 | let grid = [ 1569 | [0, 2, 0, 0, 0], 1570 | [0, 2, 2, 2, 2], 1571 | [0, 1, 0, 0, 0], 1572 | [0, 1, 0, 2, 0], 1573 | [0, 0, 0, 2, 0], 1574 | ]; 1575 | let (width, height) = (grid[0].len(), grid.len()); 1576 | fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Fn((usize, usize)) -> isize { 1577 | move |(x, y)| [1, 10, -1][grid[y][x]] 1578 | } 1579 | let pathfinding = PathCache::new( 1580 | (width, height), 1581 | cost_fn(&grid), 1582 | ManhattanNeighborhood::new(width, height), 1583 | PathCacheConfig::with_chunk_size(3), 1584 | ); 1585 | 1586 | let point = (0, 0); 1587 | assert_eq!(pathfinding.get_chunk_index(point), 0); 1588 | 1589 | let point = (4, 0); 1590 | assert_eq!(pathfinding.get_chunk_index(point), 1); 1591 | 1592 | let point = (3, 2); 1593 | assert_eq!(pathfinding.get_chunk_index(point), 1); 1594 | 1595 | let point = (4, 4); 1596 | assert_eq!(pathfinding.get_chunk_index(point), 3); 1597 | 1598 | let point = (0, 4); 1599 | assert_eq!(pathfinding.get_chunk_index(point), 2); 1600 | } 1601 | } 1602 | -------------------------------------------------------------------------------- /src/path_cache/cache_config.rs: -------------------------------------------------------------------------------- 1 | /// Options for configuring the [`PathCache`](crate::PathCache) 2 | /// 3 | /// Default options: 4 | /// ``` 5 | /// # use hierarchical_pathfinding::PathCacheConfig; 6 | /// assert_eq!( 7 | /// PathCacheConfig { 8 | /// chunk_size: 8, 9 | /// cache_paths: true, 10 | /// a_star_fallback: true, 11 | /// perfect_paths: false, 12 | /// }, 13 | /// Default::default() 14 | /// ); 15 | /// ``` 16 | /// 17 | /// ### Performance 18 | /// For testing, a set of different Grids were used: 19 | /// - "empty" is simply an Grid where all costs are `1`. 20 | /// - "snake" has a single long winding corridor 21 | /// - "random" is completely random costs and walls 22 | /// 23 | /// Each Grid was tested as 16x16, 128x128 and 1024x1024. The 16x16 Grid, however, produced only 24 | /// durations less than 100 μs, which are too inaccurate to be measured. 25 | /// 26 | /// The full List can be seen on [GitHub](https://github.com/mich101mich/hierarchical_pathfinding/blob/benchmark/benchmark/) 27 | /// in the `output_*.txt` files. 28 | /// 29 | /// Grid | 128x128 | 1024x1024 30 | /// -------|:-------:|:---------: 31 | /// empty | ![](https://github.com/mich101mich/hierarchical_pathfinding/blob/benchmark/img/plot/empty128.png?raw=true) | ![](https://github.com/mich101mich/hierarchical_pathfinding/blob/benchmark/img/plot/empty1024.png?raw=true) 32 | /// snake | ![](https://github.com/mich101mich/hierarchical_pathfinding/blob/benchmark/img/plot/snake128.png?raw=true) | ![](https://github.com/mich101mich/hierarchical_pathfinding/blob/benchmark/img/plot/snake1024.png?raw=true) 33 | /// random | ![](https://github.com/mich101mich/hierarchical_pathfinding/blob/benchmark/img/plot/random128.png?raw=true)| ![](https://github.com/mich101mich/hierarchical_pathfinding/blob/benchmark/img/plot/random1024.png?raw=true) 34 | /// 35 | /// Conclusions: 36 | /// - Bigger Chunks are usually better 37 | /// - Chunks that take up the entire Grid are useless (see 128x128) 38 | /// 39 | /// ### Memory 40 | /// for 1024x1024: 100MB - 1000MB, depending on Node density on the Grid. 41 | /// 42 | /// Can be drastically reduced by setting `cache_paths` to `false`, at the expense of repeated 43 | /// calculations when using a Path. 44 | #[derive(Clone, Copy, Debug, PartialEq)] 45 | pub struct PathCacheConfig { 46 | /// The size of the individual Chunks (defaults to `8`) 47 | /// 48 | /// tl;dr: Depends highly on the size of your Grid and Lengths of your Paths; 49 | /// requires Experimentation if you care. 50 | /// 51 | /// Rough guide: Scale with a bigger Grid and longer desired Paths. 52 | /// _(don't quote me on this)_ 53 | /// 54 | /// This has many different effects on the Performance and Memory: 55 | /// 56 | /// smaller chunks make calculations within a Chunk faster 57 | /// - => decreased update time in tiles_changed 58 | /// - => decreased time to find start and end Nodes 59 | /// 60 | /// bigger chunks lead to fewer Chunks and Nodes 61 | /// - => decreased time of actual search (especially for unreachable goals) 62 | /// - => longer Paths can be found a lot faster 63 | /// 64 | /// |Use Case|Recommendation| 65 | /// |--------|--------------| 66 | /// |Frequent Updates|Smaller Chunks| 67 | /// |Longer Paths required|Larger Chunks| 68 | /// |Larger Grid|Larger Chunks| 69 | /// |"No Path found" is common|Larger Chunks| 70 | /// |Grid consists of small, windy corridors|Smaller Chunks| 71 | pub chunk_size: usize, 72 | /// `true` (default): store the Paths inside each Chunk. 73 | /// 74 | /// `false`: only store the Cost of the Path. 75 | /// 76 | /// This will not affect the calculations or the time it takes to calculate the initial Path. 77 | /// It only makes a difference when iterating over the returned Path, because that requires 78 | /// re-calculating the next segment (see [`safe_next`](crate::internals::AbstractPath::safe_next) on [`AbstractPath`](crate::internals::AbstractPath)). 79 | /// 80 | /// Drastically reduces Memory usage. 81 | pub cache_paths: bool, 82 | /// `true` (default): When a Path is short (roughly `Length < 2 * chunk_size`), a regular 83 | /// A* search is performed on the Grid **after** HPA* calculated a Path to confirm the 84 | /// existence and length. 85 | /// 86 | /// `false`: The Paths are left as they are. 87 | /// 88 | /// Setting this option to false will improve performance a bit, but the Paths will take 89 | /// seemingly random detours that makes them longer than they should be. 90 | pub a_star_fallback: bool, 91 | /// `true`: Nodes are placed on every open entrance of a Chunk. This means that the resulting 92 | /// Paths are always the most optimal ones, but both Memory Usage and Performance are worse. 93 | /// 94 | /// `false` (default): Nodes are placed on only some chunk entrances. 95 | /// 96 | /// The exact effect depends greatly on the Grid and how the Chunks and their entrances align. 97 | /// It is questionable weather or not you should use Hierarchical Pathfinding if you enable 98 | /// this... 99 | pub perfect_paths: bool, 100 | } 101 | 102 | impl PathCacheConfig { 103 | /// Creates a new `PathCacheConfig` with the given `chunk_size`. 104 | /// ``` 105 | /// # use hierarchical_pathfinding::PathCacheConfig; 106 | /// let chunk_size = 123; 107 | /// assert_eq!( 108 | /// PathCacheConfig::with_chunk_size(chunk_size), 109 | /// PathCacheConfig { 110 | /// chunk_size, 111 | /// ..Default::default() 112 | /// } 113 | /// ); 114 | /// ``` 115 | pub fn with_chunk_size(chunk_size: usize) -> Self { 116 | Self { 117 | chunk_size, 118 | ..Self::default() 119 | } 120 | } 121 | 122 | /// an example `PathCacheConfig` with options set to reduce Memory Usage 123 | /// 124 | /// Values: 125 | /// ``` 126 | /// # use hierarchical_pathfinding::PathCacheConfig; 127 | /// assert_eq!( 128 | /// PathCacheConfig { 129 | /// chunk_size: 64, 130 | /// cache_paths: false, 131 | /// a_star_fallback: true, 132 | /// perfect_paths: false, 133 | /// }, 134 | /// PathCacheConfig::LOW_MEM 135 | /// ); 136 | /// ``` 137 | pub const LOW_MEM: PathCacheConfig = PathCacheConfig { 138 | chunk_size: 64, 139 | cache_paths: false, 140 | a_star_fallback: true, 141 | perfect_paths: false, 142 | }; 143 | /// an example `PathCacheConfig` with options set to improve Performance 144 | /// 145 | /// Values: 146 | /// ``` 147 | /// # use hierarchical_pathfinding::PathCacheConfig; 148 | /// assert_eq!( 149 | /// PathCacheConfig { 150 | /// chunk_size: 16, 151 | /// cache_paths: true, 152 | /// a_star_fallback: false, 153 | /// perfect_paths: false, 154 | /// }, 155 | /// PathCacheConfig::HIGH_PERFORMANCE 156 | /// ); 157 | /// ``` 158 | pub const HIGH_PERFORMANCE: PathCacheConfig = PathCacheConfig { 159 | chunk_size: 16, 160 | cache_paths: true, 161 | a_star_fallback: false, 162 | perfect_paths: false, 163 | }; 164 | } 165 | 166 | impl Default for PathCacheConfig { 167 | fn default() -> PathCacheConfig { 168 | PathCacheConfig { 169 | chunk_size: 8, 170 | cache_paths: true, 171 | a_star_fallback: true, 172 | perfect_paths: false, 173 | } 174 | } 175 | } 176 | -------------------------------------------------------------------------------- /src/path_cache/chunk.rs: -------------------------------------------------------------------------------- 1 | use crate::{ 2 | graph::NodeList, 3 | neighbors::Neighborhood, 4 | path::{Path, PathSegment}, 5 | *, 6 | }; 7 | 8 | #[derive(Clone, Debug)] 9 | pub(crate) struct Chunk { 10 | pub pos: Point, 11 | pub size: Point, 12 | pub nodes: NodeIDSet, 13 | pub sides: [bool; 4], 14 | } 15 | 16 | impl Chunk { 17 | pub fn new( 18 | pos: Point, 19 | size: (usize, usize), 20 | total_size: (usize, usize), 21 | mut get_cost: impl FnMut(Point) -> isize, 22 | neighborhood: &N, 23 | all_nodes: &mut NodeList, 24 | config: PathCacheConfig, 25 | ) -> Chunk { 26 | let mut chunk = Chunk { 27 | pos, 28 | size, 29 | nodes: NodeIDSet::default(), 30 | sides: [false; 4], 31 | }; 32 | 33 | let mut candidates = PointSet::default(); 34 | 35 | for dir in Dir::all() { 36 | if dir == UP && chunk.top() == 0 37 | || dir == RIGHT && chunk.right() == total_size.0 38 | || dir == DOWN && chunk.bottom() == total_size.1 39 | || dir == LEFT && chunk.left() == 0 40 | { 41 | continue; 42 | } 43 | chunk.sides[dir.num()] = true; 44 | 45 | chunk.calculate_side_nodes(dir, total_size, &mut get_cost, config, &mut candidates); 46 | } 47 | 48 | let nodes = candidates 49 | .into_iter() 50 | .map(|p| all_nodes.add_node(p, get_cost(p) as usize)) 51 | .to_vec(); 52 | 53 | chunk.add_nodes(&nodes, &mut get_cost, neighborhood, all_nodes, &config); 54 | 55 | chunk 56 | } 57 | 58 | pub fn calculate_side_nodes( 59 | &self, 60 | dir: Dir, 61 | total_size: (usize, usize), 62 | mut get_cost: impl FnMut(Point) -> isize, 63 | config: PathCacheConfig, 64 | candidates: &mut PointSet, 65 | ) { 66 | let mut current = [ 67 | (self.pos.0, self.pos.1), 68 | (self.pos.0 + self.size.0 - 1, self.pos.1), 69 | (self.pos.0, self.pos.1 + self.size.1 - 1), 70 | (self.pos.0, self.pos.1), 71 | ][dir.num()]; 72 | let (next_dir, length) = if dir.is_vertical() { 73 | (RIGHT, self.size.0) 74 | } else { 75 | (DOWN, self.size.1) 76 | }; 77 | // 0 == up: start at top-left, go right 78 | // 1 == right: start at top-right, go down 79 | // 2 == down: start at bottom-left, go right 80 | // 3 == left: start at top-left, go down 81 | if get_in_dir(current, dir, (0, 0), total_size).is_none() { 82 | return; 83 | } 84 | 85 | let opposite = |p: Point| { 86 | get_in_dir(p, dir, (0, 0), total_size) 87 | .expect("Internal Error #1 in Chunk. Please report this") 88 | }; 89 | 90 | let costs = (0..length) 91 | .map(|i| { 92 | jump_in_dir(current, next_dir, i, self.pos, self.size) 93 | .expect("Internal Error #3 in Chunk. Please report this") 94 | }) 95 | .map(|p| (get_cost(p), get_cost(opposite(p)))) 96 | .to_vec(); 97 | 98 | let solid = |i: usize| { 99 | let (c1, c2) = &costs[i]; 100 | *c1 < 0 || *c2 < 0 101 | }; 102 | let total_cost = |i: usize| { 103 | let (c1, c2) = &costs[i]; 104 | *c1 + *c2 105 | }; 106 | 107 | let mut has_gap = false; 108 | let mut gap_start = 0; 109 | let mut gap_start_pos = current; 110 | let mut previous = current; 111 | 112 | for i in 0..length { 113 | let is_last = i == length - 1; 114 | let solid = solid(i); 115 | 116 | if !solid && !has_gap { 117 | has_gap = true; 118 | gap_start = i; 119 | gap_start_pos = current; 120 | } 121 | if (solid || is_last) && has_gap { 122 | has_gap = false; 123 | let (gap_end, gap_end_pos) = if solid { 124 | (i - 1, previous) 125 | } else { 126 | (i, current) 127 | }; 128 | 129 | let gap_len = gap_end - gap_start + 1; 130 | 131 | candidates.insert(gap_start_pos); 132 | candidates.insert(gap_end_pos); 133 | 134 | if config.perfect_paths { 135 | let mut p = gap_start_pos; 136 | for _ in (gap_start + 1)..gap_end { 137 | p = get_in_dir(p, next_dir, (0, 0), total_size) 138 | .expect("Internal Error #6 in Chunk. Please report this"); 139 | candidates.insert(p); 140 | } 141 | } else { 142 | if gap_len > 2 { 143 | let mut min = total_cost(gap_start).min(total_cost(gap_end)); 144 | let mut p = gap_start_pos; 145 | for gi in (gap_start + 1)..gap_end { 146 | p = get_in_dir(p, next_dir, (0, 0), total_size) 147 | .expect("Internal Error #2 in Chunk. Please report this"); 148 | let cost = total_cost(gi); 149 | if cost < min { 150 | candidates.insert(p); 151 | min = cost; 152 | } 153 | } 154 | } 155 | 156 | if gap_len > 6 { 157 | let mid = ( 158 | (gap_start_pos.0 + gap_end_pos.0) / 2, 159 | (gap_start_pos.1 + gap_end_pos.1) / 2, 160 | ); 161 | candidates.insert(mid); 162 | } 163 | } 164 | } 165 | 166 | if !is_last { 167 | previous = current; 168 | current = get_in_dir(current, next_dir, self.pos, self.size) 169 | .expect("Internal Error #3 in Chunk. Please report this"); 170 | } 171 | } 172 | } 173 | 174 | pub fn add_nodes( 175 | &mut self, 176 | to_visit: &[NodeID], 177 | mut get_cost: impl FnMut(Point) -> isize, 178 | neighborhood: &N, 179 | all_nodes: &mut NodeList, 180 | config: &PathCacheConfig, 181 | ) { 182 | // first to_visit, then the rest => slicing works the same on both lists 183 | let points = to_visit 184 | .iter() 185 | .chain(self.nodes.iter()) 186 | .map(|id| all_nodes[*id].pos) 187 | .to_vec(); 188 | 189 | for &id in to_visit.iter() { 190 | self.nodes.insert(id); 191 | } 192 | 193 | for (i, &id) in to_visit.iter().enumerate() { 194 | let point = points[i]; 195 | let remaining = &points[(i + 1)..]; 196 | let paths = self.find_paths(point, remaining, &mut get_cost, neighborhood); 197 | for (other_pos, path) in paths { 198 | let other_id = all_nodes 199 | .id_at(other_pos) 200 | .expect("Internal Error #5 in Chunk. Please report this"); 201 | 202 | all_nodes.add_edge(id, other_id, PathSegment::new(path, config.cache_paths)); 203 | } 204 | } 205 | } 206 | 207 | #[cfg(feature = "parallel")] 208 | pub fn connect_nodes_parallel isize + Sync>( 209 | &self, 210 | get_cost: F1, 211 | neighborhood: &N, 212 | all_nodes: &NodeList, 213 | cache_paths: bool, 214 | ) -> Vec<(NodeID, NodeID, PathSegment)> { 215 | use rayon::prelude::*; 216 | 217 | let mut ids = Vec::with_capacity(self.nodes.len()); 218 | let mut points = Vec::with_capacity(self.nodes.len()); 219 | for (i, &id) in self.nodes.iter().enumerate() { 220 | ids.push((i, id)); 221 | points.push(all_nodes[id].pos); 222 | } 223 | 224 | // connect every Node to every other Node 225 | ids.par_iter() 226 | .flat_map(|&(i, id)| { 227 | let point = points[i]; 228 | let remaining = &points[(i + 1)..]; 229 | self.find_paths(point, remaining, &get_cost, neighborhood) 230 | .into_par_iter() 231 | .map(move |(other_pos, path)| { 232 | let other_id = all_nodes 233 | .id_at(other_pos) 234 | .expect("Internal Error #5 in Chunk. Please report this"); 235 | 236 | (id, other_id, PathSegment::new(path, cache_paths)) 237 | }) 238 | }) 239 | .collect() 240 | } 241 | 242 | pub fn find_paths( 243 | &self, 244 | start: Point, 245 | goals: &[Point], 246 | get_cost: impl FnMut(Point) -> isize, 247 | neighborhood: &N, 248 | ) -> PointMap> { 249 | if !self.in_chunk(start) { 250 | return PointMap::default(); 251 | } 252 | let heuristic = goals 253 | .iter() 254 | .map(|goal| neighborhood.heuristic(start, *goal)) 255 | .max() 256 | .unwrap_or(0); 257 | let max_heuristic = neighborhood.heuristic((0, 0), (self.size.0 - 1, self.size.1 - 1)); 258 | let max_size = self.size.0 * self.size.1; 259 | let size_hint = heuristic as f32 / max_heuristic as f32 * max_size as f32; 260 | grid::dijkstra_search( 261 | neighborhood, 262 | |p| self.in_chunk(p), 263 | get_cost, 264 | start, 265 | goals, 266 | false, 267 | size_hint as usize, 268 | ) 269 | } 270 | 271 | pub fn nearest_node( 272 | &self, 273 | all_nodes: &NodeList, 274 | start: Point, 275 | mut get_cost: impl FnMut(Point) -> isize, 276 | neighborhood: &N, 277 | reverse: bool, 278 | ) -> Option<(NodeID, Path)> { 279 | let start_cost = get_cost(start); 280 | if start_cost < 0 { 281 | if !reverse { 282 | return None; 283 | } 284 | self.nodes.iter().copied().find_map(|id| { 285 | self.find_path(all_nodes[id].pos, start, &mut get_cost, neighborhood) 286 | .map(|path| (id, path)) 287 | }) 288 | } else { 289 | let mut points = Vec::with_capacity(self.nodes.len()); 290 | let mut map = PointMap::default(); 291 | let max_heuristic = neighborhood.heuristic((0, 0), (self.size.0 - 1, self.size.1 - 1)); 292 | let mut min_heuristic = max_heuristic; 293 | for id in self.nodes.iter() { 294 | let node = &all_nodes[*id]; 295 | let point = node.pos; 296 | points.push(point); 297 | map.insert(point, (*id, node.walk_cost)); 298 | min_heuristic = min_heuristic.min(neighborhood.heuristic(start, point)); 299 | } 300 | 301 | let max_size = self.size.0 * self.size.1; 302 | let size_hint = min_heuristic as f32 / max_heuristic as f32 * max_size as f32; 303 | grid::dijkstra_search( 304 | neighborhood, 305 | |p| self.in_chunk(p), 306 | get_cost, 307 | start, 308 | &points, 309 | true, 310 | size_hint as usize, 311 | ) 312 | .into_iter() 313 | .next() 314 | .map(|(point, path)| { 315 | let (id, node_cost) = map[&point]; 316 | ( 317 | id, 318 | if reverse { 319 | path.reversed(start_cost as usize, node_cost) 320 | } else { 321 | path 322 | }, 323 | ) 324 | }) 325 | } 326 | } 327 | pub fn find_path( 328 | &self, 329 | start: Point, 330 | goal: Point, 331 | get_cost: impl FnMut(Point) -> isize, 332 | neighborhood: &N, 333 | ) -> Option> { 334 | if !self.in_chunk(start) || !self.in_chunk(goal) { 335 | return None; 336 | } 337 | let heuristic = neighborhood.heuristic(start, goal); 338 | let max_heuristic = neighborhood.heuristic((0, 0), (self.size.0 - 1, self.size.1 - 1)); 339 | let max_size = self.size.0 * self.size.1; 340 | let size_hint = heuristic as f32 / max_heuristic as f32 * max_size as f32; 341 | 342 | grid::a_star_search( 343 | neighborhood, 344 | |p| self.in_chunk(p), 345 | get_cost, 346 | start, 347 | goal, 348 | size_hint as usize, 349 | ) 350 | } 351 | 352 | pub fn in_chunk(&self, point: Point) -> bool { 353 | point.0 >= self.left() 354 | && point.0 < self.right() 355 | && point.1 >= self.top() 356 | && point.1 < self.bottom() 357 | } 358 | 359 | pub fn at_side(&self, point: Point, side: Dir) -> bool { 360 | match side { 361 | UP => point.1 == self.top(), 362 | RIGHT => point.0 == self.right() - 1, 363 | DOWN => point.1 == self.bottom() - 1, 364 | LEFT => point.0 == self.left(), 365 | } 366 | } 367 | 368 | pub fn is_corner(&self, point: Point) -> bool { 369 | Dir::all() 370 | .filter(|&dir| self.sides[dir.num()] && self.at_side(point, dir)) 371 | .count() 372 | == 2 373 | } 374 | 375 | pub fn top(&self) -> usize { 376 | self.pos.1 377 | } 378 | pub fn right(&self) -> usize { 379 | self.pos.0 + self.size.0 380 | } 381 | pub fn bottom(&self) -> usize { 382 | self.pos.1 + self.size.1 383 | } 384 | pub fn left(&self) -> usize { 385 | self.pos.0 386 | } 387 | } 388 | -------------------------------------------------------------------------------- /src/utils.rs: -------------------------------------------------------------------------------- 1 | #![allow(unused)] 2 | 3 | #[allow(clippy::wrong_self_convention)] 4 | pub trait IterExt: Iterator { 5 | fn to_vec(self) -> Vec; 6 | } 7 | impl> IterExt for I { 8 | #[allow(clippy::wrong_self_convention)] 9 | fn to_vec(self) -> Vec { 10 | self.collect() 11 | } 12 | } 13 | 14 | use crate::Point; 15 | 16 | #[derive(Clone, Copy, Debug, PartialEq, Eq)] 17 | pub enum Dir { 18 | UP = 0, 19 | RIGHT = 1, 20 | DOWN = 2, 21 | LEFT = 3, 22 | } 23 | pub use self::Dir::*; 24 | 25 | impl Dir { 26 | pub fn all() -> std::iter::Copied> { 27 | [UP, RIGHT, DOWN, LEFT].iter().copied() 28 | } 29 | pub fn opposite(self) -> Dir { 30 | ((self.num() + 2) % 4).into() 31 | } 32 | pub fn num(self) -> usize { 33 | self as usize 34 | } 35 | pub fn is_vertical(self) -> bool { 36 | self == UP || self == DOWN 37 | } 38 | } 39 | 40 | macro_rules! impl_from_into { 41 | ($($type:ty),+) => {$( 42 | impl From<$type> for Dir { 43 | fn from(val: $type) -> Dir { 44 | match val { 45 | 0 => UP, 46 | 1 => RIGHT, 47 | 2 => DOWN, 48 | 3 => LEFT, 49 | _ => panic!("invalid Dir: {}", val), 50 | } 51 | } 52 | } 53 | impl From

for $type { 54 | fn from(dir: Dir) -> $type { 55 | dir as $type 56 | } 57 | } 58 | )+} 59 | } 60 | 61 | impl_from_into!(u8, u16, u32, u64, usize, i8, i16, i32, i64, isize); 62 | 63 | const UNIT_CIRCLE: [(isize, isize); 4] = [(0, -1), (1, 0), (0, 1), (-1, 0)]; 64 | 65 | pub fn get_in_dir(pos: Point, dir: Dir, base: Point, (w, h): (usize, usize)) -> Option { 66 | let diff = UNIT_CIRCLE[dir.num()]; 67 | if (pos.0 == base.0 && diff.0 < 0) 68 | || (pos.1 == base.1 && diff.1 < 0) 69 | || (pos.0 == base.0 + w - 1 && diff.0 > 0) 70 | || (pos.1 == base.1 + h - 1 && diff.1 > 0) 71 | { 72 | None 73 | } else { 74 | Some(( 75 | (pos.0 as isize + diff.0) as usize, 76 | (pos.1 as isize + diff.1) as usize, 77 | )) 78 | } 79 | } 80 | 81 | pub fn jump_in_dir( 82 | pos: Point, 83 | dir: Dir, 84 | dist: usize, 85 | base: Point, 86 | (w, h): (usize, usize), 87 | ) -> Option { 88 | let dist = dist as isize; 89 | let left = base.0 as isize; 90 | let right = (base.0 + w) as isize; 91 | let top = base.1 as isize; 92 | let bottom = (base.1 + h) as isize; 93 | 94 | let diff = UNIT_CIRCLE[dir.num()]; 95 | let res = ( 96 | pos.0 as isize + diff.0 * dist, 97 | pos.1 as isize + diff.1 * dist, 98 | ); 99 | if res.0 >= left && res.0 < right && res.1 >= top && res.1 < bottom { 100 | Some((res.0 as usize, res.1 as usize)) 101 | } else { 102 | None 103 | } 104 | } 105 | 106 | #[cfg(test)] 107 | mod tests { 108 | use super::*; 109 | 110 | #[test] 111 | fn jump_test() { 112 | let pos = (1, 3); 113 | assert_eq!(jump_in_dir(pos, UP, 2, (0, 0), (5, 5)), Some((1, 1))); 114 | assert_eq!(jump_in_dir(pos, RIGHT, 2, (0, 0), (5, 5)), Some((3, 3))); 115 | assert_eq!(jump_in_dir(pos, DOWN, 2, (0, 0), (5, 5)), None); 116 | assert_eq!(jump_in_dir(pos, LEFT, 2, (0, 0), (5, 5)), None); 117 | } 118 | } 119 | -------------------------------------------------------------------------------- /test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | TMP_FILE="/tmp/hpa_test_out.txt" 4 | 5 | function handle_output { 6 | rm -f "${TMP_FILE}" 7 | while read -r line 8 | do 9 | echo "${line}" >> "${TMP_FILE}" 10 | 11 | # make sure line is not longer than the terminal width 12 | width=$(tput cols) # read this again in case the terminal was resized 13 | width=$((width - 3)) # leave space for the "..." 14 | TRIMMED_LINE=$(echo "> ${line}" | sed "s/\(.\{${width}\}\).*/\1.../") 15 | echo -en "\033[2K\r${TRIMMED_LINE}" 16 | tput init # trimmed line may have messed up coloring 17 | done 18 | echo -ne "\033[2K\r"; 19 | } 20 | 21 | function try_silent { 22 | echo "Running $*" 23 | unbuffer "$@" | handle_output 24 | if [[ ${PIPESTATUS[0]} -ne 0 ]]; then 25 | cat "${TMP_FILE}" 26 | return 1 27 | fi 28 | } 29 | 30 | BASE_DIR="$(realpath "$(dirname "$0")")" 31 | OUT_DIRS="${BASE_DIR}/test_dirs" 32 | MSRV_DIR="${OUT_DIRS}/msrv" 33 | MIN_VERSIONS_DIR="${OUT_DIRS}/min_versions" 34 | 35 | for dir in "${MSRV_DIR}" "${MIN_VERSIONS_DIR}"; do 36 | [[ -d "${dir}" ]] && continue 37 | mkdir -p "${dir}" 38 | ln -s "${BASE_DIR}/Cargo.toml" "${dir}/Cargo.toml" 39 | ln -s "${BASE_DIR}/src" "${dir}/src" 40 | ln -s "${BASE_DIR}/tests" "${dir}/tests" 41 | ln -s "${BASE_DIR}/benches" "${dir}/benches" 42 | done 43 | 44 | # main tests 45 | ( 46 | cd "${BASE_DIR}" || (echo "Failed to cd to ${BASE_DIR}"; exit 1) 47 | try_silent cargo update || exit 1 48 | try_silent cargo +stable test || exit 1 49 | try_silent cargo +stable test --no-default-features || exit 1 50 | try_silent cargo +stable test --no-default-features --features parallel || exit 1 51 | try_silent cargo +stable test --no-default-features --features log || exit 1 52 | try_silent cargo +stable test --all-features || exit 1 53 | try_silent cargo +nightly test || exit 1 54 | try_silent cargo +nightly test --no-default-features || exit 1 55 | try_silent cargo +nightly test --no-default-features --features parallel || exit 1 56 | try_silent cargo +nightly test --no-default-features --features log || exit 1 57 | try_silent cargo +nightly test --all-features || exit 1 58 | export RUSTDOCFLAGS="-D warnings" 59 | try_silent cargo +nightly doc --all-features --no-deps || exit 1 60 | try_silent cargo +nightly clippy -- -D warnings || exit 1 61 | try_silent cargo +stable fmt --check || exit 1 62 | ) || exit 1 63 | 64 | # # minimum supported rust version 65 | # ( 66 | # cd "${MSRV_DIR}" || (echo "Failed to cd to ${MSRV_DIR}"; exit 1) 67 | # try_silent cargo +1.56.0 test --no-default-features || exit 1 68 | # try_silent cargo +1.56.0 test --no-default-features --features parallel || exit 1 69 | # ) || exit 1 70 | 71 | # minimal versions 72 | ( 73 | cd "${MIN_VERSIONS_DIR}" || (echo "Failed to cd to ${MIN_VERSIONS_DIR}"; exit 1) 74 | try_silent cargo +nightly -Z minimal-versions update || exit 1 75 | 76 | try_silent cargo +stable test --no-default-features || exit 1 77 | try_silent cargo +stable test --no-default-features --features parallel || exit 1 78 | try_silent cargo +nightly test --no-default-features || exit 1 79 | try_silent cargo +nightly test --no-default-features --features parallel || exit 1 80 | ) || exit 1 81 | -------------------------------------------------------------------------------- /tests/path_cache.rs: -------------------------------------------------------------------------------- 1 | use hierarchical_pathfinding::prelude::*; 2 | 3 | #[test] 4 | fn new() { 5 | let grid = [ 6 | [0, 2, 0, 0, 0], 7 | [0, 2, 2, 2, 2], 8 | [0, 1, 0, 0, 0], 9 | [0, 1, 0, 2, 0], 10 | [0, 0, 0, 2, 0], 11 | ]; 12 | let (width, height) = (grid[0].len(), grid.len()); 13 | fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Fn((usize, usize)) -> isize { 14 | move |(x, y)| [1, 10, -1][grid[y][x]] 15 | } 16 | let pathfinding = PathCache::new( 17 | (width, height), 18 | cost_fn(&grid), 19 | ManhattanNeighborhood::new(width, height), 20 | PathCacheConfig::with_chunk_size(3), 21 | ); 22 | let start = (0, 0); 23 | let goal = (4, 4); 24 | let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 25 | let path = path.unwrap(); 26 | let points: Vec<(usize, usize)> = path.collect(); 27 | #[rustfmt::skip] 28 | assert_eq!( 29 | points, 30 | vec![(0, 1), (0, 2), (0, 3), (0, 4), (1, 4), (2, 4), (2, 3), (2, 2), (3, 2), (4, 2), (4, 3), (4, 4)], 31 | ); 32 | } 33 | 34 | #[test] 35 | fn update_path() { 36 | let mut grid = [ 37 | [0, 2, 0, 0, 0], 38 | [0, 2, 2, 2, 2], 39 | [0, 1, 0, 0, 0], 40 | [0, 1, 0, 2, 0], 41 | [0, 0, 0, 2, 0], 42 | ]; 43 | let (width, height) = (grid[0].len(), grid.len()); 44 | fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Fn((usize, usize)) -> isize { 45 | move |(x, y)| [1, 10, -1][grid[y][x]] 46 | } 47 | 48 | let mut pathfinding = PathCache::new( 49 | (width, height), 50 | cost_fn(&grid), 51 | MooreNeighborhood::new(width, height), 52 | PathCacheConfig::with_chunk_size(3), 53 | ); 54 | 55 | let start = (0, 0); 56 | let goal = (4, 4); 57 | let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 58 | let path = path.unwrap(); 59 | let points: Vec<(usize, usize)> = path.collect(); 60 | #[rustfmt::skip] 61 | assert_eq!( 62 | points, 63 | vec![(0, 1), (0, 2), (0, 3), (1, 4), (2, 3), (3, 2), (4, 3), (4, 4)], 64 | ); 65 | 66 | grid[2][1] = 0; 67 | let changed_tiles = [(1, 2)]; 68 | 69 | pathfinding.tiles_changed(&changed_tiles, cost_fn(&grid)); 70 | 71 | let start = (0, 0); 72 | let goal = (4, 4); 73 | let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 74 | let path = path.unwrap(); 75 | let points: Vec<(usize, usize)> = path.collect(); 76 | #[rustfmt::skip] 77 | assert_eq!( 78 | points, 79 | vec![(0, 1), (1, 2), (2, 2), (3, 2), (4, 3), (4, 4)], 80 | ); 81 | 82 | // Add walls along chunk borders 83 | let start = (0, 0); 84 | let goal = (3, 4); 85 | let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 86 | assert!(path.is_some()); 87 | 88 | // Vertical wall 89 | let changed_tiles: Vec<_> = (0..grid.len()).map(|y| (2, y)).collect(); 90 | grid.iter_mut().for_each(|row| row[2] = 2); 91 | pathfinding.tiles_changed(&changed_tiles, cost_fn(&grid)); 92 | 93 | let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 94 | assert!(path.is_none()); 95 | 96 | let goal = (2, 4); 97 | let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 98 | assert!(path.is_some()); 99 | 100 | // Horizontal wall 101 | let mut changed_tiles = Vec::new(); 102 | { 103 | let y = 2; 104 | for x in 0..grid[y].len() { 105 | grid[y][x] = 2; 106 | changed_tiles.push((x, y)); 107 | } 108 | } 109 | pathfinding.tiles_changed(&changed_tiles, cost_fn(&grid)); 110 | 111 | let path = pathfinding.find_path(start, goal, cost_fn(&grid)); 112 | assert!(path.is_none()); 113 | } 114 | 115 | #[test] 116 | fn skip_first_and_last() { 117 | let grid = [[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1], [1, 1, 0, 0]]; 118 | // explanation: 119 | // grid: 120 | // . . .|# // . = empty, # = wall 121 | // . S 0|1 // S = start 122 | // . G 2|# // G = goal 123 | // -----+- // chunk border 124 | // # # 4|5 125 | // 126 | // ^ chunk border 127 | // numbers 0-3: nodes 128 | // 129 | // calculated path will be S -> 0 -> 2 -> G 130 | // but: old path optimizations would add the shortcut S->2 and 0->G, which would create 131 | // an invalid path 132 | let (width, height) = (grid[0].len(), grid.len()); 133 | fn cost_fn(grid: &[[usize; 4]]) -> impl '_ + Fn((usize, usize)) -> isize { 134 | move |(x, y)| [1, -1][grid[y][x]] 135 | } 136 | let pathfinding = PathCache::new( 137 | (width, height), 138 | cost_fn(&grid), 139 | ManhattanNeighborhood::new(width, height), 140 | PathCacheConfig { 141 | chunk_size: 3, 142 | a_star_fallback: false, 143 | ..Default::default() 144 | }, 145 | ); 146 | let start = (1, 1); 147 | let goal = (1, 2); 148 | let path = pathfinding.find_closest_goal(start, &[goal, (2, 3)], cost_fn(&grid)); 149 | assert!(path.is_some()); 150 | assert_eq!(path.unwrap().1.resolve(cost_fn(&grid)), vec![goal]); 151 | } 152 | 153 | #[test] 154 | fn from_github_issue_7_example_1() { 155 | use hierarchical_pathfinding::prelude::*; 156 | 157 | type Grid = [[usize; 7]; 7]; 158 | 159 | const COST_MAP: [isize; 3] = [1, 10, -1]; // now const for ownership reasons 160 | 161 | // only borrows the Grid when called 162 | fn cost_fn(grid: &Grid) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 163 | move |(x, y)| COST_MAP[grid[y][x]] 164 | } 165 | 166 | fn neighbors(x: usize, y: usize) -> [(usize, usize); 4] { 167 | [(x + 1, y), (x - 1, y), (x, y + 1), (x, y - 1)] 168 | } 169 | 170 | fn main() { 171 | // 0 = empty, 1 = swamp, 2 = wall 172 | let grid: Grid = [ 173 | [2, 2, 0, 0, 0, 0, 0], 174 | [2, 0, 0, 0, 0, 0, 0], 175 | [1, 0, 0, 2, 2, 0, 0], 176 | [0, 1, 0, 1, 2, 0, 0], 177 | [0, 0, 0, 2, 2, 0, 0], 178 | [0, 0, 0, 0, 0, 0, 2], 179 | [0, 0, 0, 0, 0, 2, 2], 180 | ]; 181 | let (width, height) = (grid[0].len(), grid.len()); 182 | 183 | let pathfinding = PathCache::new( 184 | (width, height), 185 | cost_fn(&grid), 186 | ManhattanNeighborhood::new(width, height), 187 | PathCacheConfig::with_chunk_size(3), 188 | ); 189 | 190 | let path = pathfinding.find_closest_goal((1, 1), &neighbors(1, 3), cost_fn(&grid)); 191 | assert!(path.is_some()); 192 | let path = pathfinding.find_closest_goal((1, 3), &neighbors(1, 1), cost_fn(&grid)); 193 | assert!(path.is_some()); 194 | } 195 | 196 | main(); 197 | } 198 | 199 | #[test] 200 | fn from_github_issue_7_example_2() { 201 | use hierarchical_pathfinding::prelude::*; 202 | 203 | type Grid = [[usize; 7]; 7]; 204 | 205 | const COST_MAP: [isize; 3] = [1, 10, -1]; // now const for ownership reasons 206 | 207 | // only borrows the Grid when called 208 | fn cost_fn(grid: &Grid) -> impl '_ + Sync + Fn((usize, usize)) -> isize { 209 | move |(x, y)| COST_MAP[grid[y][x]] 210 | } 211 | 212 | fn neighbors(pos: (usize, usize)) -> [(isize, isize); 4] { 213 | [ 214 | (pos.0 as isize + 1, pos.1 as isize), 215 | (pos.0 as isize - 1, pos.1 as isize), 216 | (pos.0 as isize, pos.1 as isize + 1), 217 | (pos.0 as isize, pos.1 as isize - 1), 218 | ] 219 | } 220 | 221 | fn main() { 222 | // 0 = empty, 1 = swamp, 2 = wall 223 | let grid: Grid = [ 224 | [2, 2, 0, 0, 0, 0, 0], 225 | [2, 0, 0, 0, 0, 0, 0], 226 | [1, 0, 0, 2, 2, 0, 0], 227 | [0, 1, 0, 1, 2, 0, 0], 228 | [0, 0, 0, 2, 2, 0, 0], 229 | [0, 0, 0, 0, 0, 0, 2], 230 | [0, 0, 0, 0, 0, 2, 2], 231 | ]; 232 | let (width, height) = (grid.len(), grid[0].len()); 233 | 234 | let pathfinding = PathCache::new( 235 | (width, height), 236 | cost_fn(&grid), 237 | ManhattanNeighborhood::new(width, height), 238 | PathCacheConfig::with_chunk_size(3), 239 | ); 240 | 241 | let path = pathfind((1, 1), (3, 1), &grid, &pathfinding); 242 | assert!(path.is_some()); 243 | let len = path.unwrap().len(); 244 | assert!(len <= 3, "len={}", len); 245 | let path = pathfind((3, 1), (5, 1), &grid, &pathfinding); 246 | assert!(path.is_some()); 247 | let len = path.unwrap().len(); 248 | assert!(len <= 3, "len={}", len); 249 | let path = pathfind((5, 1), (3, 1), &grid, &pathfinding); 250 | assert!(path.is_some()); 251 | let len = path.unwrap().len(); 252 | assert!(len <= 3, "len={}", len); 253 | let path = pathfind((3, 1), (1, 1), &grid, &pathfinding); 254 | assert!(path.is_some()); 255 | let len = path.unwrap().len(); 256 | assert!(len <= 3, "len={}", len); 257 | } 258 | 259 | fn pathfind( 260 | pos: (usize, usize), 261 | goal: (usize, usize), 262 | grid: &Grid, 263 | pathfinding: &PathCache, 264 | ) -> Option> { 265 | let valid_neighbors = neighbors(goal) 266 | .iter() 267 | .cloned() 268 | .filter(|n| n.0 >= 0 && n.1 >= 0 && cost_fn(&grid)((n.0 as usize, n.1 as usize)) != -1) 269 | .map(|n| (n.0 as usize, n.1 as usize)) 270 | .collect::>(); 271 | println!("valid_neighbors: {:?}", valid_neighbors); 272 | let (_goal, path) = 273 | pathfinding.find_closest_goal(pos, &valid_neighbors.as_slice(), cost_fn(&grid))?; 274 | Some(path.resolve(cost_fn(&grid))) 275 | } 276 | 277 | main(); 278 | } 279 | --------------------------------------------------------------------------------