use crate::{
graph::{self, Node, NodeList},
neighbors::Neighborhood,
path::{AbstractPath, Cost, Path, PathSegment},
*,
};
use std::marker::PhantomData;
// a Macro to log::trace the time since $timer, and restart $timer
#[cfg(feature = "log")]
macro_rules! re_trace {
($msg: literal, $timer: ident) => {
let now = std::time::Instant::now();
log::trace!(concat!("time to ", $msg, ": {:?}"), now - $timer);
#[allow(unused)]
let $timer = now;
};
}
#[cfg(not(feature = "log"))]
macro_rules! re_trace {
// does nothing without log feature
($msg: literal, $timer: ident) => {};
}
mod cache_config;
pub use cache_config::PathCacheConfig;
mod chunk;
use chunk::Chunk;
enum CostFnWrapper<F1, F2>
where
F1: Sync + Fn(Point) -> isize,
F2: FnMut(Point) -> isize,
{
Sequential(F2, PhantomData<F1>), // F1 has to appear in the enum even if `parallel` is disabled
#[cfg(feature = "parallel")]
Parallel(F1),
}
/// A struct to store the Hierarchical Pathfinding information.
#[derive(Clone, Debug)]
pub struct PathCache<N: Neighborhood> {
width: usize,
height: usize,
chunks: Vec<Chunk>,
num_chunks: (usize, usize),
nodes: NodeList,
neighborhood: N,
config: PathCacheConfig,
}
impl<N: Neighborhood + Sync> PathCache<N> {
/// Creates a new PathCache
///
/// ## Arguments
/// - `(width, height)` - the size of the Grid
/// - `get_cost` - get the cost for walking over a Tile. (Cost < 0 means solid Tile)
/// - `neighborhood` - the Neighborhood to use. (See [`Neighborhood`])
/// - `config` - optional config for creating the cache. (See [`PathCacheConfig`])
///
/// `get_cost((x, y))` should return the cost for walking over the Tile at (x, y).
/// Costs below 0 are solid Tiles.
///
/// ## Examples
/// Basic usage:
/// ```
/// use hierarchical_pathfinding::prelude::*;
///
/// // create and initialize Grid
/// // 0 = empty, 1 = swamp, 2 = wall
/// let mut grid = [
/// [0, 2, 0, 0, 0],
/// [0, 2, 2, 2, 0],
/// [0, 1, 0, 0, 0],
/// [0, 1, 0, 2, 0],
/// [0, 0, 0, 2, 0],
/// ];
/// let (width, height) = (grid.len(), grid[0].len());
/// type Grid = [[usize; 5]; 5];
///
/// const COST_MAP: [isize; 3] = [1, 10, -1];
///
/// fn cost_fn(grid: &Grid) -> impl '_ + Sync + Fn((usize, usize)) -> isize {
/// move |(x, y)| COST_MAP[grid[y][x]]
/// }
///
/// let mut pathfinding = PathCache::new(
/// (width, height), // the size of the Grid
/// cost_fn(&grid), // get the cost for walking over a Tile
/// ManhattanNeighborhood::new(width, height), // the Neighborhood
/// PathCacheConfig::with_chunk_size(3), // config
/// );
/// ```
pub fn new<F: Sync + Fn(Point) -> isize>(
(width, height): (usize, usize),
get_cost: F,
neighborhood: N,
config: PathCacheConfig,
) -> PathCache<N> {
#[cfg(feature = "parallel")]
{
PathCache::new_internal::<F, fn(Point) -> isize>(
(width, height),
CostFnWrapper::Parallel(get_cost),
neighborhood,
config,
)
}
#[cfg(not(feature = "parallel"))]
{
PathCache::new_internal::<fn(Point) -> isize, F>(
(width, height),
CostFnWrapper::Sequential(get_cost, PhantomData),
neighborhood,
config,
)
}
}
/// Same as [`new`](PathCache::new), but doesn't use threads to allow [`FnMut`].
///
/// Equivalent to `new` if `parallel` feature is disabled.
///
/// Note that this is _**way**_ slower than `new` with `parallel`.
pub fn new_with_fn_mut<F: FnMut(Point) -> isize>(
(width, height): (usize, usize),
get_cost: F,
neighborhood: N,
config: PathCacheConfig,
) -> PathCache<N> {
PathCache::new_internal::<fn(Point) -> isize, F>(
(width, height),
CostFnWrapper::Sequential(get_cost, Default::default()),
neighborhood,
config,
)
}
/// Same as [`new`](PathCache::new), ~~but uses multiple threads.~~
///
/// Note that `get_cost` has to be `Fn` instead of `FnMut`.
#[cfg(feature = "parallel")]
#[deprecated(since = "0.5.0", note = "`new` is automatically parallel")]
pub fn new_parallel<F: Sync + Fn(Point) -> isize>(
(width, height): (usize, usize),
get_cost: F,
neighborhood: N,
config: PathCacheConfig,
) -> PathCache<N> {
PathCache::new_internal::<F, fn(Point) -> isize>(
(width, height),
CostFnWrapper::Parallel(get_cost),
neighborhood,
config,
)
}
fn new_internal<F1, F2>(
(width, height): (usize, usize),
get_cost: CostFnWrapper<F1, F2>,
neighborhood: N,
config: PathCacheConfig,
) -> PathCache<N>
where
F1: Sync + Fn(Point) -> isize,
F2: FnMut(Point) -> isize,
{
#[cfg(feature = "log")]
let (outer_timer, timer) = (std::time::Instant::now(), std::time::Instant::now());
// calculate chunk size
let (num_chunks_w, last_width) = {
let w = width / config.chunk_size;
let remain = width - w * config.chunk_size;
if remain > 0 {
(w + 1, remain)
} else {
(w, config.chunk_size)
}
};
let (num_chunks_h, last_height) = {
let h = height / config.chunk_size;
let remain = height - h * config.chunk_size;
if remain > 0 {
(h + 1, remain)
} else {
(h, config.chunk_size)
}
};
let mut nodes = NodeList::new();
// create chunks
let chunks = match get_cost {
CostFnWrapper::Sequential(mut get_cost, _) => {
let mut chunks: Vec<Chunk> = Vec::with_capacity(num_chunks_w * num_chunks_h);
for y in 0..num_chunks_h {
let h = if y == num_chunks_h - 1 {
last_height
} else {
config.chunk_size
};
for x in 0..num_chunks_w {
let w = if x == num_chunks_w - 1 {
last_width
} else {
config.chunk_size
};
chunks.push(Chunk::new(
(x * config.chunk_size, y * config.chunk_size),
(w, h),
(width, height),
&mut get_cost,
&neighborhood,
&mut nodes,
config,
));
}
}
re_trace!("create chunks", timer);
chunks
}
#[cfg(feature = "parallel")]
CostFnWrapper::Parallel(get_cost) => {
use rayon::prelude::*;
let (mut chunks, node_lists): (Vec<_>, Vec<_>) = (0..num_chunks_h * num_chunks_w)
.into_par_iter()
.map(|index| {
let x = index % num_chunks_w;
let y = index / num_chunks_w;
let w = if x == num_chunks_w - 1 {
last_width
} else {
config.chunk_size
};
let h = if y == num_chunks_h - 1 {
last_height
} else {
config.chunk_size
};
let mut node_list = NodeList::new();
let chunk = Chunk::new(
(x * config.chunk_size, y * config.chunk_size),
(w, h),
(width, height),
&get_cost,
&neighborhood,
&mut node_list,
config,
);
(chunk, node_list)
})
.collect();
re_trace!("create raw chunks", timer);
chunks
.iter_mut()
.zip(node_lists)
.map(|(mut chunk, new_nodes)| {
chunk.nodes = nodes.absorb(new_nodes);
chunk
})
.to_vec();
re_trace!("absorb nodes", timer);
chunks
}
};
let mut cache = PathCache {
width,
height,
chunks,
num_chunks: (num_chunks_w, num_chunks_h),
nodes,
neighborhood,
config,
};
// connect neighboring Nodes across Chunk borders
cache.connect_nodes(None);
re_trace!("connect nodes", timer);
re_trace!("total time", outer_timer);
cache
}
/// Calculates the Path from `start` to `goal` on the Grid.
///
/// If no Path could be found, `None` is returned.
///
/// `get_cost((x, y))` should return the cost for walking over the Tile at (x, y).
/// Costs below 0 are solid Tiles.
///
/// ## Examples
/// Basic usage:
/// ```
/// # use hierarchical_pathfinding::prelude::*;
/// # let mut grid = [
/// # [0, 2, 0, 0, 0],
/// # [0, 2, 2, 2, 2],
/// # [0, 1, 0, 0, 0],
/// # [0, 1, 0, 2, 0],
/// # [0, 0, 0, 2, 0],
/// # ];
/// # let (width, height) = (grid.len(), grid[0].len());
/// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize {
/// # move |(x, y)| [1, 10, -1][grid[y][x]]
/// # }
/// let pathfinding: PathCache<_> = // ...
/// # PathCache::new(
/// # (width, height),
/// # cost_fn(&grid),
/// # ManhattanNeighborhood::new(width, height),
/// # PathCacheConfig::with_chunk_size(3),
/// # );
///
/// let start = (0, 0);
/// let goal = (4, 4);
///
/// // find_path returns Some(Path) on success
/// let path = pathfinding.find_path(
/// start,
/// goal,
/// cost_fn(&grid),
/// );
///
/// assert!(path.is_some());
/// let path = path.unwrap();
///
/// assert_eq!(path.cost(), 12);
/// ```
///
/// The return Value gives the total Cost of the Path using `cost()` and allows to iterate over
/// the Points in the Path.
///
/// **Note**: Setting [`config.cache_paths`](PathCacheConfig::cache_paths) to `false` means
/// that the Paths need to be recalculated as needed. This means that for any sections of the
/// Path that are not present, [`safe_next`](AbstractPath::safe_next) needs to be called to supply the Cost function.
/// Calling `next` in that scenario would lead to a Panic.
///
/// Using the Path:
/// ```
/// # use hierarchical_pathfinding::prelude::*;
/// # let mut grid = [
/// # [0, 2, 0, 0, 0],
/// # [0, 2, 2, 2, 2],
/// # [0, 1, 0, 0, 0],
/// # [0, 1, 0, 2, 0],
/// # [0, 0, 0, 2, 0],
/// # ];
/// # let (width, height) = (grid.len(), grid[0].len());
/// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize {
/// # move |(x, y)| [1, 10, -1][grid[y][x]]
/// # }
/// # let pathfinding = PathCache::new(
/// # (width, height),
/// # cost_fn(&grid),
/// # ManhattanNeighborhood::new(width, height),
/// # PathCacheConfig::with_chunk_size(3),
/// # );
/// # struct Player{ pos: (usize, usize) }
/// # impl Player {
/// # pub fn move_to(&mut self, pos: (usize, usize)) {
/// # self.pos = pos;
/// # }
/// # }
/// #
/// let mut player = Player {
/// pos: (0, 0),
/// //...
/// };
/// let goal = (4, 4);
///
/// let mut path = pathfinding.find_path(
/// player.pos,
/// goal,
/// cost_fn(&grid),
/// ).unwrap();
///
/// player.move_to(path.next().unwrap());
/// assert_eq!(player.pos, (0, 1));
///
/// // wait for next turn or whatever
///
/// player.move_to(path.next().unwrap());
/// assert_eq!(player.pos, (0, 2));
/// ```
/// If the Grid changes, any Path objects still in use may become invalid. You can still
/// use them if you are certain that nothing in relation to that Path changed, but it is
/// discouraged and can lead to undefined behavior or panics.
///
/// Obtaining the entire Path:
/// ```
/// # use hierarchical_pathfinding::prelude::*;
/// # let mut grid = [
/// # [0, 2, 0, 0, 0],
/// # [0, 2, 2, 2, 2],
/// # [0, 1, 0, 0, 0],
/// # [0, 1, 0, 2, 0],
/// # [0, 0, 0, 2, 0],
/// # ];
/// # let (width, height) = (grid.len(), grid[0].len());
/// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize {
/// # move |(x, y)| [1, 10, -1][grid[y][x]]
/// # }
/// # let pathfinding = PathCache::new(
/// # (width, height),
/// # cost_fn(&grid),
/// # ManhattanNeighborhood::new(width, height),
/// # PathCacheConfig::with_chunk_size(3),
/// # );
/// # let start = (0, 0);
/// # let goal = (4, 4);
/// # let path = pathfinding.find_path(
/// # start,
/// # goal,
/// # cost_fn(&grid),
/// # );
/// // ...
/// let path = path.unwrap();
///
/// let points: Vec<(usize, usize)> = path.collect();
/// assert_eq!(
/// points,
/// 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)],
/// );
/// ```
pub fn find_path(
&self,
start: Point,
goal: Point,
mut get_cost: impl FnMut(Point) -> isize,
) -> Option<AbstractPath<N>> {
#[cfg(feature = "log")]
let (outer_timer, timer) = (std::time::Instant::now(), std::time::Instant::now());
if get_cost(start) < 0 {
// cannot start on a wall
return None;
}
let neighborhood = self.neighborhood.clone();
if start == goal {
return Some(AbstractPath::from_known_path(
neighborhood,
Path::from_slice(&[start, start], 0),
));
}
let (start_id, start_path) =
if let Some(s) = self.find_nearest_node(start, &mut get_cost, false) {
s
} else {
// no path from start to any Node => start is in cave within chunk
// => hope that goal is in the same cave
return self
.get_chunk(start)
.find_path(start, goal, get_cost, &neighborhood)
.map(|path| AbstractPath::from_known_path(neighborhood, path));
};
// try-operator: see above, but we know that start is not in a cave
let (goal_id, goal_path) = self.find_nearest_node(goal, &mut get_cost, true)?;
re_trace!("find nodes", timer);
// size hint for number of visited nodes in graph::a_star_search:
// percentage of total area visited (heuristic / max_heuristic)
// as the percentage of nodes visited ( * self.nodes.len())
let heuristic = neighborhood.heuristic(start, goal);
let max_heuristic = neighborhood.heuristic((0, 0), (self.width - 1, self.height - 1));
let max_size = self.nodes.len();
let size_hint = heuristic as f32 / max_heuristic as f32 * max_size as f32;
let path = graph::a_star_search(
&self.nodes,
start_id,
goal_id,
&neighborhood,
size_hint as usize,
)?;
re_trace!("graph::a_star_search", timer);
if path.len() == 2 || (self.config.a_star_fallback && path.len() <= 4) {
// 2: start_id == goal_id
// <= 4: start_id X X goal_id
let res = self
.grid_a_star(start, goal, get_cost)
.map(|path| AbstractPath::from_known_path(neighborhood, path));
re_trace!("A* fallback", timer);
re_trace!("total time", outer_timer);
return res;
}
let mut paths = NodeIDMap::default();
paths.insert(goal_id, path);
#[allow(clippy::let_and_return)]
let res = self
.resolve_paths(
start,
start_path,
&[(goal, goal_id, goal_path)],
&paths,
get_cost,
)
.into_iter()
.next()
.map(|(_, path)| path);
re_trace!("resolve_paths", timer);
re_trace!("total time", outer_timer);
res
}
/// Calculates the Paths from one `start` to several `goals` on the Grid.
///
/// This is equivalent to [`find_path`](PathCache::find_path), except that it is optimized to handle multiple Goals
/// at once. However, it is slower for very few goals, since it does not use a heuristic like
/// [`find_path`](PathCache::find_path) does.
///
/// Instead of returning a single Option, it returns a Hashmap, where the position of the Goal
/// is the key, and the Value is a Tuple of the Path and the Cost of that Path.
///
/// `get_cost((x, y))` should return the cost for walking over the Tile at (x, y).
/// Costs below 0 are solid Tiles.
///
/// See [`find_path`](PathCache::find_path) for more details on how to use the returned Paths.
///
/// ## Examples
/// Basic usage:
/// ```
/// # use hierarchical_pathfinding::prelude::*;
/// # let mut grid = [
/// # [0, 2, 0, 0, 0],
/// # [0, 2, 2, 2, 2],
/// # [0, 1, 0, 0, 0],
/// # [0, 1, 0, 2, 0],
/// # [0, 0, 0, 2, 0],
/// # ];
/// # let (width, height) = (grid.len(), grid[0].len());
/// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize {
/// # move |(x, y)| [1, 10, -1][grid[y][x]]
/// # }
/// let pathfinding: PathCache<_> = // ...
/// # PathCache::new(
/// # (width, height),
/// # cost_fn(&grid),
/// # ManhattanNeighborhood::new(width, height),
/// # PathCacheConfig::with_chunk_size(3),
/// # );
///
/// let start = (0, 0);
/// let goals = [(4, 4), (2, 0)];
///
/// // find_paths returns a HashMap<goal, Path> for all successes
/// let paths = pathfinding.find_paths(
/// start,
/// &goals,
/// cost_fn(&grid),
/// );
///
/// // (4, 4) is reachable
/// assert!(paths.contains_key(&goals[0]));
///
/// // (2, 0) is not reachable
/// assert!(!paths.contains_key(&goals[1]));
/// ```
///
/// The returned Path is always equivalent to the one returned by [`find_path`](PathCache::find_path):
/// ```
/// # use hierarchical_pathfinding::prelude::*;
/// # let mut grid = [
/// # [0, 2, 0, 0, 0],
/// # [0, 2, 2, 2, 2],
/// # [0, 1, 0, 0, 0],
/// # [0, 1, 0, 2, 0],
/// # [0, 0, 0, 2, 0],
/// # ];
/// # let (width, height) = (grid.len(), grid[0].len());
/// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize {
/// # move |(x, y)| [1, 10, -1][grid[y][x]]
/// # }
/// # let pathfinding = PathCache::new(
/// # (width, height),
/// # cost_fn(&grid),
/// # ManhattanNeighborhood::new(width, height),
/// # PathCacheConfig::with_chunk_size(3),
/// # );
/// let start = (0, 0);
/// let goal = (4, 4);
///
/// let paths = pathfinding.find_paths(
/// start,
/// &[goal],
/// cost_fn(&grid),
/// );
/// let dijkstra_path: Vec<_> = paths[&goal].clone().collect();
///
/// let a_star_path: Vec<_> = pathfinding.find_path(
/// start,
/// goal,
/// cost_fn(&grid),
/// ).unwrap().collect();
///
/// assert_eq!(dijkstra_path, a_star_path);
/// ```
pub fn find_paths(
&self,
start: Point,
goals: &[Point],
get_cost: impl FnMut(Point) -> isize,
) -> PointMap<AbstractPath<N>> {
self.find_paths_internal(start, goals, get_cost, false)
}
/// Finds the closest from a list of goals.
///
/// Returns a tuple of the goal and the Path to that goal, or `None` if none of the goals are
/// reachable.
///
/// Similar to [`find_paths`](PathCache::find_paths) in performance and search strategy, but
/// stops after the first goal is found.
///
/// ## Examples
/// Basic usage:
/// ```
/// # use hierarchical_pathfinding::prelude::*;
/// # let mut grid = [
/// # [0, 2, 0, 0, 0],
/// # [0, 2, 2, 2, 2],
/// # [0, 1, 0, 0, 0],
/// # [0, 1, 0, 2, 0],
/// # [0, 0, 0, 2, 0],
/// # ];
/// # let (width, height) = (grid.len(), grid[0].len());
/// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize {
/// # move |(x, y)| [1, 10, -1][grid[y][x]]
/// # }
/// let pathfinding: PathCache<_> = // ...
/// # PathCache::new(
/// # (width, height),
/// # cost_fn(&grid),
/// # ManhattanNeighborhood::new(width, height),
/// # PathCacheConfig::with_chunk_size(3),
/// # );
///
/// let start = (0, 0);
/// let goals = [(4, 4), (2, 0), (2, 2)];
///
/// // find_closest_goal returns Some((goal, Path)) on success
/// let (goal, path) = pathfinding.find_closest_goal(
/// start,
/// &goals,
/// cost_fn(&grid),
/// ).unwrap();
///
/// assert_eq!(goal, goals[2]);
///
/// let naive_closest = pathfinding
/// .find_paths(start, &goals, cost_fn(&grid))
/// .into_iter()
/// .min_by_key(|(_, path)| path.cost())
/// .unwrap();
///
/// assert_eq!(goal, naive_closest.0);
///
/// let path: Vec<_> = path.collect();
/// let naive_path: Vec<_> = naive_closest.1.collect();
/// assert_eq!(path, naive_path);
/// ```
/// Comparison with [`find_paths`](PathCache::find_paths):
/// ```
/// # use hierarchical_pathfinding::prelude::*;
/// # let mut grid = [
/// # [0, 2, 0, 0, 0],
/// # [0, 2, 2, 2, 2],
/// # [0, 1, 0, 0, 0],
/// # [0, 1, 0, 2, 0],
/// # [0, 0, 0, 2, 0],
/// # ];
/// # let (width, height) = (grid.len(), grid[0].len());
/// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize {
/// # move |(x, y)| [1, 10, -1][grid[y][x]]
/// # }
/// # let pathfinding = PathCache::new(
/// # (width, height),
/// # cost_fn(&grid),
/// # ManhattanNeighborhood::new(width, height),
/// # PathCacheConfig::with_chunk_size(3),
/// # );
/// # let start = (0, 0);
/// # let goals = [(4, 4), (2, 0), (2, 2)];
/// let (goal, path) = pathfinding.find_closest_goal(
/// start,
/// &goals,
/// cost_fn(&grid),
/// ).unwrap();
///
/// let naive_closest = pathfinding
/// .find_paths(start, &goals, cost_fn(&grid))
/// .into_iter()
/// .min_by_key(|(_, path)| path.cost())
/// .unwrap();
///
/// assert_eq!(goal, naive_closest.0);
///
/// let path: Vec<_> = path.collect();
/// let naive_path: Vec<_> = naive_closest.1.collect();
/// assert_eq!(path, naive_path);
/// ```
pub fn find_closest_goal(
&self,
start: Point,
goals: &[Point],
get_cost: impl FnMut(Point) -> isize,
) -> Option<(Point, AbstractPath<N>)> {
self.find_paths_internal(start, goals, get_cost, true)
.into_iter()
.next()
}
fn find_paths_internal(
&self,
start: Point,
goals: &[Point],
mut get_cost: impl FnMut(Point) -> isize,
only_closest_goal: bool,
) -> PointMap<AbstractPath<N>> {
if get_cost(start) < 0 || goals.is_empty() {
return PointMap::default();
}
if goals.len() == 1 {
let goal = goals[0];
return self
.find_path(start, goal, get_cost)
.map(|path| (goal, path))
.into_iter()
.collect();
}
let neighborhood = self.neighborhood.clone();
let (start_id, start_path) =
if let Some(s) = self.find_nearest_node(start, &mut get_cost, false) {
s
} else {
// no path from start to any Node => start is in cave within chunk
// => find all goals in the same cave
return self
.get_chunk(start)
.find_paths(start, goals, get_cost, &neighborhood)
.into_iter()
.map(|(goal, path)| {
(
goal,
AbstractPath::from_known_path(neighborhood.clone(), path),
)
})
.collect();
};
let mut goal_data = Vec::with_capacity(goals.len());
let mut goal_ids = Vec::with_capacity(goals.len());
let mut ret = PointMap::default();
let mut heuristic = 0;
for goal in goals.iter().copied() {
if goal == start {
let path = AbstractPath::from_known_path(
self.neighborhood.clone(),
Path::from_slice(&[start, start], 0),
);
ret.insert(goal, path);
continue;
}
let (goal_id, goal_path) =
if let Some(g) = self.find_nearest_node(goal, &mut get_cost, true) {
g
} else {
continue;
};
goal_data.push((goal, goal_id, goal_path));
goal_ids.push(goal_id);
if only_closest_goal {
heuristic = heuristic.min(self.neighborhood.heuristic(start, goal));
} else {
heuristic = heuristic.max(self.neighborhood.heuristic(start, goal));
}
}
let max_heuristic = neighborhood.heuristic((0, 0), (self.width - 1, self.height - 1));
let max_size = self.nodes.len();
let size_hint = heuristic as f32 / max_heuristic as f32 * max_size as f32;
let paths = graph::dijkstra_search(
&self.nodes,
start_id,
&goal_ids,
only_closest_goal,
size_hint as usize,
);
self.resolve_paths(start, start_path, &goal_data, &paths, get_cost)
}
/// Notifies the PathCache that the Grid changed.
///
/// This Method updates any internal Paths that might have changed when the Grid changed. This
/// is an expensive operation and should only be performed if the change affected the walking
/// cost of a tile and the PathCache is needed again. If possible, try to bundle as many
/// changes as possible into a single call to `tiles_changed` to avoid unnecessary
/// recalculations.
///
/// Side note: if anybody has a way to improve this method, open a GitHub Issue / Pull Request.
///
/// ## Examples
/// Basic usage:
/// ```
/// # use hierarchical_pathfinding::prelude::*;
/// # let mut grid = [
/// # [0, 2, 0, 0, 0],
/// # [0, 2, 2, 2, 2],
/// # [0, 1, 0, 0, 0],
/// # [0, 1, 0, 2, 0],
/// # [0, 0, 0, 2, 0],
/// # ];
/// # let (width, height) = (grid.len(), grid[0].len());
/// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize {
/// # move |(x, y)| [1, 10, -1][grid[y][x]]
/// # }
/// let mut pathfinding: PathCache<_> = // ...
/// # PathCache::new(
/// # (width, height),
/// # cost_fn(&grid),
/// # ManhattanNeighborhood::new(width, height),
/// # PathCacheConfig::with_chunk_size(3),
/// # );
///
/// let (start, goal) = ((0, 0), (2, 0));
///
/// let path = pathfinding.find_path(start, goal, cost_fn(&grid));
/// assert!(path.is_none());
///
/// grid[1][2] = 0;
/// grid[3][2] = 2;
///
/// assert_eq!(grid, [
/// [0, 2, 0, 0, 0],
/// [0, 2, 0, 2, 2],
/// [0, 1, 0, 0, 0],
/// [0, 1, 2, 2, 0],
/// [0, 0, 0, 2, 0],
/// ]);
///
/// pathfinding.tiles_changed(
/// &[(2, 1), (2, 3)],
/// cost_fn(&grid),
/// );
///
/// let path = pathfinding.find_path(start, goal, cost_fn(&grid));
/// assert!(path.is_some());
/// ```
pub fn tiles_changed<F: Sync + Fn(Point) -> isize>(&mut self, tiles: &[Point], get_cost: F) {
#[cfg(feature = "parallel")]
{
self.tiles_changed_internal::<F, fn(Point) -> isize>(
tiles,
CostFnWrapper::Parallel(get_cost),
)
}
#[cfg(not(feature = "parallel"))]
{
self.tiles_changed_internal::<fn(Point) -> isize, F>(
tiles,
CostFnWrapper::Sequential(get_cost, PhantomData),
)
}
}
/// Same as [`tiles_changed`](PathCache::tiles_changed), but doesn't use threads to allow [`FnMut`].
///
/// Equivalent to `tiles_changed` if `parallel` feature is disabled.
///
/// Note that this is _**way**_ slower than `tiles_changed` with `parallel`.
pub fn tiles_changed_with_fn_mut<F: FnMut(Point) -> isize>(
&mut self,
tiles: &[Point],
get_cost: F,
) {
self.tiles_changed_internal::<fn(Point) -> isize, F>(
tiles,
CostFnWrapper::Sequential(get_cost, PhantomData),
)
}
fn tiles_changed_internal<F1, F2>(
&mut self,
tiles: &[Point],
mut get_cost: CostFnWrapper<F1, F2>,
) where
F1: Sync + Fn(Point) -> isize,
F2: FnMut(Point) -> isize,
{
let size = self.config.chunk_size;
#[cfg(feature = "log")]
let (outer_timer, timer) = (std::time::Instant::now(), std::time::Instant::now());
let mut dirty = PointMap::default();
for &p in tiles {
let chunk_pos = self.get_chunk_pos(p);
dirty.entry(chunk_pos).or_insert_with(Vec::new).push(p);
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum Renew {
No,
Inner,
Corner(Point),
All,
}
// map of chunk_pos => array: [Renew; 4] where array[side] says if chunk[side] needs to be renewed
let mut renew = PointMap::default();
for (&cp, positions) in dirty.iter() {
let chunk = self.get_chunk(cp);
// for every changed tile in the chunk
for &p in positions {
// check every side that this tile is on
for dir in Dir::all().filter(|dir| chunk.sides[dir.num()] && chunk.at_side(p, *dir))
{
// if there is a chunk in that direction
let other_pos = jump_in_dir(cp, dir, size, (0, 0), (self.width, self.height))
.expect("Internal Error #2 in PathCache. Please report this");
// mark the current and other side
let own = &mut renew.entry(cp).or_insert([Renew::No; 4])[dir.num()];
let old = *own;
if chunk.is_corner(p) {
if old == Renew::No || old == Renew::Inner {
*own = Renew::Corner(p);
} else if let Renew::Corner(p2) = old {
if p2 != p {
*own = Renew::All;
}
} else if old != Renew::All {
*own = Renew::Corner(p)
}
} else {
// All > Corner > Inner > No, and we don't want to override anything greater than Inner
if *own == Renew::No {
*own = Renew::Inner;
}
}
let other =
&mut renew.entry(other_pos).or_insert([Renew::No; 4])[dir.opposite().num()];
if *other == Renew::No {
*other = Renew::Inner;
}
}
}
}
re_trace!("establish renew", timer);
// remove all nodes of sides in renew
for (&cp, sides) in renew.iter() {
let chunk_index = self.get_chunk_index(cp);
let chunk = &self.chunks[chunk_index];
let removed = chunk
.nodes
.iter()
.filter(|id| {
let pos = self.nodes[**id].pos;
let corner = chunk.is_corner(pos);
Dir::all().any(|dir| match sides[dir.num()] {
Renew::No => false,
Renew::Inner => !corner,
Renew::Corner(c) => !corner || c == pos,
Renew::All => true,
} && chunk.at_side(pos, dir))
})
.copied()
.to_vec();
let chunk = &mut self.chunks[chunk_index];
for id in removed {
chunk.nodes.remove(&id);
self.nodes.remove_node(id);
}
}
re_trace!("remove nodes of sides in renew", timer);
let mut changed_nodes = NodeIDSet::default();
// remove all Paths in changed chunks
for cp in dirty.keys() {
let chunk_index = self.get_chunk_index(*cp);
let chunk = &self.chunks[chunk_index];
for id in chunk.nodes.iter() {
self.nodes[*id].edges.clear();
}
}
{
let mut get_cost: &mut dyn FnMut(Point) -> isize = match &mut get_cost {
CostFnWrapper::Sequential(get_cost, _) => get_cost,
#[cfg(feature = "parallel")]
CostFnWrapper::Parallel(get_cost) => get_cost,
};
// recreate sides in renew
for (&cp, sides) in renew.iter() {
let mut candidates = PointSet::default();
let chunk_index = self.get_chunk_index(cp);
let chunk = &self.chunks[chunk_index];
for dir in Dir::all() {
if sides[dir.num()] != Renew::No {
chunk.calculate_side_nodes(
dir,
(self.width, self.height),
&mut get_cost,
self.config,
&mut candidates,
);
}
}
// Only include nodes that aren't already part of the map
candidates.retain(|&pos| self.nodes.id_at(pos).is_none());
if candidates.is_empty() {
continue;
}
let all_nodes = &mut self.nodes;
let nodes = candidates
.into_iter()
.map(|p| all_nodes.add_node(p, get_cost(p) as usize))
.to_vec();
let chunk = &mut self.chunks[chunk_index];
if !dirty.contains_key(&cp) {
for node in nodes.iter() {
changed_nodes.insert(*node);
}
chunk.add_nodes(
&nodes,
&mut get_cost,
&self.neighborhood,
&mut self.nodes,
&self.config,
);
} else {
for id in nodes {
chunk.nodes.insert(id);
}
}
}
}
re_trace!("recreates sides in renew", timer);
match get_cost {
CostFnWrapper::Sequential(mut get_cost, _) => {
for cp in dirty.keys() {
let chunk_index = self.get_chunk_index(*cp);
let chunk = &mut self.chunks[chunk_index];
let nodes = chunk.nodes.iter().copied().to_vec();
for node in nodes.iter() {
changed_nodes.insert(*node);
}
chunk.nodes.clear();
chunk.add_nodes(
&nodes,
&mut get_cost,
&self.neighborhood,
&mut self.nodes,
&self.config,
);
}
re_trace!("recreate Paths", timer);
}
#[cfg(feature = "parallel")]
CostFnWrapper::Parallel(get_cost) => {
use rayon::prelude::*;
let dirty_indices: hashbrown::HashSet<usize> = dirty
.keys()
.map(|(x, y)| self.get_chunk_index((*x, *y)))
.collect();
let paths: Vec<_> = {
let neighborhood = &self.neighborhood;
let all_nodes = &self.nodes;
let cache_paths = self.config.cache_paths;
self.chunks
.par_iter()
.enumerate()
.filter(|(chunk_index, _)| dirty_indices.contains(chunk_index))
.map(|(_, chunk)| {
chunk.connect_nodes_parallel(
&get_cost,
neighborhood,
all_nodes,
cache_paths,
)
})
.collect()
};
re_trace!("get paths", timer);
for (id, other_id, path) in paths.into_iter().flatten() {
self.nodes.add_edge(id, other_id, path);
}
for chunk_index in dirty_indices.iter() {
for node in self.chunks[*chunk_index].nodes.iter() {
changed_nodes.insert(*node);
}
}
re_trace!("update edges", timer);
}
}
// re-establish cross-chunk connections
self.connect_nodes(Some(changed_nodes));
re_trace!("connect nodes", timer);
re_trace!("total time", outer_timer);
}
/// Allows for debugging and visualizing the PathCache
///
/// The returned object gives read-only access to the current state of the PathCache, mainly the
/// Nodes and how they are connected to each other
///
/// ## Examples
/// Basic usage:
/// ```
/// # use hierarchical_pathfinding::prelude::*;
/// # let mut grid = [
/// # [0, 2, 0, 0, 0],
/// # [0, 2, 2, 2, 2],
/// # [0, 1, 0, 0, 0],
/// # [0, 1, 0, 2, 0],
/// # [0, 0, 0, 2, 0],
/// # ];
/// # let (width, height) = (grid.len(), grid[0].len());
/// # fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Sync + Fn((usize, usize)) -> isize {
/// # move |(x, y)| [1, 10, -1][grid[y][x]]
/// # }
/// let pathfinding: PathCache<_> = // ...
/// # PathCache::new(
/// # (width, height),
/// # cost_fn(&grid),
/// # ManhattanNeighborhood::new(width, height),
/// # PathCacheConfig::with_chunk_size(3),
/// # );
///
/// // only draw the connections between Nodes once
/// # use std::collections::HashSet;
/// let mut visited = HashSet::new();
///
/// for node in pathfinding.inspect_nodes() {
/// let pos = node.pos();
/// // draw Node at x: pos.0, y: pos.1
///
/// visited.insert(node.id());
///
/// for (neighbor, cost) in node.connected().filter(|(n, _)| !visited.contains(&n.id())) {
/// let other_pos = neighbor.pos();
/// // draw Line from pos to other_pos, colored by cost
/// }
/// }
/// ```
pub fn inspect_nodes(&self) -> CacheInspector<N> {
CacheInspector::new(self)
}
/// Prints all Nodes
#[allow(dead_code)]
fn print_nodes(&self) {
for node in self.inspect_nodes() {
print!("{} at {:?}: ", node.id(), node.pos());
for (neighbor, cost) in node.connected() {
print!("{:?}({}), ", neighbor.pos(), cost);
}
println!();
}
}
fn get_chunk_pos(&self, point: Point) -> Point {
let size = self.config.chunk_size;
((point.0 / size) * size, (point.1 / size) * size)
}
fn get_chunk(&self, point: Point) -> &Chunk {
let index = self.get_chunk_index(point);
&self.chunks[index]
}
fn get_chunk_index(&self, point: Point) -> usize {
let size = self.config.chunk_size;
let (x, y) = ((point.0 / size), (point.1 / size));
y * self.num_chunks.0 + x
}
fn same_chunk(&self, a: Point, b: Point) -> bool {
let size = self.config.chunk_size;
a.0 / size == b.0 / size && a.1 / size == b.1 / size
}
fn node_at(&self, pos: Point) -> Option<NodeID> {
self.nodes.id_at(pos)
}
/// Returns the config used to create this PathCache
pub fn config(&self) -> &PathCacheConfig {
&self.config
}
fn find_nearest_node(
&self,
pos: Point,
get_cost: impl FnMut(Point) -> isize,
reverse: bool,
) -> Option<(NodeID, Option<Path<Point>>)> {
if let Some(id) = self.node_at(pos) {
return Some((id, None));
}
self.get_chunk(pos)
.nearest_node(&self.nodes, pos, get_cost, &self.neighborhood, reverse)
.map(|(id, path)| (id, Some(path)))
}
fn grid_a_star(
&self,
start: Point,
goal: Point,
get_cost: impl FnMut(Point) -> isize,
) -> Option<Path<Point>> {
let heuristic = self.neighborhood.heuristic(start, goal);
let max_heuristic = self
.neighborhood
.heuristic((0, 0), (self.width - 1, self.height - 1));
let max_size = self.width * self.height;
let size_hint = heuristic as f32 / max_heuristic as f32 * max_size as f32;
grid::a_star_search(
&self.neighborhood,
|_| true,
get_cost,
start,
goal,
size_hint as usize,
)
}
fn resolve_paths(
&self,
start: Point,
start_path: Option<Path<Point>>,
goal_data: &[(Point, NodeID, Option<Path<Point>>)],
paths: &NodeIDMap<Path<NodeID>>,
mut get_cost: impl FnMut(Point) -> isize,
) -> PointMap<AbstractPath<N>> {
let mut start_path_map = PointMap::default();
let mut ret = PointMap::default();
for (goal, goal_id, goal_path) in goal_data {
let path = if let Some(path) = paths.get(goal_id) {
path
} else {
continue;
};
let mut start_path = start_path.as_ref();
let mut skip_first = false;
let mut skip_last = false;
if start_path.is_some() {
let after_start = self.nodes[path[1]].pos;
if self.same_chunk(start, after_start) {
start_path = Some(start_path_map.entry(after_start).or_insert_with(|| {
// this is contained within a chunk, because start_path is contained and
// (start_id, after_start) must be contained:
// Direct paths between nodes are only added in chunk::(connect/add)_nodes,
// or in the cross-chunk connect_nodes
self.get_chunk(start)
.find_path(start, after_start, &mut get_cost, &self.neighborhood)
.expect("Inconsistency in Pathfinding")
}));
skip_first = true;
}
}
// path: ... -> before_goal (len-2) -> goal_id (len-1) (-> actual goal (would be next))
// check if direct connection of before_goal -> actual goal is feasible
let before_goal = self.nodes[path[path.len() - 2]].pos;
if goal_path.is_some() && self.same_chunk(*goal, before_goal) {
skip_last = true;
}
let mut final_path = if let Some(path) = start_path {
AbstractPath::from_known_path(self.neighborhood.clone(), path.clone())
} else {
AbstractPath::new(self.neighborhood.clone(), start)
};
for (i, (a, b)) in path.iter().zip(path.iter().skip(1)).enumerate() {
if (skip_first && i == 0) || (skip_last && i == path.len() - 2) {
// len() - 2 because skip(1) already removes one
continue;
}
final_path.add_path_segment(self.nodes[*a].edges[&b].clone());
}
if skip_last {
final_path.add_path(
// reasoning for chunk containment: see start_path equivalent
self.get_chunk(before_goal)
.find_path(before_goal, *goal, &mut get_cost, &self.neighborhood)
.expect("Inconsistency in Pathfinding"),
);
} else if let Some(path) = goal_path {
final_path.add_path(path.clone());
}
ret.insert(*goal, final_path);
}
ret
}
fn connect_nodes(&mut self, ids: Option<NodeIDSet>) {
let ids = ids.unwrap_or_else(|| self.nodes.keys().collect());
let mut target = vec![];
for id in ids {
let (pos, cost) = {
let node = &self.nodes[id];
(node.pos, node.walk_cost)
};
target.clear();
self.neighborhood.get_all_neighbors(pos, &mut target);
for &other_pos in target.iter() {
if let Some(other_id) = self.node_at(other_pos) {
self.nodes.add_edge(
id,
other_id,
PathSegment::new(
Path::from_slice(&[pos, other_pos], cost),
self.config.cache_paths,
),
);
}
}
}
}
}
/// Allows for debugging and visualizing a PathCache.
///
/// See [`inspect_nodes`](PathCache::inspect_nodes) for details and an example.
///
/// Allows iteration over all Nodes and specific lookup with [`get_node`](CacheInspector::get_node)
#[derive(Debug)]
pub struct CacheInspector<'a, N: Neighborhood> {
src: &'a PathCache<N>,
inner: std::vec::IntoIter<NodeID>,
}
impl<'a, N: Neighborhood> CacheInspector<'a, N> {
/// Creates a new CacheInspector
///
/// Same as calling [`.inspect_nodes()`](PathCache::inspect_nodes) on the cache
pub fn new(src: &'a PathCache<N>) -> Self {
CacheInspector {
src,
inner: src.nodes.keys().to_vec().into_iter(),
}
}
/// Provides the handle to a specific Node.
///
/// It is recommended to use the `Iterator` implementation instead
pub fn get_node(&self, id: NodeID) -> NodeInspector<N> {
NodeInspector::new(self.src, id)
}
}
impl<'a, N: Neighborhood> Iterator for CacheInspector<'a, N> {
type Item = NodeInspector<'a, N>;
fn next(&mut self) -> Option<Self::Item> {
self.inner.next().map(|id| NodeInspector::new(self.src, id))
}
}
/// Allows for debugging and visualizing a Node
///
/// See [`inspect_nodes`](PathCache::inspect_nodes) for details and an example.
///
/// Can be obtained by iterating over a [`CacheInspector`] or from [`get_node`](CacheInspector::get_node).
///
/// Gives basic info about the Node and an Iterator over all connected Nodes
#[derive(Debug, Clone, Copy)]
pub struct NodeInspector<'a, N: Neighborhood> {
src: &'a PathCache<N>,
node: &'a Node,
}
impl<'a, N: Neighborhood> NodeInspector<'a, N> {
fn new(src: &'a PathCache<N>, id: NodeID) -> Self {
NodeInspector {
src,
node: &src.nodes[id],
}
}
/// The position of the Node on the Grid
pub fn pos(&self) -> (usize, usize) {
self.node.pos
}
/// The internal unique ID
///
/// IDs are unique at any point in time, but may be reused if Nodes are deleted.
pub fn id(&self) -> NodeID {
self.node.id
}
/// Provides an iterator over all connected Nodes with the Cost of the Path to that Node
pub fn connected(&'a self) -> impl Iterator<Item = (NodeInspector<'a, N>, Cost)> + 'a {
self.node
.edges
.iter()
.map(move |(id, path)| (NodeInspector::new(self.src, *id), path.cost()))
}
}
#[cfg(test)]
mod tests {
use crate::prelude::*;
#[test]
fn new() {
let grid = [
[0, 2, 0, 0, 0],
[0, 2, 2, 2, 2],
[0, 1, 0, 0, 0],
[0, 1, 0, 2, 0],
[0, 0, 0, 2, 0],
];
let (width, height) = (grid.len(), grid[0].len());
fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Fn((usize, usize)) -> isize {
move |(x, y)| [1, 10, -1][grid[y][x]]
}
let pathfinding = PathCache::new(
(width, height),
cost_fn(&grid),
ManhattanNeighborhood::new(width, height),
PathCacheConfig::with_chunk_size(3),
);
let start = (0, 0);
let goal = (4, 4);
let path = pathfinding.find_path(start, goal, cost_fn(&grid));
let path = path.unwrap();
let points: Vec<(usize, usize)> = path.collect();
#[rustfmt::skip]
assert_eq!(
points,
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)],
);
}
#[test]
fn get_chunk_index() {
let grid = [
[0, 2, 0, 0, 0],
[0, 2, 2, 2, 2],
[0, 1, 0, 0, 0],
[0, 1, 0, 2, 0],
[0, 0, 0, 2, 0],
];
let (width, height) = (grid.len(), grid[0].len());
fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Fn((usize, usize)) -> isize {
move |(x, y)| [1, 10, -1][grid[y][x]]
}
let pathfinding = PathCache::new(
(width, height),
cost_fn(&grid),
ManhattanNeighborhood::new(width, height),
PathCacheConfig::with_chunk_size(3),
);
let point = (0, 0);
assert_eq!(pathfinding.get_chunk_index(point), 0);
let point = (4, 0);
assert_eq!(pathfinding.get_chunk_index(point), 1);
let point = (3, 2);
assert_eq!(pathfinding.get_chunk_index(point), 1);
let point = (4, 4);
assert_eq!(pathfinding.get_chunk_index(point), 3);
let point = (0, 4);
assert_eq!(pathfinding.get_chunk_index(point), 2);
}
#[test]
fn update_path() {
let mut grid = [
[0, 2, 0, 0, 0],
[0, 2, 2, 2, 2],
[0, 1, 0, 0, 0],
[0, 1, 0, 2, 0],
[0, 0, 0, 2, 0],
];
let (width, height) = (grid.len(), grid[0].len());
fn cost_fn(grid: &[[usize; 5]; 5]) -> impl '_ + Fn((usize, usize)) -> isize {
move |(x, y)| [1, 10, -1][grid[y][x]]
}
let mut pathfinding = PathCache::new(
(width, height),
cost_fn(&grid),
MooreNeighborhood::new(width, height),
PathCacheConfig::with_chunk_size(3),
);
let start = (0, 0);
let goal = (4, 4);
let path = pathfinding.find_path(start, goal, cost_fn(&grid));
let path = path.unwrap();
let points: Vec<(usize, usize)> = path.collect();
#[rustfmt::skip]
assert_eq!(
points,
vec![(0, 1), (0, 2), (0, 3), (1, 4), (2, 3), (3, 2), (4, 3), (4, 4)],
);
grid[2][1] = 0;
let changed_tiles = [(1, 2)];
pathfinding.tiles_changed(&changed_tiles, cost_fn(&grid));
let start = (0, 0);
let goal = (4, 4);
let path = pathfinding.find_path(start, goal, cost_fn(&grid));
let path = path.unwrap();
let points: Vec<(usize, usize)> = path.collect();
#[rustfmt::skip]
assert_eq!(
points,
vec![(0, 1), (1, 2), (2, 2), (3, 2), (4, 3), (4, 4)],
);
// Add walls along chunk borders
let start = (0, 0);
let goal = (3, 4);
let path = pathfinding.find_path(start, goal, cost_fn(&grid));
assert!(path.is_some());
// Vertical wall
let changed_tiles: Vec<_> = (0..grid.len()).map(|y| (2, y)).collect();
grid.iter_mut().for_each(|row| row[2] = 2);
pathfinding.tiles_changed(&changed_tiles, cost_fn(&grid));
let path = pathfinding.find_path(start, goal, cost_fn(&grid));
assert!(path.is_none());
let goal = (2, 4);
let path = pathfinding.find_path(start, goal, cost_fn(&grid));
assert!(path.is_some());
// Horizontal wall
let mut changed_tiles = Vec::new();
{
let y = 2;
for x in 0..grid[y].len() {
grid[y][x] = 2;
changed_tiles.push((x, y));
}
}
pathfinding.tiles_changed(&changed_tiles, cost_fn(&grid));
let path = pathfinding.find_path(start, goal, cost_fn(&grid));
assert!(path.is_none());
}
#[allow(unused)]
// #[test]
#[cfg(feature = "parallel")]
fn random_test() {
use nanorand::Rng;
use rayon::prelude::*;
for &size in [8, 128, 1024].iter() {
let mut grid = vec![vec![0; size]; size];
grid.par_iter_mut().for_each(|row| {
// let mut rng = StdRng::from_entropy();
let mut rng = nanorand::tls_rng();
row.fill_with(|| rng.generate_range(-2_isize..7));
});
let cost_fn = |(x, y): (usize, usize)| grid[y][x];
for &chunk_size in [8, 16, 64].iter() {
println!("size: {}, chunk_size: {}", size, chunk_size);
let pathfinding = PathCache::new(
(size, size),
cost_fn,
ManhattanNeighborhood::new(size, size),
PathCacheConfig::with_chunk_size(chunk_size),
);
// for _ in 0..100 {
[0..100].par_iter().for_each(|_| {
let mut rng = nanorand::tls_rng();
let start = (rng.generate_range(0..size), rng.generate_range(0..size));
let goal = (rng.generate_range(0..size), rng.generate_range(0..size));
let a_star_path = pathfinding.grid_a_star(start, goal, cost_fn);
let path = pathfinding.find_path(start, goal, cost_fn);
if a_star_path.is_some() != path.is_some() {
use std::io::Write;
let mut out = std::fs::File::create("cache.txt").unwrap();
writeln!(out, "start: {:?}, goal: {:?}", start, goal).unwrap();
writeln!(out, "a_star_path: {:?}", a_star_path).unwrap();
writeln!(out, "our path: {:?}", path).unwrap();
writeln!(out, "{:#?}", pathfinding).unwrap();
let mut out = std::fs::File::create("grid.txt").unwrap();
for row in grid.iter() {
for cell in row.iter() {
write!(out, "{}", if *cell < 0 { '#' } else { ' ' }).unwrap();
}
writeln!(out).unwrap();
}
panic!("Failed");
}
});
// }
}
}
}
}