Initial commit

src/belt_finding/mod.rs

@@ -0,0 +1,170 @@
+use crate::{misc::Map, priority_queue::PriorityQueue};
+
+#[derive(Clone, Copy)]
+pub enum Direction {
+    Up,
+    Right,
+    Down,
+    Left,
+}
+
+#[derive(Clone, Copy, Debug, PartialEq, Eq)]
+pub struct Position {
+    pub x: usize,
+    pub y: usize,
+}
+
+#[derive(Default, Clone, Copy)]
+pub struct Field {
+    pub blocked: bool,
+}
+
+#[derive(Debug, Clone)]
+pub struct QueueObject {
+    pos: Position,
+    score: usize,
+}
+
+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 {
+        Self {
+            score: usize::MAX,
+            marked: false,
+            key: None,
+            parent: None,
+        }
+    }
+}
+
+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);
+            }
+        } 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),
+                },
+            )
+        }
+    }
+}
+
+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();
+
+    let mut internal_map: Map<FieldInternal<P::Handle>> = Map::new(map.width, map.height);
+
+    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 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
+}