astar

[Done] Astar Pathing Library for Rust
git clone https://git.jojolepro.com/astar.git
Log | Files | Refs | LICENSE

commit f66462b1f17592f8132d3c6970deeb8e9cc1b051
parent a182d92e44356963491e039b297f45b9d2e51b0c
Author: Joël Lupien (Jojolepro) <jojolepromain@gmail.com>
Date:   Wed, 15 May 2019 11:47:54 -0400

Finish the algorithm, fix issues

Diffstat:
Msrc/lib.rs | 97+++++++++++++++++++++++++++++++++++++++++++++++++------------------------------
1 file changed, 60 insertions(+), 37 deletions(-)

diff --git a/src/lib.rs b/src/lib.rs @@ -4,6 +4,11 @@ use hibitset::BitSet; /// Expressed as a tuple of the (x, y) coordinates. pub type Position = (usize, usize); +/// The weight indicates how easy it is to travel a square. +/// A weight of 1 indicates that the "length" of the square is 1. +/// A weight of 0 indicates a square of infinite length (that cannot be traversed). A wall for +/// example would have a weight of 0. +/// A weight of 2 indicates that the length of the square is 0.5 (2x travel speed). pub trait Weighted { fn get_weight(&self) -> f32; } @@ -14,38 +19,48 @@ impl Weighted for f32 { } } +/// A map is a fixed size grid composed of squares. +/// Each square is of type B (the generic parameter). +/// +/// # Generics +/// B: The type of square of which the map in composed. See the `Weighted` trait for more +/// information. pub struct Map<B: Weighted> { map: Vec<B>, size: (usize, usize), } -impl<B: Weighted + Default> Map<B> { +impl<B: Weighted + Default + Clone> Map<B> { + /// Create a new map of the specified size. pub fn new(size_x: usize, size_y: usize) -> Self { Map { - map: Vec::fill(B::default(), size_x * size_y), + map: vec![B::default(); size_x * size_y], size: (size_x, size_y), } } + /// Get the grid at the specified position. pub fn get_grid(&self, x: usize, y: usize) -> &B { - let idx = get_index_for_position(x, y); - map.get(idx).unwrap() + let idx = self.get_index_for_position(x, y); + self.map.get(idx).unwrap() } - pub fn get_grid_mut(&self, x: usize, y: usize) -> &mut B { - let idx = get_index_for_position(x, y); - map.get_mut(idx).unwrap() + /// Get the grid mutably at the specified position. + pub fn get_grid_mut(&mut self, x: usize, y: usize) -> &mut B { + let idx = self.get_index_for_position(x, y); + self.map.get_mut(idx).unwrap() } - pub fn insert_grid(&self, x: usize, y: usize, grid: B) { - let idx = get_index_for_position(x, y); - *map.get_mut(idx).unwrap() = grid; + /// Inserts a grid element at the specified position. + pub fn insert_grid(&mut self, x: usize, y: usize, grid: B) { + let idx = self.get_index_for_position(x, y); + *self.map.get_mut(idx).unwrap() = grid; } fn get_index_for_position(&self, x: usize, y: usize) -> usize { let idx = y * self.size.1 + x; - assert!(idx < self.max_index); + assert!(idx < self.max_index()); idx } @@ -63,10 +78,15 @@ impl<B: Weighted + Default> Map<B> { let from = self.get_position_for_index(from); let to = self.get_position_for_index(to); - ((to.0 - from.0) * (to.0 - from.0) + (to.1 - from.1) * (to.1 - from.1)).sqrt() + let to0 = to.0 as f32; + let to1 = to.1 as f32; + let from0 = from.0 as f32; + let from1 = from.1 as f32; + + ((to0 - from0) * (to0 - from0) + (to1 - from1) * (to1 - from1)).sqrt() } - fn heuristic(&self, from: Position, to: Position) -> f32 { + fn heuristic(&self, _from: Position, _to: Position) -> f32 { // Slowest possible heuristic for testing. 0.0 } @@ -77,79 +97,79 @@ impl<B: Weighted + Default> Map<B> { // Left if pos.0 > 0 { - res.push((pos.0 - 1, pos.1)); + res.push(self.get_index_for_position(pos.0 - 1, pos.1)); } // Right if pos.0 < self.size.0 { - res.push((pos.0 + 1, pos.1)); + res.push(self.get_index_for_position(pos.0 + 1, pos.1)); } // Up if pos.1 > 0 { - res.push((pos.0, pos.1 - 1)); + res.push(self.get_index_for_position(pos.0, pos.1 - 1)); } // Down if pos.1 < self.size.1 { - res.push((pos.0, pos.1 + 1)); + res.push(self.get_index_for_position(pos.0, pos.1 + 1)); } res } pub fn shortest_path(&self, from: Position, to: Position) -> Option<Vec<Position>> { - let max_index = self.max_index; - let node_count = self.max_index + 1; + let max_index = self.max_index(); + let node_count = max_index + 1; assert!(from.0 * from.1 < max_index); assert!(to.0 * to.1 < max_index); // Merge closed_set into came_from? - let mut closed_set = BitSet::with_capacity(node_count); - let mut open_set = BitSet::with_capacity(node_count); - let mut came_from = vec![None; node_count]; - let mut gscore = vec![std::f32::MAX; node_count]; - let mut fscore = vec![std::f32::MAX; node_count]; + let mut closed_set = BitSet::with_capacity(node_count as u32); + let mut open_set = BitSet::with_capacity(node_count as u32); + let mut came_from = vec![None; node_count as usize]; + let mut gscore = vec![std::f32::MAX; node_count as usize]; + let mut fscore = vec![std::f32::MAX; node_count as usize]; let start_idx = self.get_index_for_position(from.0, from.1); let to_idx = self.get_index_for_position(to.0, to.1); - open_set.add(start_idx); - *gscore.get_mut(start_idx).unwrap() = 0; + open_set.add(start_idx as u32); + *gscore.get_mut(start_idx).unwrap() = 0.0; *fscore.get_mut(start_idx).unwrap() = self.heuristic(from, to); // Find node in open set with lowest fscore. - while let Some((idx, fscore)) = (0..max_index).iter().filter(|i| open_set.contains(i)).map(|i| (i, fscore.get(i).unwrap())).fold(None, |cur, accum| { + while let Some((idx, _fscore)) = (0..max_index).filter(|i| open_set.contains(*i as u32)).map(|i| (i, fscore.get(i).unwrap())).fold(None, |accum: Option<(usize, f32)>, cur| { if let Some(acc) = accum { - if acc.1 > cur.1 { - Some(cur) + if acc.1 > *cur.1 { + Some((cur.0, *cur.1)) } else { Some(acc) } } else { - Some(cur) + Some((cur.0, *cur.1)) } }) { // Exit condition if to_idx == idx { - return self.reconstruct_path(came_from, idx); + return Some(self.reconstruct_path(came_from, idx)); } - open_set.remove(idx); - closed_set.add(idx); + open_set.remove(idx as u32); + closed_set.add(idx as u32); for neighbor in self.neighbors_of(idx) { - if closed_set.contains(neighbor) { + if closed_set.contains(neighbor as u32) { continue; } let attempt_gscore = gscore.get(idx).unwrap() + self.dist_between(idx, neighbor); - if !open_set.contains(neighbor) { - open_set.add(neighbor); - } else if attempt_gscore >= gscore.get(neighbor).unwrap() { + if !open_set.contains(neighbor as u32) { + open_set.add(neighbor as u32); + } else if attempt_gscore >= *gscore.get(neighbor).unwrap() { continue; } @@ -168,6 +188,7 @@ impl<B: Weighted + Default> Map<B> { total_path.push(self.get_position_for_index(*prev)); current = *prev; } + total_path.reverse(); total_path } } @@ -176,6 +197,8 @@ impl<B: Weighted + Default> Map<B> { #[cfg(test)] mod tests { + use crate::*; + #[test] fn astar() { let map = Map::<f32>::new(20, 20);