Simple Path
This commit is contained in:
parent
7d47a10acf
commit
6f6a7f978b
6 changed files with 333 additions and 161 deletions
|
|
@ -1,4 +1,10 @@
|
|||
use crate::graph::wheighted_graph::shortest_path::dijkstra;
|
||||
use crate::graph::wheighted_graph::WheightedGraph;
|
||||
use crate::priority_queue::BinaryHeap;
|
||||
use crate::{misc::Map, priority_queue::PriorityQueue};
|
||||
use colored::Colorize;
|
||||
use std::fmt::{write, Display};
|
||||
use std::io::Write;
|
||||
|
||||
#[derive(Clone, Copy)]
|
||||
pub enum Direction {
|
||||
|
|
@ -8,163 +14,163 @@ pub enum Direction {
|
|||
Left,
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
|
||||
pub struct Position {
|
||||
pub x: usize,
|
||||
pub y: usize,
|
||||
}
|
||||
impl Position {
|
||||
pub fn new(x: usize, y: usize) -> Self {
|
||||
Self { x, y }
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Default, Clone, Copy)]
|
||||
pub struct Field {
|
||||
pub blocked: bool,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct QueueObject {
|
||||
pos: Position,
|
||||
score: usize,
|
||||
pub struct Problem {
|
||||
map: Map<Field>,
|
||||
start: Position,
|
||||
end: Position,
|
||||
path: Vec<Position>,
|
||||
}
|
||||
|
||||
impl QueueObject {
|
||||
fn new(pos: Position, score: usize) -> Self {
|
||||
Self { pos, score }
|
||||
}
|
||||
}
|
||||
|
||||
impl Eq for QueueObject {}
|
||||
|
||||
impl Ord for QueueObject {
|
||||
fn cmp(&self, other: &Self) -> std::cmp::Ordering {
|
||||
self.score.cmp(&other.score)
|
||||
}
|
||||
}
|
||||
|
||||
impl PartialOrd for QueueObject {
|
||||
fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
|
||||
Some(self.score.cmp(&other.score))
|
||||
}
|
||||
}
|
||||
|
||||
impl PartialEq for QueueObject {
|
||||
fn eq(&self, other: &Self) -> bool {
|
||||
self.score == other.score
|
||||
}
|
||||
}
|
||||
|
||||
struct FieldInternal<K> {
|
||||
score: usize,
|
||||
marked: bool,
|
||||
key: Option<K>,
|
||||
parent: Option<Direction>,
|
||||
}
|
||||
|
||||
impl<K> Default for FieldInternal<K> {
|
||||
fn default() -> Self {
|
||||
impl Problem {
|
||||
pub fn new(width: usize, height: usize, start: Position, end: Position) -> Self {
|
||||
Self {
|
||||
score: usize::MAX,
|
||||
marked: false,
|
||||
key: None,
|
||||
parent: None,
|
||||
map: Map::new(width, height),
|
||||
start,
|
||||
end,
|
||||
path: Vec::new(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn set_blocked(&mut self, x: usize, y: usize, blocked: bool) {
|
||||
self.map.get_mut(x, y).blocked = blocked;
|
||||
}
|
||||
}
|
||||
|
||||
fn relax<P>(
|
||||
internal_map: &mut Map<FieldInternal<P::Handle>>,
|
||||
q: &mut P,
|
||||
x: usize,
|
||||
y: usize,
|
||||
score: usize,
|
||||
parent: Direction,
|
||||
) where
|
||||
P: PriorityQueue<QueueObject>,
|
||||
{
|
||||
let f = internal_map.get_mut(x, y);
|
||||
if !f.marked {
|
||||
if let Some(h) = &mut f.key {
|
||||
if score < f.score {
|
||||
f.score = score;
|
||||
f.parent = Some(parent);
|
||||
q.decrease_key(h, |o| o.score = score);
|
||||
impl Display for Problem {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
let width_digits = self.map.width.ilog10() + 1;
|
||||
let height_digits = self.map.height.ilog10() + 1;
|
||||
|
||||
// print header
|
||||
for i in 0..width_digits {
|
||||
let d = width_digits - i - 1;
|
||||
// padding
|
||||
for _ in 0..height_digits {
|
||||
write!(f, " ")?;
|
||||
}
|
||||
|
||||
for x in 0..self.map.width {
|
||||
let digits = x / (usize::pow(10, d));
|
||||
if digits == 0 && d > 0 {
|
||||
write!(f, " ")?;
|
||||
} else {
|
||||
write!(f, "{}", char::from_u32((digits % 10) as u32 + 48).unwrap())?;
|
||||
}
|
||||
}
|
||||
|
||||
writeln!(f)?;
|
||||
}
|
||||
|
||||
// Print body
|
||||
|
||||
for y in 0..self.map.height {
|
||||
write!(f, "{:1$}", y, height_digits as usize)?;
|
||||
|
||||
for x in 0..self.map.width {
|
||||
if self.start == Position::new(x, y) {
|
||||
write!(f, "{}", "s".blue())?;
|
||||
} else if self.end == Position::new(x, y) {
|
||||
write!(f, "{}", "t".blue())?;
|
||||
} else if let Some(p) = self.path.iter().position(|p| p == &Position::new(x, y)) {
|
||||
if self.path[p].x < self.path[p + 1].x {
|
||||
write!(f, "{}", "→".blue())?;
|
||||
} else if self.path[p].x > self.path[p + 1].x {
|
||||
write!(f, "{}", "←".blue())?;
|
||||
} else if self.path[p].y < self.path[p + 1].y {
|
||||
write!(f, "{}", "↓".blue())?;
|
||||
} else if self.path[p].y > self.path[p + 1].y {
|
||||
write!(f, "{}", "↑".blue())?;
|
||||
}
|
||||
} else if self.map.get(x, y).blocked {
|
||||
write!(f, "{}", "#")?;
|
||||
} else {
|
||||
write!(f, " ")?;
|
||||
}
|
||||
}
|
||||
|
||||
writeln!(f)?;
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
struct MapInternal<'a> {
|
||||
map: &'a Map<Field>,
|
||||
}
|
||||
|
||||
impl<'a> MapInternal<'a> {
|
||||
fn get_direction(&self, pos: Position) -> [Option<Position>; 4] {
|
||||
[
|
||||
(pos.x > 0)
|
||||
.then_some((pos.x.saturating_sub(1), pos.y))
|
||||
.map(|(x, y)| Position::new(x, y)),
|
||||
(pos.x < self.map.width - 1)
|
||||
.then_some((pos.x + 1, pos.y))
|
||||
.map(|(x, y)| Position::new(x, y)),
|
||||
(pos.y > 0)
|
||||
.then_some((pos.x, pos.y.saturating_sub(1)))
|
||||
.map(|(x, y)| Position::new(x, y)),
|
||||
(pos.y < self.map.height - 1)
|
||||
.then_some((pos.x, pos.y + 1))
|
||||
.map(|(x, y)| Position::new(x, y)),
|
||||
]
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a> WheightedGraph for MapInternal<'a> {
|
||||
type Node = Position;
|
||||
|
||||
fn num_edges(&self, node: &Self::Node) -> usize {
|
||||
if self.map.get(node.x, node.y).blocked {
|
||||
0
|
||||
} else {
|
||||
let key = q.insert(QueueObject::new(Position { x, y }, score));
|
||||
internal_map.set(
|
||||
x,
|
||||
y,
|
||||
FieldInternal {
|
||||
score,
|
||||
marked: false,
|
||||
key: Some(key),
|
||||
parent: Some(parent),
|
||||
},
|
||||
)
|
||||
let t = self.get_direction(*node);
|
||||
|
||||
let v = t
|
||||
.iter()
|
||||
.flatten()
|
||||
.filter_map(|&p| (!self.map.get(p.x, p.y).blocked).then_some(p));
|
||||
|
||||
v.count()
|
||||
}
|
||||
}
|
||||
|
||||
fn edge(&self, node: &Self::Node, num: usize) -> Option<(Self::Node, f64)> {
|
||||
let t = self.get_direction(*node);
|
||||
|
||||
t.iter()
|
||||
.flatten()
|
||||
.filter_map(|&p| (!self.map.get(p.x, p.y).blocked).then_some(p))
|
||||
.nth(num)
|
||||
.map(|p| (p, 1.0))
|
||||
}
|
||||
}
|
||||
|
||||
pub fn find_path<P>(map: Map<Field>, start: Position, end: Position) -> Option<Vec<Position>>
|
||||
where
|
||||
P: PriorityQueue<QueueObject> + std::fmt::Debug,
|
||||
{
|
||||
let mut q = P::new();
|
||||
impl Problem {
|
||||
pub fn find_path(&mut self) {
|
||||
let m = MapInternal { map: &self.map };
|
||||
|
||||
let mut internal_map: Map<FieldInternal<P::Handle>> = Map::new(map.width, map.height);
|
||||
let p = dijkstra::<MapInternal, BinaryHeap<_>>(&m, self.start, self.end);
|
||||
|
||||
q.insert(QueueObject::new(start, 0));
|
||||
internal_map.get_mut(start.x, start.y).score = 0;
|
||||
|
||||
while let Some(o) = q.pop_min() {
|
||||
dbg!(&o);
|
||||
if o.pos.x > 0 && !map.get(o.pos.x - 1, o.pos.y).blocked {
|
||||
relax::<P>(
|
||||
&mut internal_map,
|
||||
&mut q,
|
||||
o.pos.x - 1,
|
||||
o.pos.y,
|
||||
o.score + 1,
|
||||
Direction::Right,
|
||||
);
|
||||
if let Some(p) = p {
|
||||
self.path = p;
|
||||
}
|
||||
|
||||
if o.pos.x < map.width - 1 && !map.get(o.pos.x + 1, o.pos.y).blocked {
|
||||
relax::<P>(
|
||||
&mut internal_map,
|
||||
&mut q,
|
||||
o.pos.x + 1,
|
||||
o.pos.y,
|
||||
o.score + 1,
|
||||
Direction::Left,
|
||||
);
|
||||
}
|
||||
|
||||
if o.pos.y > 0 && !map.get(o.pos.x, o.pos.y - 1).blocked {
|
||||
relax::<P>(
|
||||
&mut internal_map,
|
||||
&mut q,
|
||||
o.pos.x,
|
||||
o.pos.y - 1,
|
||||
o.score + 1,
|
||||
Direction::Down,
|
||||
);
|
||||
}
|
||||
|
||||
if o.pos.y < map.height - 1 && !map.get(o.pos.x, o.pos.y + 1).blocked {
|
||||
relax::<P>(
|
||||
&mut internal_map,
|
||||
&mut q,
|
||||
o.pos.x,
|
||||
o.pos.y + 1,
|
||||
o.score + 1,
|
||||
Direction::Up,
|
||||
);
|
||||
}
|
||||
// dbg!(&q);
|
||||
if o.pos == end {
|
||||
break;
|
||||
}
|
||||
internal_map.get_mut(o.pos.x, o.pos.y).marked = true;
|
||||
}
|
||||
|
||||
None
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue