commit
14b6c54ea5
@ -1,89 +1,68 @@ |
||||
use std::collections::{HashMap, HashSet}; |
||||
|
||||
#[derive(Hash, PartialEq, Eq, Clone, Debug, Copy)] |
||||
pub struct Point(pub i32, pub i32); |
||||
|
||||
impl Point { |
||||
pub fn get_neighbors(&self) -> Vec<Point> { |
||||
vec![ |
||||
Point(self.0, self.1 + 1), |
||||
Point(self.0, self.1 - 1), |
||||
Point(self.0 - 1, self.1), |
||||
Point(self.0 + 1, self.1), |
||||
] |
||||
vec![self.left(), self.right(), self.up(), self.down()] |
||||
} |
||||
} |
||||
|
||||
fn manhattan(c1: &Point, c2: &Point) -> i32 { |
||||
return (c2.0 - c1.0).abs() + (c2.1 - c1.1).abs(); |
||||
} |
||||
|
||||
pub fn astar(start: &Point, goal: &Point) -> Vec<Point> { |
||||
let mut open_set : HashSet<Point> = HashSet::new(); |
||||
open_set.insert(*start); |
||||
|
||||
let mut came_from: HashMap<Point, Point> = HashMap::new(); |
||||
|
||||
let mut gscore: HashMap<Point, i32> = HashMap::new(); |
||||
gscore.insert(*start, 0); |
||||
|
||||
let mut fscore: HashMap<Point, i32> = HashMap::new(); |
||||
fscore.insert(*start, manhattan(&start, &goal)); |
||||
pub fn left(&self) -> Point { |
||||
Point(self.0 - 1, self.1) |
||||
} |
||||
|
||||
let mut answer = Vec::new(); |
||||
pub fn right(&self) -> Point { |
||||
Point(self.0 + 1, self.1) |
||||
} |
||||
|
||||
let mut current: Point = *start; |
||||
while !open_set.is_empty() { |
||||
let mut min_score = i32::MAX; |
||||
for c in open_set.iter() { |
||||
let val = fscore[c]; |
||||
current = if val < min_score { c.clone() } else { current }; |
||||
min_score = if val < min_score { val } else { min_score }; |
||||
// y values are reversed in ncurses
|
||||
pub fn down(&self) -> Point { |
||||
Point(self.0, self.1 + 1) |
||||
} |
||||
|
||||
if current == *goal { |
||||
answer.push(current.clone()); |
||||
while came_from.contains_key(¤t) { |
||||
current = came_from[¤t].clone(); |
||||
if current != *start { |
||||
answer.push(current.clone()); |
||||
pub fn up(&self) -> Point { |
||||
Point(self.0, self.1 - 1) |
||||
} |
||||
} |
||||
|
||||
#[derive(Clone, Copy, PartialEq, Eq, Debug)] |
||||
pub enum Direction { |
||||
Up = 0, |
||||
Right = 1, |
||||
Down = 2, |
||||
Left = 3, |
||||
} |
||||
|
||||
impl Direction { |
||||
pub fn from_u8(v: u8) -> Direction { |
||||
match v { |
||||
0 => Direction::Up, |
||||
1 => Direction::Right, |
||||
2 => Direction::Down, |
||||
3 => Direction::Left, |
||||
_ => panic!("Shouldn't happen") |
||||
} |
||||
break; |
||||
} |
||||
|
||||
open_set.remove(¤t); |
||||
|
||||
let current_gscore = gscore[¤t]; |
||||
pub fn cw(&self) -> Direction { |
||||
let val = *self as u8; |
||||
Direction::from_u8((val + 1) % 4) |
||||
} |
||||
|
||||
let all_neighbors: Vec<Point> = current.get_neighbors(); |
||||
for neighbor in all_neighbors.iter() { |
||||
let neighbor_score = if gscore.contains_key(&neighbor) { |
||||
gscore[neighbor] |
||||
pub fn ccw(&self) -> Direction { |
||||
let val = *self as u8; |
||||
if val == 0 { |
||||
Direction::Left |
||||
} else { |
||||
i32::MAX |
||||
}; |
||||
let score = current_gscore + 1; |
||||
if score < neighbor_score { |
||||
gscore.insert(neighbor.clone(), score); |
||||
fscore.insert(neighbor.clone(), score + manhattan(&neighbor, &goal)); |
||||
came_from.insert(neighbor.clone(), current.clone()); |
||||
open_set.insert(neighbor.clone()); |
||||
} |
||||
Direction::from_u8(val - 1) |
||||
} |
||||
} |
||||
|
||||
answer |
||||
} |
||||
|
||||
#[test] |
||||
fn test_astar() { |
||||
let start = Point(0, 0); |
||||
|
||||
let goal = Point(10, 10); |
||||
let answers: Vec<Point> = astar(&start, &goal); |
||||
|
||||
for a in answers.iter() { |
||||
println!("{:?}", &a); |
||||
pub fn relative_point(&self, p: &Point) -> Point { |
||||
match self { |
||||
Direction::Up => p.up(), |
||||
Direction::Left => p.left(), |
||||
Direction::Right => p.right(), |
||||
Direction::Down => p.down(), |
||||
} |
||||
} |
||||
} |
||||
|
Loading…
Reference in new issue