diff --git a/benches/comparison_bench.rs b/benches/comparison_bench.rs index df9dd35..3a2cff3 100644 --- a/benches/comparison_bench.rs +++ b/benches/comparison_bench.rs @@ -1,32 +1,41 @@ use criterion::{criterion_group, criterion_main, Criterion}; -use grid_pathfinding::PathingGrid; +use grid_pathfinding::{ + pathing_grid::PathingGrid, + solver::{astar::AstarSolver, dijkstra::DijkstraSolver, jps::JPSSolver, GridSolver}, + Pathfinder, +}; use grid_pathfinding_benchmark::*; use grid_util::grid::ValueGrid; +use smallvec::{smallvec, SmallVec}; use std::hint::black_box; -fn dao_bench(c: &mut Criterion) { - for (allow_diag, pruning) in [(true, false), (true, true)] { - let bench_set = if allow_diag { - ["dao/arena", "dao/den312d", "dao/arena2"] - } else { - ["dao/arena", "dao/den009d", "dao/den312d"] - }; +fn dao_bench(c: &mut Criterion) { + let arr: SmallVec<[bool; 2]> = if ALLOW_DIAGONAL { + smallvec![false] + } else { + smallvec![false] + }; + let bench_set = if ALLOW_DIAGONAL { + ["dao/arena2"] + } else { + ["dao/arena2"] + }; + for pruning in arr { for name in bench_set { let (bool_grid, scenarios) = get_benchmark(name.to_owned()); - let mut pathing_grid: PathingGrid = - PathingGrid::new(bool_grid.width, bool_grid.height, true); + let mut pathing_grid: Pathfinder = + Pathfinder::new(bool_grid.width, bool_grid.height, true); pathing_grid.grid = bool_grid.clone(); - pathing_grid.allow_diagonal_move = allow_diag; pathing_grid.improved_pruning = pruning; pathing_grid.initialize(); pathing_grid.generate_components(); - let diag_str = if allow_diag { "8-grid" } else { "4-grid" }; + let diag_str = if ALLOW_DIAGONAL { "8-grid" } else { "4-grid" }; let improved_str = if pruning { " (improved pruning)" } else { "" }; c.bench_function(format!("{name}, {diag_str}{improved_str}").as_str(), |b| { b.iter(|| { - for (start, end) in &scenarios { - black_box(pathing_grid.get_path_single_goal(*start, *end, false)); + for (start, end, _) in &scenarios { + black_box(pathing_grid.get_path_single_goal(*start, *end)); } }) }); @@ -34,5 +43,68 @@ fn dao_bench(c: &mut Criterion) { } } -criterion_group!(benches, dao_bench); +fn dao_bench_solver( + c: &mut Criterion, + solver_name: &str, + create_solver: FS, +) where + S: GridSolver, + FS: Fn(&mut PathingGrid) -> S, +{ + let bench_set = if ALLOW_DIAGONAL { + ["dao/arena2"] + } else { + ["dao/arena2"] + }; + for name in bench_set { + let (bool_grid, scenarios) = get_benchmark(name.to_owned()); + let mut pathing_grid: PathingGrid = + PathingGrid::new(bool_grid.width, bool_grid.height, true); + pathing_grid.grid = bool_grid.clone(); + pathing_grid.generate_components(); + let solver = create_solver(&mut pathing_grid); + let diag_str = if ALLOW_DIAGONAL { "8-grid" } else { "4-grid" }; + + c.bench_function(format!("{name}, {solver_name} {diag_str}").as_str(), |b| { + b.iter(|| { + for (start, end, _) in &scenarios { + black_box(solver.get_path_single_goal(&mut pathing_grid, *start, *end)); + } + }) + }); + } +} + +fn dao_bench_jps(c: &mut Criterion) { + let arr: SmallVec<[bool; 2]> = if ALLOW_DIAGONAL { + smallvec![false] + } else { + smallvec![false] + }; + for pruning in arr { + let improved_str = if pruning { " (improved pruning)" } else { "" }; + dao_bench_solver( + c, + format!("JPS{improved_str}").as_str(), + |pathing_grid: &mut PathingGrid| { + let mut solver = JPSSolver::new(&pathing_grid, pruning); + solver.initialize(&pathing_grid); + solver + }, + ); + } +} +fn dao_bench_astar(c: &mut Criterion) { + dao_bench_solver(c, "Astar", |_: &mut PathingGrid| { + AstarSolver::new() + }); +} + +fn dao_bench_dijkstra(c: &mut Criterion) { + dao_bench_solver(c, "Dijkstra", |_: &mut PathingGrid| { + DijkstraSolver + }); +} + +criterion_group!(benches, dao_bench, dao_bench_jps,); criterion_main!(benches); diff --git a/benches/single_bench.rs b/benches/single_bench.rs index 60c27b3..8aecb15 100644 --- a/benches/single_bench.rs +++ b/benches/single_bench.rs @@ -1,28 +1,30 @@ use criterion::{criterion_group, criterion_main, Criterion}; -use grid_pathfinding::PathingGrid; +use grid_pathfinding::{ + pathing_grid::PathingGrid, + solver::{jps::JPSSolver, GridSolver}, +}; use grid_pathfinding_benchmark::*; use grid_util::grid::ValueGrid; use std::hint::black_box; -fn dao_bench_single(c: &mut Criterion) { - for (allow_diag, pruning) in [(true, false)] { +fn dao_bench_single(c: &mut Criterion) { + for pruning in [false] { let bench_set = ["dao/arena"]; for name in bench_set { let (bool_grid, scenarios) = get_benchmark(name.to_owned()); - let mut pathing_grid: PathingGrid = + let mut pathing_grid: PathingGrid = PathingGrid::new(bool_grid.width, bool_grid.height, true); pathing_grid.grid = bool_grid.clone(); - pathing_grid.allow_diagonal_move = allow_diag; - pathing_grid.improved_pruning = pruning; - pathing_grid.initialize(); pathing_grid.generate_components(); - let diag_str = if allow_diag { "8-grid" } else { "4-grid" }; + let mut solver = JPSSolver::new(&pathing_grid, pruning); + solver.initialize(&pathing_grid); + let diag_str = if ALLOW_DIAGONAL { "8-grid" } else { "4-grid" }; let improved_str = if pruning { " (improved pruning)" } else { "" }; c.bench_function(format!("{name}, {diag_str}{improved_str}").as_str(), |b| { b.iter(|| { - for (start, end) in &scenarios { - black_box(pathing_grid.get_path_single_goal(*start, *end, false)); + for (start, end, _) in &scenarios { + black_box(solver.get_path_single_goal(&mut pathing_grid, *start, *end)); } }) }); @@ -30,5 +32,5 @@ fn dao_bench_single(c: &mut Criterion) { } } -criterion_group!(benches, dao_bench_single); +criterion_group!(benches, dao_bench_single, dao_bench_single); criterion_main!(benches); diff --git a/examples/benchmark_runner.rs b/examples/benchmark_runner.rs index 2f22aa2..9a1f7cf 100644 --- a/examples/benchmark_runner.rs +++ b/examples/benchmark_runner.rs @@ -1,4 +1,4 @@ -use grid_pathfinding::PathingGrid; +use grid_pathfinding::Pathfinder; use grid_pathfinding_benchmark::*; use grid_util::grid::ValueGrid; use grid_util::point::Point; @@ -12,32 +12,32 @@ fn main() { let (bool_grid, scenarios) = get_benchmark(name); // for (allow_diag, pruning) in [(false, false), (true, false), (true, true)] { - for (allow_diag, pruning) in [(true, false)] { - let mut pathing_grid: PathingGrid = - PathingGrid::new(bool_grid.width, bool_grid.height, true); - pathing_grid.grid = bool_grid.clone(); - pathing_grid.allow_diagonal_move = allow_diag; - pathing_grid.improved_pruning = pruning; - pathing_grid.initialize(); - pathing_grid.generate_components(); - let number_of_scenarios = scenarios.len() as u32; - let before = Instant::now(); - run_scenarios(&pathing_grid, &scenarios); - let elapsed = before.elapsed(); - println!( - "\tElapsed time: {:.2?}; per scenario: {:.2?}", - elapsed, - elapsed / number_of_scenarios - ); - total_time += elapsed; - } + let mut pathing_grid: Pathfinder = + Pathfinder::new(bool_grid.width, bool_grid.height, true); + pathing_grid.grid = bool_grid.clone(); + pathing_grid.improved_pruning = false; + pathing_grid.initialize(); + pathing_grid.generate_components(); + let number_of_scenarios = scenarios.len() as u32; + let before = Instant::now(); + run_scenarios(&pathing_grid, &scenarios); + let elapsed = before.elapsed(); + println!( + "\tElapsed time: {:.2?}; per scenario: {:.2?}", + elapsed, + elapsed / number_of_scenarios + ); + total_time += elapsed; } println!("\tTotal benchmark time: {:.2?}", total_time); } -pub fn run_scenarios(pathing_grid: &PathingGrid, scenarios: &Vec<(Point, Point)>) { - for (start, goal) in scenarios { - let path: Option> = pathing_grid.get_waypoints_single_goal(*start, *goal, false); +pub fn run_scenarios( + pathing_grid: &Pathfinder, + scenarios: &Vec<(Point, Point, f64)>, +) { + for (start, goal, _) in scenarios { + let path: Option> = pathing_grid.get_waypoints_single_goal(*start, *goal); assert!(path.is_some()); } } diff --git a/examples/heuristic_factor.rs b/examples/heuristic_factor.rs index a5f918f..4b73706 100644 --- a/examples/heuristic_factor.rs +++ b/examples/heuristic_factor.rs @@ -1,4 +1,4 @@ -use grid_pathfinding::PathingGrid; +use grid_pathfinding::Pathfinder; use grid_util::grid::ValueGrid; use grid_util::point::Point; use grid_util::Rect; @@ -8,7 +8,7 @@ use grid_util::Rect; fn main() { const N: i32 = 30; - let mut pathing_grid: PathingGrid = PathingGrid::new(N as usize, N as usize, true); + let mut pathing_grid: Pathfinder = Pathfinder::new(N as usize, N as usize, true); pathing_grid.heuristic_factor = 1.3; pathing_grid.set_rect(Rect::new(1, 1, N - 2, N - 2), false); pathing_grid.set_rect(Rect::new(8, 8, 8, 8), true); @@ -18,9 +18,7 @@ fn main() { println!("{}", pathing_grid); let start = Point::new(1, 1); let end = Point::new(N - 3, N - 3); - let path = pathing_grid - .get_path_single_goal(start, end, false) - .unwrap(); + let path = pathing_grid.get_path_single_goal(start, end).unwrap(); println!("Path:"); for p in path { println!("{:?}", p); diff --git a/examples/multiple_goals.rs b/examples/multiple_goals.rs index a467c14..8930816 100644 --- a/examples/multiple_goals.rs +++ b/examples/multiple_goals.rs @@ -1,4 +1,4 @@ -use grid_pathfinding::PathingGrid; +use grid_pathfinding::Pathfinder; use grid_util::grid::ValueGrid; use grid_util::point::Point; @@ -15,7 +15,7 @@ use grid_util::point::Point; // The found path moves to the closest goal, which is the top one. fn main() { - let mut pathing_grid: PathingGrid = PathingGrid::new(3, 3, false); + let mut pathing_grid: Pathfinder = Pathfinder::new(3, 3, false); pathing_grid.set(1, 1, true); pathing_grid.generate_components(); println!("{}", pathing_grid); diff --git a/examples/paths_and_waypoints.rs b/examples/paths_and_waypoints.rs index 21f5c72..89b9e2c 100644 --- a/examples/paths_and_waypoints.rs +++ b/examples/paths_and_waypoints.rs @@ -1,4 +1,4 @@ -use grid_pathfinding::{waypoints_to_path, PathingGrid}; +use grid_pathfinding::{waypoints_to_path, Pathfinder}; use grid_util::grid::ValueGrid; use grid_util::point::Point; @@ -20,13 +20,13 @@ use grid_util::point::Point; // path, as a shorthand for the two previous calls. fn main() { - let mut pathing_grid: PathingGrid = PathingGrid::new(5, 5, false); + let mut pathing_grid: Pathfinder = Pathfinder::new(5, 5, false); pathing_grid.set(1, 1, true); pathing_grid.generate_components(); println!("{}", pathing_grid); let start = Point::new(0, 0); let end = Point::new(4, 4); - if let Some(path) = pathing_grid.get_waypoints_single_goal(start, end, false) { + if let Some(path) = pathing_grid.get_waypoints_single_goal(start, end) { println!("Waypoints:"); for p in &path { println!("{:?}", p); @@ -37,9 +37,7 @@ fn main() { } } println!("\nDirectly computed path"); - let expanded_path = pathing_grid - .get_path_single_goal(start, end, false) - .unwrap(); + let expanded_path = pathing_grid.get_path_single_goal(start, end).unwrap(); for p in expanded_path { println!("{:?}", p); } diff --git a/examples/simple_4.rs b/examples/simple_4.rs index e3da7c2..7fce188 100644 --- a/examples/simple_4.rs +++ b/examples/simple_4.rs @@ -1,4 +1,4 @@ -use grid_pathfinding::PathingGrid; +use grid_pathfinding::Pathfinder; use grid_util::grid::ValueGrid; use grid_util::point::Point; @@ -16,16 +16,13 @@ use grid_util::point::Point; // Nodes have a 4-neighborhood fn main() { - let mut pathing_grid: PathingGrid = PathingGrid::new(3, 3, false); - pathing_grid.allow_diagonal_move = false; + let mut pathing_grid: Pathfinder = Pathfinder::new(3, 3, false); pathing_grid.set(1, 1, true); pathing_grid.generate_components(); println!("{}", pathing_grid); let start = Point::new(0, 0); let end = Point::new(2, 2); - let path = pathing_grid - .get_path_single_goal(start, end, false) - .unwrap(); + let path = pathing_grid.get_path_single_goal(start, end).unwrap(); println!("Path:"); for p in path { println!("{:?}", p); diff --git a/examples/simple_8.rs b/examples/simple_8.rs index 0654deb..4837c91 100644 --- a/examples/simple_8.rs +++ b/examples/simple_8.rs @@ -1,4 +1,4 @@ -use grid_pathfinding::PathingGrid; +use grid_pathfinding::Pathfinder; use grid_util::grid::ValueGrid; use grid_util::point::Point; @@ -16,15 +16,13 @@ use grid_util::point::Point; // Nodes have an 8-neighborhood fn main() { - let mut pathing_grid: PathingGrid = PathingGrid::new(3, 3, false); + let mut pathing_grid: Pathfinder = Pathfinder::new(3, 3, false); pathing_grid.set(1, 1, true); pathing_grid.generate_components(); println!("{}", pathing_grid); let start = Point::new(0, 0); let end = Point::new(2, 2); - let path = pathing_grid - .get_path_single_goal(start, end, false) - .unwrap(); + let path = pathing_grid.get_path_single_goal(start, end).unwrap(); println!("Path:"); for p in path { println!("{:?}", p); diff --git a/grid_pathfinding_benchmark/src/lib.rs b/grid_pathfinding_benchmark/src/lib.rs index a1e318a..7ddd7a2 100644 --- a/grid_pathfinding_benchmark/src/lib.rs +++ b/grid_pathfinding_benchmark/src/lib.rs @@ -22,7 +22,7 @@ pub struct Scenario { distance: f64, } -fn load_benchmark(name: &str) -> (BoolGrid, Vec<(Point, Point)>) { +fn load_benchmark(name: &str) -> (BoolGrid, Vec<(Point, Point, f64)>) { let map_str = fs::read_to_string(Path::new(&format!("./maps/{}.map", name))) .expect("Could not read scenario file"); @@ -45,14 +45,14 @@ fn load_benchmark(name: &str) -> (BoolGrid, Vec<(Point, Point)>) { // .flexible(true) .from_reader(remaining_data.as_bytes()); // Initialize an empty vector to store the parsed data - let mut data_array: Vec<(Point, Point)> = Vec::new(); + let mut data_array: Vec<(Point, Point, f64)> = Vec::new(); // Iterate over the records in the file for result in csv_reader.deserialize() { let record: Scenario = result.expect("Could not parse scenario record"); let start = Point::new(record.y1 as i32, record.x1 as i32); let goal = Point::new(record.y2 as i32, record.x2 as i32); - data_array.push((start, goal)); + data_array.push((start, goal, record.distance)); } let lines: Vec<&str> = map_str.lines().collect(); @@ -73,7 +73,7 @@ fn load_benchmark(name: &str) -> (BoolGrid, Vec<(Point, Point)>) { for x in 0..bool_grid.width() as i32 { // Not sure why x, y have to be swapped here... let tile_val = lines[offset + x as usize].as_bytes()[y as usize]; - let val = ![b'.', b'G'].contains(&tile_val); + let val = ![b'.', b'G', b'S'].contains(&tile_val); bool_grid.set(x, y, val); } } @@ -105,7 +105,7 @@ pub fn get_benchmark_names() -> Vec { names } -pub fn get_benchmark(name: String) -> (BoolGrid, Vec<(Point, Point)>) { +pub fn get_benchmark(name: String) -> (BoolGrid, Vec<(Point, Point, f64)>) { let benchmark_names = get_benchmark_names(); if benchmark_names.contains(&name) { load_benchmark(name.as_str()) diff --git a/src/astar_jps.rs b/src/astar_jps.rs index f2089c9..507ac06 100644 --- a/src/astar_jps.rs +++ b/src/astar_jps.rs @@ -17,7 +17,7 @@ use std::hash::Hash; #[derive(Clone, Debug)] -struct SearchNode { +pub(crate) struct SearchNode { estimated_cost: K, cost: K, index: usize, @@ -68,24 +68,55 @@ where path } +pub trait Frontier: Default +where + C: Zero + Ord + Copy, +{ + fn clear(&mut self); + fn pop(&mut self) -> Option>; + fn push(&mut self, item: SearchNode); +} + +impl Frontier for BinaryHeap> +where + C: Zero + Ord + Copy, +{ + fn clear(&mut self) { + self.clear(); + } + + fn pop(&mut self) -> Option> { + self.pop() + } + + fn push(&mut self, item: SearchNode) { + self.push(item); + } +} + /// [AstarContext] represents the search fringe and node parent map, facilitating reuse of memory allocations. #[derive(Clone, Debug)] -pub struct AstarContext { - fringe: BinaryHeap>, +pub struct SearchContext { + fringe: F, parents: FxIndexMap, } -impl AstarContext +pub type BinaryHeapSearchContext = SearchContext>>; +pub type DefaultSearchContext = BinaryHeapSearchContext; + +impl SearchContext where N: Eq + Hash + Clone, C: Zero + Ord + Copy, + F: Frontier, { - pub fn new() -> AstarContext { - AstarContext { - fringe: BinaryHeap::new(), + pub fn new() -> SearchContext { + SearchContext { + fringe: F::default(), parents: FxIndexMap::default(), } } + pub fn astar_jps( &mut self, start: &N, @@ -167,12 +198,12 @@ pub fn astar_jps( ) -> Option<(Vec, C)> where N: Eq + Hash + Clone, - C: Zero + Ord + Copy, + C: Zero + Ord + Copy + Hash, FN: FnMut(&Option<&N>, &N) -> IN, IN: IntoIterator, FH: FnMut(&N) -> C, FS: FnMut(&N) -> bool, { - let mut search = AstarContext::new(); + let mut search = DefaultSearchContext::new(); search.astar_jps(start, successors, heuristic, success) } diff --git a/src/lib.rs b/src/lib.rs index f21c5cf..1246142 100755 --- a/src/lib.rs +++ b/src/lib.rs @@ -7,8 +7,11 @@ //! pathfinding. Note that this assumes a uniform-cost grid. Pre-computes //! [connected components](https://en.wikipedia.org/wiki/Component_(graph_theory)) //! to avoid flood-filling behaviour if no path exists. + mod astar_jps; -use astar_jps::AstarContext; +pub mod pathing_grid; +pub mod solver; +use astar_jps::SearchContext; use core::fmt; use grid_util::direction::Direction; use grid_util::grid::{BoolGrid, SimpleValueGrid, ValueGrid}; @@ -18,6 +21,9 @@ use smallvec::SmallVec; use std::collections::VecDeque; use std::sync::{Arc, Mutex}; +use crate::astar_jps::DefaultSearchContext; + +pub const ALLOW_CORNER_CUTTING: bool = false; const EQUAL_EDGE_COST: bool = false; const GRAPH_PRUNING: bool = true; const N_SMALLVEC_SIZE: usize = 8; @@ -29,6 +35,11 @@ const D: i32 = if EQUAL_EDGE_COST { 1 } else { 99 }; const C: i32 = if EQUAL_EDGE_COST { 1 } else { 70 }; const E: i32 = 2 * C - D; +/// Converts the integer cost to an approximate floating point equivalent where cardinal directions have cost 1.0. +pub fn convert_cost_to_unit_cost_float(cost: i32) -> f64 { + (cost as f64) / (C as f64) +} + /// Helper function for debugging binary representations of neighborhoods. pub fn explain_bin_neighborhood(nn: u8) { for i in 0..8_i32 { @@ -62,7 +73,7 @@ pub fn waypoints_to_path(waypoints: Vec) -> Vec { /// empty ([false]). It also records neighbours in [u8] format for fast lookups during search. /// Implements [Grid] by building on [BoolGrid]. #[derive(Clone, Debug)] -pub struct PathingGrid { +pub struct Pathfinder { pub grid: BoolGrid, pub neighbours: SimpleValueGrid, pub jump_point: SimpleValueGrid, @@ -70,13 +81,12 @@ pub struct PathingGrid { pub components_dirty: bool, pub heuristic_factor: f32, pub improved_pruning: bool, - pub allow_diagonal_move: bool, - context: Arc>>, + context: Arc>>, } -impl Default for PathingGrid { - fn default() -> PathingGrid { - let mut grid = PathingGrid { +impl Default for Pathfinder { + fn default() -> Pathfinder { + let mut grid = Pathfinder { grid: BoolGrid::default(), neighbours: SimpleValueGrid::default(), jump_point: SimpleValueGrid::default(), @@ -84,16 +94,15 @@ impl Default for PathingGrid { components_dirty: false, improved_pruning: true, heuristic_factor: 1.0, - allow_diagonal_move: true, - context: Arc::new(Mutex::new(AstarContext::new())), + context: Arc::new(Mutex::new(SearchContext::new())), }; grid.initialize(); grid } } -impl PathingGrid { +impl Pathfinder { fn neighborhood_points(&self, point: &Point) -> SmallVec<[Point; 8]> { - if self.allow_diagonal_move { + if ALLOW_DIAGONAL { point.moore_neighborhood_smallvec() } else { point.neumann_neighborhood_smallvec() @@ -105,14 +114,14 @@ impl PathingGrid { ) -> SmallVec<[(Point, i32); N_SMALLVEC_SIZE]> { self.neighborhood_points(pos) .into_iter() - .filter(|p| self.can_move_to(*p)) + .filter(|p| self.can_move_to(*p, *pos)) // See comment in pruned_neighborhood about cost calculation .map(move |p| (p, (pos.dir_obj(&p).num() % 2) * (D - C) + C)) .collect::>() } /// Uses C as cost for cardinal (straight) moves and D for diagonal moves. pub fn heuristic(&self, p1: &Point, p2: &Point) -> i32 { - if self.allow_diagonal_move { + if ALLOW_DIAGONAL { let delta_x = (p1.x - p2.x).abs(); let delta_y = (p1.y - p2.y).abs(); // Formula from https://github.com/riscy/a_star_on_grids @@ -123,7 +132,17 @@ impl PathingGrid { p1.manhattan_distance(p2) * C } } - fn can_move_to(&self, pos: Point) -> bool { + fn can_move_to(&self, pos: Point, start: Point) -> bool { + if ALLOW_CORNER_CUTTING { + self.can_move_to_simple(pos) + } else { + debug_assert!((start.x - pos.x).abs() <= 1 && (start.y - pos.y).abs() <= 1); + self.can_move_to_simple(pos) + && (!self.grid.get_point(Point::new(start.x, pos.y)) + && !self.grid.get_point(Point::new(pos.x, start.y))) + } + } + fn can_move_to_simple(&self, pos: Point) -> bool { self.point_in_bounds(pos) && !self.grid.get_point(pos) } fn in_bounds(&self, x: i32, y: i32) -> bool { @@ -142,8 +161,9 @@ impl PathingGrid { let mut forced_mask: u8 = 0; for dir_num in 0..8 { if dir_num % 2 == 1 { - if !self.indexed_neighbor(node, 3 + dir_num) - || !self.indexed_neighbor(node, 5 + dir_num) + if ALLOW_CORNER_CUTTING + && (!self.indexed_neighbor(node, 3 + dir_num) + || !self.indexed_neighbor(node, 5 + dir_num)) { forced_mask |= 1 << dir_num; } @@ -166,7 +186,7 @@ impl PathingGrid { let dir_num = dir.num(); let mut n_mask: u8; let mut neighbours = self.neighbours.get_point(*node); - if !self.allow_diagonal_move { + if !ALLOW_DIAGONAL { neighbours &= 0b01010101; n_mask = 0b01000101_u8.rotate_left(dir_num as u32); } else if dir.diagonal() { @@ -178,17 +198,23 @@ impl PathingGrid { n_mask |= 1 << ((dir_num + 6) % 8); } } else { - n_mask = 0b00000001 << dir_num; - if !self.indexed_neighbor(node, 2 + dir_num) { - n_mask |= 1 << ((dir_num + 1) % 8); - } - if !self.indexed_neighbor(node, 6 + dir_num) { - n_mask |= 1 << ((dir_num + 7) % 8); + if ALLOW_CORNER_CUTTING { + n_mask = 0b00000001 << dir_num; + if !self.indexed_neighbor(node, 2 + dir_num) { + n_mask |= 1 << ((dir_num + 1) % 8); + } + if !self.indexed_neighbor(node, 6 + dir_num) { + n_mask |= 1 << ((dir_num + 7) % 8); + } + } else { + // TODO: look into whether this is minimal, this at least makes the algorithm + // optimal and complete following the no corner cutting rule + n_mask = 0b11010111_u8.rotate_left(dir_num as u32); } } let comb_mask = neighbours & n_mask; (0..8) - .step_by(if self.allow_diagonal_move { 1 } else { 2 }) + .step_by(if ALLOW_DIAGONAL { 1 } else { 2 }) .filter(move |x| comb_mask & (1 << *x) != 0) // (dir_num % 2) * (D-C) + C) // is an optimized version without a conditional of @@ -210,7 +236,7 @@ impl PathingGrid { debug_assert!(!direction.diagonal()); loop { initial = initial + direction; - if !self.can_move_to(initial) { + if !self.can_move_to_simple(initial) { return None; } @@ -234,11 +260,13 @@ impl PathingGrid { where F: Fn(&Point) -> bool, { + let mut new_initial: Point; loop { - initial = initial + direction; - if !self.can_move_to(initial) { + new_initial = initial + direction; + if !self.can_move_to(new_initial, initial) { return None; } + initial = new_initial; if goal(&initial) || self.is_forced(direction, &initial) { return Some((initial, cost)); @@ -257,7 +285,7 @@ impl PathingGrid { // When using a 4-neighborhood (specified by setting allow_diagonal_move to false), // jumps perpendicular to the direction are performed. This is necessary to not miss the // goal when passing by. - if !self.allow_diagonal_move { + if !ALLOW_DIAGONAL || !ALLOW_CORNER_CUTTING && !direction.diagonal() { let perp_1 = direction.rotate_ccw(2); let perp_2 = direction.rotate_cw(2); if self.jump_straight(initial, 1, perp_1, goal).is_some() @@ -265,6 +293,15 @@ impl PathingGrid { { return Some((initial, cost)); } + if !ALLOW_CORNER_CUTTING && !direction.diagonal() { + let diag_1 = direction.rotate_ccw(1); + let diag_2 = direction.rotate_cw(1); + if self.jump(initial, 1, diag_1, goal).is_some() + || self.jump(initial, 1, diag_2, goal).is_some() + { + return Some((initial, cost)); + } + } } // See comment in pruned_neighborhood about cost calculation @@ -386,13 +423,16 @@ impl PathingGrid { /// called Weighted A*. In pathfinding language, a factor greater than /// 1.0 will make the heuristic [inadmissible](https://en.wikipedia.org/wiki/Admissible_heuristic), a requirement for solution optimality. By default, /// the [heuristic_factor](Self::heuristic_factor) is 1.0 which gives optimal solutions. - pub fn get_path_single_goal( + pub fn get_path_single_goal(&self, start: Point, goal: Point) -> Option> { + self.get_waypoints_single_goal(start, goal) + .map(waypoints_to_path) + } + pub fn get_path_single_goal_approximate( &self, start: Point, goal: Point, - approximate: bool, ) -> Option> { - self.get_waypoints_single_goal(start, goal, approximate) + self.get_waypoints_single_goal_approximate(start, goal) .map(waypoints_to_path) } @@ -437,54 +477,54 @@ impl PathingGrid { result.map(|(v, _c)| (*v.last().unwrap(), v)) } /// The raw waypoints (jump points) from which [get_path_single_goal](Self::get_path_single_goal) makes a path. - pub fn get_waypoints_single_goal( + pub fn get_waypoints_single_goal(&self, start: Point, goal: Point) -> Option> { + // Check if start and goal are on the same connected component. + if self.unreachable(&start, &goal) { + return None; + } + // The goal is reachable from the start, compute a path + let mut ct = self.context.lock().unwrap(); + ct.astar_jps( + &start, + |parent, node| { + if GRAPH_PRUNING { + self.jps_neighbours(*parent, node, &|node_pos| *node_pos == goal) + } else { + self.neighborhood_points_and_cost(node) + } + }, + |point| (self.heuristic(point, &goal) as f32 * self.heuristic_factor) as i32, + |point| *point == goal, + ) + .map(|(v, _c)| v) + } + /// The raw waypoints (jump points) from which [get_path_single_goal](Self::get_path_single_goal) makes a path. + pub fn get_waypoints_single_goal_approximate( &self, start: Point, goal: Point, - approximate: bool, ) -> Option> { - if approximate { - // Check if start and one of the goal neighbours are on the same connected component. - if self.neighbours_unreachable(&start, &goal) { - // No neigbhours of the goal are reachable from the start - return None; - } - // A neighbour of the goal can be reached, compute a path - let mut ct = self.context.lock().unwrap(); - ct.astar_jps( - &start, - |parent, node| { - if GRAPH_PRUNING { - self.jps_neighbours(*parent, node, &|node_pos| { - self.heuristic(node_pos, &goal) <= if EQUAL_EDGE_COST { 1 } else { 99 } - }) - } else { - self.neighborhood_points_and_cost(node) - } - }, - |point| (self.heuristic(point, &goal) as f32 * self.heuristic_factor) as i32, - |point| self.heuristic(point, &goal) <= if EQUAL_EDGE_COST { 1 } else { 99 }, - ) - } else { - // Check if start and goal are on the same connected component. - if self.unreachable(&start, &goal) { - return None; - } - // The goal is reachable from the start, compute a path - let mut ct = self.context.lock().unwrap(); - ct.astar_jps( - &start, - |parent, node| { - if GRAPH_PRUNING { - self.jps_neighbours(*parent, node, &|node_pos| *node_pos == goal) - } else { - self.neighborhood_points_and_cost(node) - } - }, - |point| (self.heuristic(point, &goal) as f32 * self.heuristic_factor) as i32, - |point| *point == goal, - ) + // Check if start and one of the goal neighbours are on the same connected component. + if self.neighbours_unreachable(&start, &goal) { + // No neigbhours of the goal are reachable from the start + return None; } + // A neighbour of the goal can be reached, compute a path + let mut ct = self.context.lock().unwrap(); + ct.astar_jps( + &start, + |parent, node| { + if GRAPH_PRUNING { + self.jps_neighbours(*parent, node, &|node_pos| { + self.heuristic(node_pos, &goal) <= if EQUAL_EDGE_COST { 1 } else { 99 } + }) + } else { + self.neighborhood_points_and_cost(node) + } + }, + |point| (self.heuristic(point, &goal) as f32 * self.heuristic_factor) as i32, + |point| self.heuristic(point, &goal) <= if EQUAL_EDGE_COST { 1 } else { 99 }, + ) .map(|(v, _c)| v) } /// Regenerates the components if they are marked as dirty. @@ -551,7 +591,7 @@ impl PathingGrid { let point = Point::new(x, y); let parent_ix = self.grid.get_ix_point(&point); - if self.allow_diagonal_move { + if ALLOW_DIAGONAL { vec![ Point::new(point.x, point.y + 1), Point::new(point.x, point.y - 1), @@ -568,7 +608,9 @@ impl PathingGrid { ] } .into_iter() - .filter(|p| self.grid.point_in_bounds(*p) && !self.grid.get_point(*p)) + .filter(|p| self.can_move_to(*p, point)) + .collect::>() + .iter() .for_each(|p| { let ix = self.grid.get_ix_point(&p); self.components.union(parent_ix, ix); @@ -578,7 +620,7 @@ impl PathingGrid { } } } -impl fmt::Display for PathingGrid { +impl fmt::Display for Pathfinder { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { writeln!(f, "Grid:")?; for y in 0..self.grid.height as i32 { @@ -598,9 +640,9 @@ impl fmt::Display for PathingGrid { } } -impl ValueGrid for PathingGrid { +impl ValueGrid for Pathfinder { fn new(width: usize, height: usize, default_value: bool) -> Self { - let mut base_grid = PathingGrid { + let mut base_grid = Pathfinder { grid: BoolGrid::new(width, height, default_value), jump_point: SimpleValueGrid::new(width, height, 0b00000000), neighbours: SimpleValueGrid::new(width, height, 0b11111111), @@ -608,8 +650,7 @@ impl ValueGrid for PathingGrid { components_dirty: false, improved_pruning: true, heuristic_factor: 1.0, - allow_diagonal_move: true, - context: Arc::new(Mutex::new(AstarContext::new())), + context: Arc::new(Mutex::new(SearchContext::new())), }; base_grid.initialize(); base_grid @@ -625,9 +666,9 @@ impl ValueGrid for PathingGrid { self.components_dirty = true; } else { let p_ix = self.grid.compute_ix(x, y); - for p in self.neighborhood_points(&p) { - if self.can_move_to(p) { - self.components.union(p_ix, self.grid.get_ix_point(&p)); + for n in self.neighborhood_points(&p) { + if self.can_move_to(n, p) { + self.components.union(p_ix, self.grid.get_ix_point(&n)); } } } @@ -642,215 +683,3 @@ impl ValueGrid for PathingGrid { self.grid.height() } } - -#[cfg(test)] -mod tests { - use grid_util::Rect; - - use super::*; - - /// Tests whether points are correctly mapped to different connected components - #[test] - fn test_component_generation() { - // Corresponds to the following 3x3 grid: - // ___ - // | # | - // | # | - // ___ - let mut path_graph = PathingGrid::new(3, 2, false); - path_graph.grid.set(1, 0, true); - path_graph.grid.set(1, 1, true); - let f_ix = |p| path_graph.get_ix_point(p); - let p1 = Point::new(0, 0); - let p2 = Point::new(1, 1); - let p3 = Point::new(0, 1); - let p4 = Point::new(2, 0); - let p1_ix = f_ix(&p1); - let p2_ix = f_ix(&p2); - let p3_ix = f_ix(&p3); - let p4_ix = f_ix(&p4); - path_graph.generate_components(); - assert!(!path_graph.components.equiv(p1_ix, p2_ix)); - assert!(path_graph.components.equiv(p1_ix, p3_ix)); - assert!(!path_graph.components.equiv(p1_ix, p4_ix)); - } - - #[test] - fn reachable_with_diagonals() { - let mut path_graph = PathingGrid::new(3, 2, false); - path_graph.grid.set(1, 0, true); - path_graph.grid.set(1, 1, true); - let p1 = Point::new(0, 0); - let p2 = Point::new(1, 0); - let p3 = Point::new(0, 1); - let p4 = Point::new(2, 0); - path_graph.generate_components(); - assert!(path_graph.unreachable(&p1, &p2)); - assert!(!path_graph.unreachable(&p1, &p3)); - assert!(path_graph.unreachable(&p1, &p4)); - assert!(!path_graph.neighbours_unreachable(&p1, &p2)); - assert!(path_graph.neighbours_unreachable(&p1, &p4)); - } - - /// Asserts that the two corners are connected on a 4-grid. - #[test] - fn reachable_without_diagonals() { - // |S | - // | # | - // | G| - // ___ - let mut pathing_grid: PathingGrid = PathingGrid::new(3, 3, false); - pathing_grid.improved_pruning = false; - pathing_grid.allow_diagonal_move = false; - pathing_grid.set(1, 1, true); - pathing_grid.generate_components(); - let start = Point::new(0, 0); - let end = Point::new(2, 2); - assert!(pathing_grid.reachable(&start, &end)); - } - - /// Asserts that the case in which start and goal are equal is handled correctly. - #[test] - fn equal_start_goal() { - for (allow_diag, pruning) in [(false, false), (true, false), (true, true)] { - let mut pathing_grid: PathingGrid = PathingGrid::new(1, 1, false); - pathing_grid.allow_diagonal_move = allow_diag; - pathing_grid.improved_pruning = pruning; - pathing_grid.generate_components(); - let start = Point::new(0, 0); - let path = pathing_grid - .get_path_single_goal(start, start, false) - .unwrap(); - assert!(path.len() == 1); - } - } - - /// Asserts that the optimal 4 step solution is found. - #[test] - fn solve_simple_problem() { - for (allow_diag, pruning, expected) in - [(false, false, 5), (true, false, 4), (true, true, 4)] - { - let mut pathing_grid: PathingGrid = PathingGrid::new(3, 3, false); - pathing_grid.allow_diagonal_move = allow_diag; - pathing_grid.improved_pruning = pruning; - pathing_grid.set(1, 1, true); - pathing_grid.generate_components(); - let start = Point::new(0, 0); - let end = Point::new(2, 2); - let path = pathing_grid - .get_path_single_goal(start, end, false) - .unwrap(); - assert!(path.len() == expected); - } - } - - #[test] - fn test_multiple_goals() { - for (allow_diag, pruning, expected) in - [(false, false, 7), (true, false, 5), (true, true, 5)] - { - let mut pathing_grid: PathingGrid = PathingGrid::new(5, 5, false); - pathing_grid.allow_diagonal_move = allow_diag; - pathing_grid.improved_pruning = pruning; - pathing_grid.set(1, 1, true); - pathing_grid.generate_components(); - let start = Point::new(0, 0); - let goal_1 = Point::new(4, 4); - let goal_2 = Point::new(3, 3); - let goals = vec![&goal_1, &goal_2]; - let (selected_goal, path) = pathing_grid.get_path_multiple_goals(start, goals).unwrap(); - assert_eq!(selected_goal, Point::new(3, 3)); - assert!(path.len() == expected); - } - } - - #[test] - fn test_complex() { - for (allow_diag, pruning, expected) in - [(false, false, 15), (true, false, 10), (true, true, 10)] - { - let mut pathing_grid: PathingGrid = PathingGrid::new(10, 10, false); - pathing_grid.set_rect(Rect::new(1, 1, 1, 1), true); - pathing_grid.set_rect(Rect::new(5, 0, 1, 1), true); - pathing_grid.set_rect(Rect::new(0, 5, 1, 1), true); - pathing_grid.set_rect(Rect::new(8, 8, 1, 1), true); - // pathing_grid.improved_pruning = false; - pathing_grid.allow_diagonal_move = allow_diag; - pathing_grid.improved_pruning = pruning; - pathing_grid.generate_components(); - let start = Point::new(0, 0); - let end = Point::new(7, 7); - let path = pathing_grid - .get_path_single_goal(start, end, false) - .unwrap(); - assert!(path.len() == expected); - } - } - #[test] - fn test_complex_waypoints() { - for (allow_diag, pruning, expected) in - [(false, false, 11), (true, false, 7), (true, true, 5)] - { - let mut pathing_grid: PathingGrid = PathingGrid::new(10, 10, false); - pathing_grid.set_rect(Rect::new(1, 1, 1, 1), true); - pathing_grid.set_rect(Rect::new(5, 0, 1, 1), true); - pathing_grid.set_rect(Rect::new(0, 5, 1, 1), true); - pathing_grid.set_rect(Rect::new(8, 8, 1, 1), true); - // pathing_grid.improved_pruning = false; - pathing_grid.allow_diagonal_move = allow_diag; - pathing_grid.improved_pruning = pruning; - pathing_grid.generate_components(); - let start = Point::new(0, 0); - let end = Point::new(7, 7); - let path = pathing_grid - .get_waypoints_single_goal(start, end, false) - .unwrap(); - assert!(path.len() == expected); - } - } - - // Tests whether allowing diagonals has the expected effect on diagonal reachability in a minimal setting. - #[test] - fn test_diagonal_switch_reachable() { - // ___ - // | #| - // |# | - // __ - let mut pathing_grid: PathingGrid = PathingGrid::new(2, 2, true); - pathing_grid.allow_diagonal_move = false; - let mut pathing_grid_diag: PathingGrid = PathingGrid::new(2, 2, true); - for pathing_grid in [&mut pathing_grid, &mut pathing_grid_diag] { - pathing_grid.set(0, 0, false); - pathing_grid.set(1, 1, false); - pathing_grid.generate_components(); - } - let start = Point::new(0, 0); - let end = Point::new(1, 1); - assert!(pathing_grid.unreachable(&start, &end)); - assert!(pathing_grid_diag.reachable(&start, &end)); - } - - // Tests whether allowing diagonals has the expected effect on path existence in a minimal setting. - #[test] - fn test_diagonal_switch_path() { - // ___ - // | #| - // |# | - // __ - let mut pathing_grid: PathingGrid = PathingGrid::new(2, 2, true); - pathing_grid.allow_diagonal_move = false; - let mut pathing_grid_diag: PathingGrid = PathingGrid::new(2, 2, true); - for pathing_grid in [&mut pathing_grid, &mut pathing_grid_diag] { - pathing_grid.set(0, 0, false); - pathing_grid.set(1, 1, false); - pathing_grid.generate_components(); - } - let start = Point::new(0, 0); - let goal = Point::new(1, 1); - let path = pathing_grid.get_path_single_goal(start, goal, false); - let path_diag = pathing_grid_diag.get_path_single_goal(start, goal, false); - assert!(path.is_none()); - assert!(path_diag.is_some()); - } -} diff --git a/src/pathing_grid.rs b/src/pathing_grid.rs new file mode 100644 index 0000000..b13be3e --- /dev/null +++ b/src/pathing_grid.rs @@ -0,0 +1,300 @@ +use crate::astar_jps::DefaultSearchContext; + +use super::*; +use core::fmt; +use grid_util::grid::{BoolGrid, ValueGrid}; +use grid_util::point::Point; +use petgraph::unionfind::UnionFind; +use smallvec::SmallVec; +use std::sync::{Arc, Mutex}; + +#[derive(Clone, Debug)] +pub struct PathingGrid { + pub grid: BoolGrid, + pub components: UnionFind, + pub components_dirty: bool, + pub(crate) context: Arc>>, +} + +impl Default for PathingGrid { + fn default() -> PathingGrid { + let grid = PathingGrid { + grid: BoolGrid::default(), + components: UnionFind::new(0), + components_dirty: false, + context: Arc::new(Mutex::new(SearchContext::new())), + }; + grid + } +} + +impl PathingGrid { + pub fn neighborhood_points(&self, point: &Point) -> SmallVec<[Point; 8]> { + if ALLOW_DIAGONAL { + point.moore_neighborhood_smallvec() + } else { + point.neumann_neighborhood_smallvec() + } + } + pub fn neighborhood_points_and_cost( + &self, + pos: &Point, + ) -> SmallVec<[(Point, i32); N_SMALLVEC_SIZE]> { + self.neighborhood_points(pos) + .into_iter() + .filter(|p| self.can_move_to(*p, *pos)) + // See comment in pruned_neighborhood about cost calculation + .map(move |p| (p, (pos.dir_obj(&p).num() % 2) * (D - C) + C)) + .collect::>() + } + + pub fn can_move_to(&self, pos: Point, start: Point) -> bool { + if ALLOW_CORNER_CUTTING { + self.can_move_to_simple(pos) + } else { + debug_assert!((start.x - pos.x).abs() <= 1 && (start.y - pos.y).abs() <= 1); + self.can_move_to_simple(pos) + && (!self.grid.get_point(Point::new(start.x, pos.y)) + && !self.grid.get_point(Point::new(pos.x, start.y))) + } + } + pub fn can_move_to_simple(&self, pos: Point) -> bool { + self.point_in_bounds(pos) && !self.grid.get_point(pos) + } + fn in_bounds(&self, x: i32, y: i32) -> bool { + self.grid.index_in_bounds(x, y) + } + + /// Retrieves the component id a given [Point] belongs to. + pub fn get_component(&self, point: &Point) -> usize { + self.components.find(self.get_ix_point(point)) + } + /// Checks if start and goal are on the same component. + pub fn reachable(&self, start: &Point, goal: &Point) -> bool { + !self.unreachable(start, goal) + } + + /// Checks if start and goal are not on the same component. + pub fn unreachable(&self, start: &Point, goal: &Point) -> bool { + if self.in_bounds(start.x, start.y) && self.in_bounds(goal.x, goal.y) { + let start_ix = self.get_ix_point(start); + let goal_ix = self.get_ix_point(goal); + !self.components.equiv(start_ix, goal_ix) + } else { + true + } + } + + /// Checks if any neighbour of the goal is on the same component as the start. + pub fn neighbours_reachable(&self, start: &Point, goal: &Point) -> bool { + if self.in_bounds(start.x, start.y) && self.in_bounds(goal.x, goal.y) { + let start_ix = self.get_ix_point(start); + let neighborhood = self.neighborhood_points(goal); + neighborhood.iter().any(|p| { + self.in_bounds(p.x, p.y) && self.components.equiv(start_ix, self.get_ix_point(p)) + }) + } else { + true + } + } + + /// Checks if every neighbour of the goal is on a different component as the start. + pub fn neighbours_unreachable(&self, start: &Point, goal: &Point) -> bool { + if self.in_bounds(start.x, start.y) && self.in_bounds(goal.x, goal.y) { + let start_ix = self.get_ix_point(start); + let neighborhood = self.neighborhood_points(goal); + neighborhood.iter().all(|p| { + !self.in_bounds(p.x, p.y) || !self.components.equiv(start_ix, self.get_ix_point(p)) + }) + } else { + true + } + } + + /// Regenerates the components if they are marked as dirty. + pub fn update(&mut self) { + if self.components_dirty { + // The components are dirty, regenerate them + self.generate_components(); + } + } + + /// Generates a new [UnionFind] structure and links up grid neighbours to the same components. + pub fn generate_components(&mut self) { + let w = self.grid.width; + let h = self.grid.height; + self.components = UnionFind::new(w * h); + self.components_dirty = false; + for x in 0..w as i32 { + for y in 0..h as i32 { + if !self.grid.get(x, y) { + let point = Point::new(x, y); + let parent_ix = self.grid.get_ix_point(&point); + + if ALLOW_DIAGONAL { + vec![ + Point::new(point.x, point.y + 1), + Point::new(point.x, point.y - 1), + Point::new(point.x + 1, point.y), + Point::new(point.x + 1, point.y - 1), + Point::new(point.x + 1, point.y), + Point::new(point.x + 1, point.y + 1), + ] + } else { + vec![ + Point::new(point.x, point.y + 1), + Point::new(point.x, point.y - 1), + Point::new(point.x + 1, point.y), + ] + } + .into_iter() + .filter(|p| self.can_move_to(*p, point)) + .collect::>() + .iter() + .for_each(|p| { + let ix = self.grid.get_ix_point(&p); + self.components.union(parent_ix, ix); + }); + } + } + } + } +} +impl fmt::Display for PathingGrid { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + writeln!(f, "Grid:")?; + for y in 0..self.grid.height as i32 { + let values = (0..self.grid.width as i32) + .map(|x| self.grid.get(x, y) as i32) + .collect::>(); + writeln!(f, "{:?}", values)?; + } + Ok(()) + } +} + +impl ValueGrid for PathingGrid { + fn new(width: usize, height: usize, default_value: bool) -> Self { + let base_grid = PathingGrid { + grid: BoolGrid::new(width, height, default_value), + + components: UnionFind::new(width * height), + components_dirty: false, + context: Arc::new(Mutex::new(SearchContext::new())), + }; + base_grid + } + fn get(&self, x: i32, y: i32) -> bool { + self.grid.get(x, y) + } + /// Updates a position on the grid. Joins newly connected components and flags the components + /// as dirty if components are (potentially) broken apart into multiple. + fn set(&mut self, x: i32, y: i32, blocked: bool) { + let p = Point::new(x, y); + if self.grid.get(x, y) != blocked && blocked { + self.components_dirty = true; + } else { + let p_ix = self.grid.compute_ix(x, y); + for n in self.neighborhood_points(&p) { + if self.can_move_to(n, p) { + self.components.union(p_ix, self.grid.get_ix_point(&n)); + } + } + } + self.grid.set(x, y, blocked); + } + fn width(&self) -> usize { + self.grid.width() + } + fn height(&self) -> usize { + self.grid.height() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + /// Tests whether points are correctly mapped to different connected components + #[test] + fn test_component_generation() { + // Corresponds to the following 3x3 grid: + // ___ + // | # | + // | # | + // ___ + let mut path_graph: PathingGrid = PathingGrid::new(3, 2, false); + path_graph.grid.set(1, 0, true); + path_graph.grid.set(1, 1, true); + let f_ix = |p| path_graph.get_ix_point(p); + let p1 = Point::new(0, 0); + let p2 = Point::new(1, 1); + let p3 = Point::new(0, 1); + let p4 = Point::new(2, 0); + let p1_ix = f_ix(&p1); + let p2_ix = f_ix(&p2); + let p3_ix = f_ix(&p3); + let p4_ix = f_ix(&p4); + path_graph.generate_components(); + assert!(!path_graph.components.equiv(p1_ix, p2_ix)); + assert!(path_graph.components.equiv(p1_ix, p3_ix)); + assert!(!path_graph.components.equiv(p1_ix, p4_ix)); + } + + #[test] + fn reachable_with_diagonals() { + let mut path_graph: PathingGrid = PathingGrid::new(3, 2, false); + path_graph.grid.set(1, 0, true); + path_graph.grid.set(1, 1, true); + let p1 = Point::new(0, 0); + let p2 = Point::new(1, 0); + let p3 = Point::new(0, 1); + let p4 = Point::new(2, 0); + path_graph.generate_components(); + assert!(path_graph.unreachable(&p1, &p2)); + assert!(!path_graph.unreachable(&p1, &p3)); + assert!(path_graph.unreachable(&p1, &p4)); + assert!(!path_graph.neighbours_unreachable(&p1, &p2)); + assert!(path_graph.neighbours_unreachable(&p1, &p4)); + } + + /// Asserts that the two corners are connected on a 4-grid. + #[test] + fn reachable_without_diagonals() { + // |S | + // | # | + // | G| + // ___ + let mut pathing_grid: PathingGrid = PathingGrid::new(3, 3, false); + pathing_grid.set(1, 1, true); + pathing_grid.generate_components(); + let start = Point::new(0, 0); + let end = Point::new(2, 2); + assert!(pathing_grid.reachable(&start, &end)); + } + // Tests whether allowing diagonals has the expected effect on diagonal reachability in a minimal setting. + #[test] + fn test_diagonal_switch_reachable() { + // ___ + // | #| + // |# | + // __ + let mut pathing_grid: PathingGrid = PathingGrid::new(2, 2, true); + let mut pathing_grid_diag: PathingGrid = PathingGrid::new(2, 2, true); + + pathing_grid.set(0, 0, false); + pathing_grid.set(1, 1, false); + pathing_grid.generate_components(); + pathing_grid_diag.set(0, 0, false); + pathing_grid_diag.set(1, 1, false); + pathing_grid_diag.generate_components(); + let start = Point::new(0, 0); + let end = Point::new(1, 1); + assert!(pathing_grid.unreachable(&start, &end)); + if ALLOW_CORNER_CUTTING { + assert!(pathing_grid_diag.reachable(&start, &end)); + } else { + assert!(pathing_grid_diag.unreachable(&start, &end)); + } + } +} diff --git a/src/solver/astar.rs b/src/solver/astar.rs new file mode 100644 index 0000000..b926c74 --- /dev/null +++ b/src/solver/astar.rs @@ -0,0 +1,213 @@ +use grid_util::Point; +use smallvec::SmallVec; + +use crate::{pathing_grid::PathingGrid, solver::GridSolver, N_SMALLVEC_SIZE}; + +#[derive(Clone, Debug)] +pub struct AstarSolver { + pub heuristic_factor: f32, +} + +impl AstarSolver { + pub fn new() -> AstarSolver { + AstarSolver { + heuristic_factor: 1.0, + } + } +} + +impl GridSolver for AstarSolver { + type Successors = SmallVec<[(Point, i32); N_SMALLVEC_SIZE]>; + + fn successors( + &self, + grid: &PathingGrid, + _parent: Option<&Point>, + node: &Point, + _goal: &F, + ) -> Self::Successors + where + F: Fn(&Point) -> bool, + { + grid.neighborhood_points_and_cost(node) + } + + /// Just the normal cost times a heuristic factor. + fn heuristic( + &self, + grid: &PathingGrid, + p1: &Point, + p2: &Point, + ) -> i32 { + (self.cost(grid, p1, p2) as f32 * self.heuristic_factor) as i32 + } +} + +#[cfg(test)] +mod tests { + use grid_util::{Rect, ValueGrid}; + + use crate::ALLOW_CORNER_CUTTING; + + use super::*; + + /// Asserts that the case in which start and goal are equal is handled correctly. + #[test] + fn equal_start_goal() { + let mut pathing_grid: PathingGrid = PathingGrid::new(1, 1, false); + pathing_grid.generate_components(); + let solver = AstarSolver::new(); + let start = Point::new(0, 0); + let path = solver + .get_path_single_goal(&mut pathing_grid, start, start) + .unwrap(); + assert!(path.len() == 1); + } + + /// Asserts that the case in which start and goal are equal is handled correctly. + #[test] + fn equal_start_goal_diagonal() { + let mut pathing_grid: PathingGrid = PathingGrid::new(1, 1, false); + pathing_grid.generate_components(); + let solver = AstarSolver::new(); + let start = Point::new(0, 0); + let path = solver + .get_path_single_goal(&mut pathing_grid, start, start) + .unwrap(); + assert!(path.len() == 1); + } + + /// Asserts that the optimal 4 step solution is found. + #[test] + fn solve_simple_problem() { + let expected = 5; + let mut pathing_grid: PathingGrid = PathingGrid::new(3, 3, false); + pathing_grid.set(1, 1, true); + pathing_grid.generate_components(); + let solver = AstarSolver::new(); + + let start = Point::new(0, 0); + let end = Point::new(2, 2); + let path = solver + .get_path_single_goal(&mut pathing_grid, start, end) + .unwrap(); + assert!(path.len() == expected); + } + + /// Asserts that the optimal 4 step solution is found. + #[test] + fn solve_simple_problem_diagonal() { + let expected = if ALLOW_CORNER_CUTTING { 4 } else { 5 }; + let mut pathing_grid: PathingGrid = PathingGrid::new(3, 3, false); + pathing_grid.set(1, 1, true); + pathing_grid.generate_components(); + let solver = AstarSolver::new(); + + let start = Point::new(0, 0); + let end = Point::new(2, 2); + let path = solver + .get_path_single_goal(&mut pathing_grid, start, end) + .unwrap(); + assert!(path.len() == expected); + } + + #[test] + fn test_multiple_goals() { + let expected = 7; + let mut pathing_grid: PathingGrid = PathingGrid::new(5, 5, false); + pathing_grid.set(1, 1, true); + pathing_grid.generate_components(); + let solver = AstarSolver::new(); + let start = Point::new(0, 0); + let goal_1 = Point::new(4, 4); + let goal_2 = Point::new(3, 3); + let goals = vec![&goal_1, &goal_2]; + let (selected_goal, path) = solver + .get_path_multiple_goals(&mut pathing_grid, start, goals) + .unwrap(); + assert_eq!(selected_goal, Point::new(3, 3)); + assert!(path.len() == expected); + } + + #[test] + fn test_multiple_goal_diagonal() { + let expected = if ALLOW_CORNER_CUTTING { 5 } else { 6 }; + let mut pathing_grid: PathingGrid = PathingGrid::new(5, 5, false); + pathing_grid.set(1, 1, true); + pathing_grid.generate_components(); + let solver = AstarSolver::new(); + let start = Point::new(0, 0); + let goal_1 = Point::new(4, 4); + let goal_2 = Point::new(3, 3); + let goals = vec![&goal_1, &goal_2]; + let (selected_goal, path) = solver + .get_path_multiple_goals(&mut pathing_grid, start, goals) + .unwrap(); + assert_eq!(selected_goal, Point::new(3, 3)); + assert!(path.len() == expected); + } + #[test] + fn test_complex() { + let expected = 15; + let mut pathing_grid: PathingGrid = PathingGrid::new(10, 10, false); + pathing_grid.set_rect(Rect::new(1, 1, 1, 1), true); + pathing_grid.set_rect(Rect::new(5, 0, 1, 1), true); + pathing_grid.set_rect(Rect::new(0, 5, 1, 1), true); + pathing_grid.set_rect(Rect::new(8, 8, 1, 1), true); + pathing_grid.generate_components(); + let solver = AstarSolver::new(); + + let start = Point::new(0, 0); + let end = Point::new(7, 7); + let path = solver + .get_path_single_goal(&mut pathing_grid, start, end) + .unwrap(); + assert!(path.len() == expected); + } + #[test] + fn test_complex_diagonal() { + let expected = if ALLOW_CORNER_CUTTING { 10 } else { 11 }; + let mut pathing_grid: PathingGrid = PathingGrid::new(10, 10, false); + pathing_grid.set_rect(Rect::new(1, 1, 1, 1), true); + pathing_grid.set_rect(Rect::new(5, 0, 1, 1), true); + pathing_grid.set_rect(Rect::new(0, 5, 1, 1), true); + pathing_grid.set_rect(Rect::new(8, 8, 1, 1), true); + pathing_grid.generate_components(); + let solver = AstarSolver::new(); + + let start = Point::new(0, 0); + let end = Point::new(7, 7); + let path = solver + .get_path_single_goal(&mut pathing_grid, start, end) + .unwrap(); + assert!(path.len() == expected); + } + // Tests whether allowing diagonals has the expected effect on path existence in a minimal setting. + #[test] + fn test_diagonal_switch_path() { + // ___ + // | #| + // |# | + // __ + let mut pathing_grid: PathingGrid = PathingGrid::new(2, 2, true); + let mut pathing_grid_diag: PathingGrid = PathingGrid::new(2, 2, true); + pathing_grid.set(0, 0, false); + pathing_grid.set(1, 1, false); + pathing_grid.generate_components(); + pathing_grid_diag.set(0, 0, false); + pathing_grid_diag.set(1, 1, false); + pathing_grid_diag.generate_components(); + let solver = AstarSolver::new(); + + let start = Point::new(0, 0); + let goal = Point::new(1, 1); + let path = solver.get_path_single_goal(&mut pathing_grid, start, goal); + let path_diag = solver.get_path_single_goal(&mut pathing_grid_diag, start, goal); + assert!(path.is_none()); + if ALLOW_CORNER_CUTTING { + assert!(path_diag.is_some()); + } else { + assert!(path_diag.is_none()); + } + } +} diff --git a/src/solver/dijkstra.rs b/src/solver/dijkstra.rs new file mode 100644 index 0000000..a45a3e3 --- /dev/null +++ b/src/solver/dijkstra.rs @@ -0,0 +1,34 @@ +use grid_util::Point; +use smallvec::SmallVec; + +use crate::{pathing_grid::PathingGrid, solver::GridSolver, N_SMALLVEC_SIZE}; + +#[derive(Clone, Debug)] +pub struct DijkstraSolver; + +impl GridSolver for DijkstraSolver { + type Successors = SmallVec<[(Point, i32); N_SMALLVEC_SIZE]>; + + fn successors( + &self, + grid: &PathingGrid, + _: Option<&Point>, + node: &Point, + _: &F, + ) -> Self::Successors + where + F: Fn(&Point) -> bool, + { + grid.neighborhood_points_and_cost(node) + } + + /// Just the cost times a heuristic factor. + fn heuristic( + &self, + _: &PathingGrid, + _: &Point, + _: &Point, + ) -> i32 { + 0 + } +} diff --git a/src/solver/jps.rs b/src/solver/jps.rs new file mode 100644 index 0000000..3fb7584 --- /dev/null +++ b/src/solver/jps.rs @@ -0,0 +1,351 @@ +use core::fmt; +use grid_util::{Direction, Point, SimpleValueGrid, ValueGrid}; +use smallvec::SmallVec; + +use crate::{ + pathing_grid::PathingGrid, solver::GridSolver, ALLOW_CORNER_CUTTING, C, D, N_SMALLVEC_SIZE, +}; + +#[derive(Clone, Debug)] +pub struct JPSSolver { + pub jump_point: SimpleValueGrid, + pub neighbours: SimpleValueGrid, + pub improved_pruning: bool, +} + +impl GridSolver for JPSSolver { + type Successors = SmallVec<[(Point, i32); N_SMALLVEC_SIZE]>; + + fn successors( + &self, + grid: &PathingGrid, + parent: Option<&Point>, + node: &Point, + goal: &F, + ) -> SmallVec<[(Point, i32); N_SMALLVEC_SIZE]> + where + F: Fn(&Point) -> bool, + { + match parent { + Some(parent_node) => { + let mut succ = SmallVec::new(); + let dir = parent_node.dir_obj(node); + for (n, c) in self.pruned_neighborhood::(dir, node) { + let dir = node.dir_obj(&n); + // Jumps the neighbor, skipping over unnecessary nodes. + if let Some((jumped_node, cost)) = self.jump(*node, c, dir, goal, grid) { + // If improved pruning is enabled, expand any diagonal unforced nodes + if self.improved_pruning + && dir.diagonal() + && !goal(&jumped_node) + && !self.is_forced(dir, &jumped_node) + { + // Recursively expand the unforced diagonal node + let jump_points = self.successors(grid, parent, &jumped_node, goal); + + // Extend the successors with the neighbours of the unforced node, correcting the + // cost to include the cost from parent_node to jumped_node + succ.extend(jump_points.into_iter().map(|(p, c)| (p, c + cost))); + } else { + succ.push((jumped_node, cost)); + } + } + } + succ + } + None => { + // For the starting node, just generate the full normal neighborhood without any pruning or jumping. + grid.neighborhood_points_and_cost(node) + } + } + } + + /// Uses C as cost for cardinal (straight) moves and D for diagonal moves. + fn heuristic( + &self, + grid: &PathingGrid, + p1: &Point, + p2: &Point, + ) -> i32 { + self.cost(grid, p1, p2) + } +} +impl JPSSolver { + pub fn new( + grid: &PathingGrid, + improved_pruning: bool, + ) -> JPSSolver { + let mut solver = JPSSolver { + jump_point: SimpleValueGrid::new(grid.width(), grid.height(), 0), + neighbours: SimpleValueGrid::new(grid.width(), grid.height(), 0b11111111), + improved_pruning, + }; + solver.initialize(grid); + solver + } + + fn is_forced(&self, dir: Direction, node: &Point) -> bool { + let dir_num = dir.num(); + self.jump_point.get_point(*node) & (1 << dir_num) != 0 + } + /// The neighbour indexing used here corresponds to that used in [grid_util::Direction]. + pub fn indexed_neighbor(&self, node: &Point, index: i32) -> bool { + (self.neighbours.get_point(*node) & 1 << (index.rem_euclid(8))) != 0 + } + fn width(&self) -> usize { + self.neighbours.width() + } + fn height(&self) -> usize { + self.neighbours.height() + } + fn in_bounds(&self, x: i32, y: i32) -> bool { + self.neighbours.index_in_bounds(x, y) + } + /// Updates the neighbours grid after changing the grid. + fn update_neighbours(&mut self, x: i32, y: i32, blocked: bool) { + let p = Point::new(x, y); + for i in 0..8 { + let neighbor = p.moore_neighbor(i); + if self.in_bounds(neighbor.x, neighbor.y) { + let ix = (i + 4) % 8; + let mut n_mask = self.neighbours.get_point(neighbor); + if blocked { + n_mask &= !(1 << ix); + } else { + n_mask |= 1 << ix; + } + self.neighbours.set_point(neighbor, n_mask); + } + } + } + fn forced_mask(&self, node: &Point) -> u8 { + let mut forced_mask: u8 = 0; + for dir_num in 0..8 { + if dir_num % 2 == 1 { + if ALLOW_CORNER_CUTTING + && (!self.indexed_neighbor(node, 3 + dir_num) + || !self.indexed_neighbor(node, 5 + dir_num)) + { + forced_mask |= 1 << dir_num; + } + } else { + if !self.indexed_neighbor(node, 2 + dir_num) + || !self.indexed_neighbor(node, 6 + dir_num) + { + forced_mask |= 1 << dir_num; + } + }; + } + forced_mask + } + + fn pruned_neighborhood<'a, const ALLOW_DIAGONAL: bool>( + &self, + dir: Direction, + node: &'a Point, + ) -> impl Iterator + 'a { + let dir_num = dir.num(); + let mut n_mask: u8; + let mut neighbours = self.neighbours.get_point(*node); + if !ALLOW_DIAGONAL { + neighbours &= 0b01010101; + n_mask = 0b01000101_u8.rotate_left(dir_num as u32); + } else if dir.diagonal() { + n_mask = 0b10000011_u8.rotate_left(dir_num as u32); + if !self.indexed_neighbor(node, 3 + dir_num) { + n_mask |= 1 << ((dir_num + 2) % 8); + } + if !self.indexed_neighbor(node, 5 + dir_num) { + n_mask |= 1 << ((dir_num + 6) % 8); + } + } else { + if ALLOW_CORNER_CUTTING { + n_mask = 0b00000001 << dir_num; + if !self.indexed_neighbor(node, 2 + dir_num) { + n_mask |= 1 << ((dir_num + 1) % 8); + } + if !self.indexed_neighbor(node, 6 + dir_num) { + n_mask |= 1 << ((dir_num + 7) % 8); + } + } else { + // TODO: look into whether this is minimal, this at least makes the algorithm + // optimal and complete following the no corner cutting rule + n_mask = 0b11010111_u8.rotate_left(dir_num as u32); + } + } + let comb_mask = neighbours & n_mask; + (0..8) + .step_by(if ALLOW_DIAGONAL { 1 } else { 2 }) + .filter(move |x| comb_mask & (1 << *x) != 0) + // (dir_num % 2) * (D-C) + C) + // is an optimized version without a conditional of + // if dir.diagonal() {D} else {C} + .map(move |d| (node.moore_neighbor(d), (dir_num % 2) * (D - C) + C)) + } + + /// Straight jump in a cardinal direction. + fn jump_straight( + &self, + mut initial: Point, + mut cost: i32, + direction: Direction, + goal: &F, + grid: &PathingGrid, + ) -> Option<(Point, i32)> + where + F: Fn(&Point) -> bool, + { + debug_assert!(!direction.diagonal()); + loop { + initial = initial + direction; + if !grid.can_move_to_simple(initial) { + return None; + } + + if goal(&initial) || self.is_forced(direction, &initial) { + return Some((initial, cost)); + } + + // Straight jumps always take cardinal cost + cost += C; + } + } + + /// Performs the jumping of node neighbours, skipping over unnecessary nodes until a goal or a forced node is found. + fn jump( + &self, + mut initial: Point, + mut cost: i32, + direction: Direction, + goal: &F, + grid: &PathingGrid, + ) -> Option<(Point, i32)> + where + F: Fn(&Point) -> bool, + { + let mut new_initial: Point; + loop { + new_initial = initial + direction; + if !grid.can_move_to(new_initial, initial) { + return None; + } + initial = new_initial; + + if goal(&initial) || self.is_forced(direction, &initial) { + return Some((initial, cost)); + } + if direction.diagonal() + && (self + .jump_straight(initial, 1, direction.x_dir(), goal, grid) + .is_some() + || self + .jump_straight(initial, 1, direction.y_dir(), goal, grid) + .is_some()) + { + return Some((initial, cost)); + } + + // When using a 4-neighborhood (specified by setting allow_diagonal_move to false), + // jumps perpendicular to the direction are performed. This is necessary to not miss the + // goal when passing by. + if !ALLOW_DIAGONAL || !ALLOW_CORNER_CUTTING && !direction.diagonal() { + if ALLOW_DIAGONAL { + let diag_1 = direction.rotate_ccw(1); + let diag_2 = direction.rotate_cw(1); + if self.jump(initial, 1, diag_1, goal, grid).is_some() + || self.jump(initial, 1, diag_2, goal, grid).is_some() + { + return Some((initial, cost)); + } + } + let perp_1 = direction.rotate_ccw(2); + let perp_2 = direction.rotate_cw(2); + if self.jump_straight(initial, 1, perp_1, goal, grid).is_some() + || self.jump_straight(initial, 1, perp_2, goal, grid).is_some() + { + return Some((initial, cost)); + } + } + + // See comment in pruned_neighborhood about cost calculation + cost += (direction.num() % 2) * (D - C) + C; + } + } + + pub fn set_jumppoints(&mut self, point: Point) { + let value = self.forced_mask(&point); + self.jump_point.set_point(point, value); + } + pub fn fix_jumppoints( + &mut self, + point: Point, + grid: &PathingGrid, + ) { + self.set_jumppoints(point); + for p in grid.neighborhood_points(&point) { + if grid.point_in_bounds(p) { + self.set_jumppoints(p); + } + } + } + + /// Performs the full jump point precomputation + pub fn set_all_jumppoints(&mut self) { + for x in 0..self.width() { + for y in 0..self.height() { + self.set_jumppoints(Point::new(x as i32, y as i32)); + } + } + } + + /// Updates the neighbours and jumppoints + fn set( + &mut self, + x: i32, + y: i32, + blocked: bool, + grid: &PathingGrid, + ) { + let p = Point::new(x, y); + self.update_neighbours(p.x, p.y, blocked); + self.fix_jumppoints(p, grid); + } + + pub fn update_all_neighbours( + &mut self, + grid: &PathingGrid, + ) { + for x in 0..self.width() as i32 { + for y in 0..self.height() as i32 { + self.update_neighbours(x, y, grid.get(x, y)); + } + } + } + pub fn initialize(&mut self, grid: &PathingGrid) { + // Emulates 'placing' of blocked tile around map border to correctly initialize neighbours + // and make behaviour of a map bordered by tiles the same as a borderless map. + for i in -1..=(self.width() as i32) { + self.update_neighbours(i, -1, true); + self.update_neighbours(i, self.height() as i32, true); + } + for j in -1..=(self.height() as i32) { + self.update_neighbours(-1, j, true); + self.update_neighbours(self.width() as i32, j, true); + } + self.update_all_neighbours(grid); + self.set_all_jumppoints(); + } +} + +impl fmt::Display for JPSSolver { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + writeln!(f, "Neighbours:")?; + for y in 0..self.neighbours.height as i32 { + let values = (0..self.neighbours.width as i32) + .map(|x| self.neighbours.get(x, y) as i32) + .collect::>(); + writeln!(f, "{:?}", values)?; + } + Ok(()) + } +} diff --git a/src/solver/mod.rs b/src/solver/mod.rs new file mode 100644 index 0000000..b042d87 --- /dev/null +++ b/src/solver/mod.rs @@ -0,0 +1,176 @@ +use crate::{pathing_grid::PathingGrid, waypoints_to_path, C, D, E, EQUAL_EDGE_COST}; +use grid_util::Point; + +pub mod astar; +pub mod dijkstra; +pub mod jps; + +/// Converts the integer cost to an approximate floating point equivalent where cardinal directions have cost 1.0. +pub fn convert_cost_to_unit_cost_float(cost: i32) -> f64 { + (cost as f64) / (C as f64) +} + +pub trait GridSolver { + type Successors: IntoIterator; + + fn heuristic( + &self, + grid: &PathingGrid, + p1: &Point, + p2: &Point, + ) -> i32; + + /// Uses C as cost for cardinal (straight) moves and D for diagonal moves. + fn cost( + &self, + _grid: &PathingGrid, + p1: &Point, + p2: &Point, + ) -> i32 { + if ALLOW_DIAGONAL { + let delta_x = (p1.x - p2.x).abs(); + let delta_y = (p1.y - p2.y).abs(); + // Formula from https://github.com/riscy/a_star_on_grids + // to efficiently compute the cost of a path taking the maximal amount + // of diagonal steps before going straight + (E * (delta_x - delta_y).abs() + D * (delta_x + delta_y)) / 2 + } else { + p1.manhattan_distance(p2) * C + } + } + + fn successors( + &self, + grid: &PathingGrid, + parent: Option<&Point>, + node: &Point, + goal: &F, + ) -> Self::Successors + where + F: Fn(&Point) -> bool; + + fn get_path_cost( + &self, + path: &Vec, + pathing_grid: &PathingGrid, + ) -> i32 { + let mut v = path[0]; + let n = path.len(); + let mut total_cost_int = 0; + for i in 1..n { + let v_old = v; + v = path[i]; + let cost = self.cost(&pathing_grid, &v_old, &v); + total_cost_int += cost; + } + total_cost_int + } + fn get_path_cost_float( + &self, + path: &Vec, + pathing_grid: &PathingGrid, + ) -> f64 { + convert_cost_to_unit_cost_float(self.get_path_cost(path, pathing_grid)) + } + fn get_path_single_goal( + &self, + grid: &mut PathingGrid, + start: Point, + goal: Point, + ) -> Option> { + self.get_waypoints_single_goal(grid, start, goal) + .map(waypoints_to_path) + } + fn get_path_single_goal_approximate( + &self, + grid: &mut PathingGrid, + start: Point, + goal: Point, + ) -> Option> { + self.get_waypoints_single_goal_approximate(grid, start, goal) + .map(waypoints_to_path) + } + /// The raw waypoints (jump points) from which [get_path_single_goal](Self::get_path_single_goal) makes a path. + fn get_waypoints_single_goal( + &self, + grid: &mut PathingGrid, + start: Point, + goal: Point, + ) -> Option> { + // Check if start and goal are on the same connected component. + if grid.unreachable(&start, &goal) { + return None; + } + // The goal is reachable from the start, compute a path + let mut ct = grid.context.lock().unwrap(); + ct.astar_jps( + &start, + |parent, node| self.successors(grid, *parent, node, &|node_pos| *node_pos == goal), + |point| self.heuristic(grid, point, &goal), + |point| *point == goal, + ) + .map(|(v, _c)| v) + } + + /// The raw waypoints (jump points) from which [get_path_single_goal](Self::get_path_single_goal) makes a path. + fn get_waypoints_single_goal_approximate( + &self, + grid: &mut PathingGrid, + start: Point, + goal: Point, + ) -> Option> { + // Check if start and one of the goal neighbours are on the same connected component. + if grid.neighbours_unreachable(&start, &goal) { + // No neigbhours of the goal are reachable from the start + return None; + } + // A neighbour of the goal can be reached, compute a path + let mut ct = grid.context.lock().unwrap(); + ct.astar_jps( + &start, + |parent, node| { + self.successors(grid, *parent, node, &|node_pos| { + self.heuristic(grid, node_pos, &goal) <= if EQUAL_EDGE_COST { 1 } else { 99 } + }) + }, + |point| self.heuristic(grid, point, &goal), + |point| self.heuristic(grid, point, &goal) <= if EQUAL_EDGE_COST { 1 } else { 99 }, + ) + .map(|(v, _c)| v) + } + /// Computes a path from the start to one of the given goals and returns the selected goal in addition to the found path. Otherwise behaves similar to [get_path_single_goal](Self::get_path_single_goal). + fn get_path_multiple_goals( + &self, + grid: &mut PathingGrid, + start: Point, + goals: Vec<&Point>, + ) -> Option<(Point, Vec)> { + self.get_waypoints_multiple_goals(grid, start, goals) + .map(|(x, y)| (x, waypoints_to_path(y))) + } + /// The raw waypoints (jump points) from which [get_path_multiple_goals](Self::get_path_multiple_goals) makes a path. + fn get_waypoints_multiple_goals( + &self, + grid: &mut PathingGrid, + start: Point, + goals: Vec<&Point>, + ) -> Option<(Point, Vec)> { + if goals.is_empty() { + return None; + } + let mut ct = grid.context.lock().unwrap(); + let result = ct.astar_jps( + &start, + |parent, node| self.successors(grid, *parent, node, &|point| goals.contains(&point)), + |point| { + goals + .iter() + .map(|x| self.heuristic(grid, point, x)) + .min() + .unwrap() + }, + |point| goals.contains(&point), + ); + result.map(|(v, _c)| (*v.last().unwrap(), v)) + } +} diff --git a/tests/benchmark_distances.rs b/tests/benchmark_distances.rs new file mode 100644 index 0000000..20a737a --- /dev/null +++ b/tests/benchmark_distances.rs @@ -0,0 +1,71 @@ +use grid_pathfinding::pathing_grid::PathingGrid; +use grid_pathfinding::solver::{astar::AstarSolver, jps::JPSSolver, GridSolver}; +use grid_pathfinding_benchmark::get_benchmark; +use grid_util::*; +use std::fs::File; +use std::io::{BufWriter, Write}; + +fn save_path(path: Vec, filename: &str) -> std::io::Result<()> { + let f = File::create(filename)?; + let mut w = BufWriter::new(f); + for p in path { + writeln!(w, "{},{}", p.x, p.y)?; + } + Ok(()) +} + +#[test] +fn verify_solution_distance_jps() { + let bench_set = ["dao/arena", "dao/lak107d", "dao/den101d"]; + for name in bench_set { + let (bool_grid, scenarios) = get_benchmark(name.to_owned()); + let mut pathing_grid: PathingGrid = + PathingGrid::new(bool_grid.width, bool_grid.height, true); + + pathing_grid.grid = bool_grid.clone(); + pathing_grid.generate_components(); + let mut solver = JPSSolver::new(&pathing_grid, false); + solver.initialize(&pathing_grid); + + for (start, end, distance) in &scenarios { + println!("Start: {start}; End: {end}; Distance: {distance}"); + let path = solver + .get_path_single_goal(&mut pathing_grid, *start, *end) + .unwrap(); + save_path(path.clone(), "path.csv").unwrap(); + let float_cost = solver.get_path_cost_float(&path, &pathing_grid); + println!("My distance: {float_cost}"); + if *distance >= 0.01 { + let delta_dist = (float_cost - distance).abs() / distance; + assert!(delta_dist < 0.05); + } + } + } +} + +#[test] +fn verify_solution_distance_astar() { + let bench_set = ["dao/arena", "dao/lak107d", "dao/den101d"]; + let solver = AstarSolver::new(); + + for name in bench_set { + let (bool_grid, scenarios) = get_benchmark(name.to_owned()); + let mut pathing_grid: PathingGrid = + PathingGrid::new(bool_grid.width, bool_grid.height, true); + pathing_grid.grid = bool_grid.clone(); + pathing_grid.generate_components(); + for (start, end, distance) in &scenarios { + println!("Start: {start}; End: {end}; Distance: {distance}"); + let path = solver + .get_path_single_goal(&mut pathing_grid, *start, *end) + .unwrap(); + save_path(path.clone(), "path.csv").unwrap(); + let float_cost = solver.get_path_cost_float(&path, &pathing_grid); + println!("My distance: {float_cost}"); + if *distance >= 0.01 { + let delta_dist = (float_cost - distance).abs() / distance; + assert!(delta_dist < 0.05); + } + } + } +} diff --git a/tests/fuzz_test.rs b/tests/fuzz_test.rs index af997db..e657805 100644 --- a/tests/fuzz_test.rs +++ b/tests/fuzz_test.rs @@ -1,23 +1,34 @@ /// Fuzzes pathfinding system by checking for many random grids that a path is always found if the goal is reachable /// by being part of the same connected component. All system settings (diagonals, improved pruning) are tested. -use grid_pathfinding::*; +use grid_pathfinding::{ + pathing_grid::PathingGrid, + solver::{astar::AstarSolver, jps::JPSSolver, GridSolver}, + ALLOW_CORNER_CUTTING, +}; use grid_util::*; use rand::prelude::*; +use smallvec::{smallvec, SmallVec}; -fn random_grid(n: usize, rng: &mut StdRng, diagonal: bool, improved_pruning: bool) -> PathingGrid { - let mut pathing_grid: PathingGrid = PathingGrid::new(n, n, false); - pathing_grid.allow_diagonal_move = diagonal; - pathing_grid.improved_pruning = improved_pruning; +fn random_grid( + w: usize, + h: usize, + rng: &mut StdRng, +) -> PathingGrid { + let mut pathing_grid: PathingGrid = PathingGrid::new(w, h, false); for x in 0..pathing_grid.width() as i32 { for y in 0..pathing_grid.height() as i32 { - pathing_grid.set(x, y, rng.gen_bool(0.4)) + pathing_grid.set(x, y, rng.random_bool(0.4)) } } pathing_grid.generate_components(); pathing_grid } -fn visualize_grid(grid: &PathingGrid, start: &Point, end: &Point) { +fn visualize_grid( + grid: &PathingGrid, + start: &Point, + end: &Point, +) { let grid = &grid.grid; for y in (0..grid.height as i32).rev() { for x in 0..grid.width as i32 { @@ -36,24 +47,30 @@ fn visualize_grid(grid: &PathingGrid, start: &Point, end: &Point) { } } -#[test] -fn fuzz() { +fn reachable_fuzzer() { const N: usize = 10; const N_GRIDS: usize = 10000; let mut rng = StdRng::seed_from_u64(0); - for (diagonal, improved_pruning) in [(false, false), (true, false), (true, true)] { - let mut random_grids: Vec = Vec::new(); + let arr: SmallVec<[bool; 2]> = if ALLOW_DIAGONAL { + smallvec![false, true] + } else { + smallvec![false] + }; + for improved_pruning in arr { + let mut random_grids: Vec> = Vec::new(); for _ in 0..N_GRIDS { - random_grids.push(random_grid(N, &mut rng, diagonal, improved_pruning)) + random_grids.push(random_grid(N, N, &mut rng)) } let start = Point::new(0, 0); let end = Point::new(N as i32 - 1, N as i32 - 1); for mut random_grid in random_grids { + let mut solver = JPSSolver::new(&random_grid, improved_pruning); random_grid.set_point(start, false); random_grid.set_point(end, false); + solver.initialize(&random_grid); let reachable = random_grid.reachable(&start, &end); - let path = random_grid.get_path_single_goal(start, end, false); + let path = solver.get_path_single_goal(&mut random_grid, start, end); // Show the grid if a path is not found if path.is_some() != reachable { visualize_grid(&random_grid, &start, &end); @@ -62,3 +79,101 @@ fn fuzz() { } } } + +fn distance_fuzzer() { + const N: usize = 10; + const N_GRIDS: usize = 10000; + let tolerance = 0.001; + + let mut rng = StdRng::seed_from_u64(0); + let astar_solver = AstarSolver::new(); + let arr: SmallVec<[bool; 2]> = if ALLOW_DIAGONAL { + smallvec![false, true] + } else { + smallvec![false] + }; + for improved_pruning in arr { + let mut random_grids: Vec> = Vec::new(); + for _ in 0..N_GRIDS { + random_grids.push(random_grid(N, N, &mut rng)) + } + + let start = Point::new(0, 0); + let end = Point::new(N as i32 - 1, N as i32 - 1); + for mut random_grid in random_grids { + let mut jps_solver = JPSSolver::new(&random_grid, improved_pruning); + random_grid.set_point(start, false); + random_grid.set_point(end, false); + jps_solver.initialize(&random_grid); + let reachable = random_grid.reachable(&start, &end); + if reachable { + let jps_path = jps_solver + .get_path_single_goal(&mut random_grid, start, end) + .unwrap(); + let astar_path = astar_solver + .get_path_single_goal(&mut random_grid, start, end) + .unwrap(); + + let astar_cost = astar_solver.get_path_cost_float(&astar_path, &random_grid); + let jps_cost = jps_solver.get_path_cost_float(&jps_path, &random_grid); + if astar_cost >= tolerance { + let delta_dist = (jps_cost - astar_cost).abs() / astar_cost; + if delta_dist >= tolerance { + println!("Astar distance: {astar_cost:4}; JPS distance: {jps_cost:4}"); + println!("diagonal: {ALLOW_DIAGONAL}; improved_pruning: {improved_pruning}; corner_cutting: {ALLOW_CORNER_CUTTING}"); + + let mut problem_start: Point = start; + for (idx, &p) in jps_path.iter().enumerate().rev() { + let jps_suffix = &jps_path[idx..].to_vec(); + let jps_suffix_cost = + jps_solver.get_path_cost_float(jps_suffix, &random_grid); + let astar_suffix_path = astar_solver + .get_path_single_goal(&mut random_grid, p, end) + .expect("A* should find a path from intermediate JPS node"); + let astar_suffix_cost = + astar_solver.get_path_cost_float(&astar_suffix_path, &random_grid); + + let rel_diff = (jps_suffix_cost - astar_suffix_cost).abs() + / astar_suffix_cost.max(1e-6); + + // First point where JPS suffix is no longer optimal + if rel_diff >= tolerance { + println!( + "=> First suboptimal JPS step at index {idx}, point {p:?}" + ); + println!("- JPS suffix from here: {jps_suffix:?}"); + println!("- A* optimal suffix: {astar_suffix_path:?}"); + problem_start = p; + break; + } + } + + visualize_grid(&random_grid, &problem_start, &end); + } + + assert!(delta_dist < tolerance); + } + } + } + } +} + +#[test] +fn fuzz_reachable() { + reachable_fuzzer::() +} + +#[test] +fn fuzz_reachable_diagonal() { + reachable_fuzzer::() +} + +#[test] +fn fuzz_distance() { + distance_fuzzer::() +} + +#[test] +fn fuzz_distance_diagonal() { + distance_fuzzer::() +}