1 files changed, 98 insertions, 0 deletions
diff --git a/server/src/entity/customers/pathfinding.rs b/server/src/entity/customers/pathfinding.rs
new file mode 100644
index 00000000..97bd8328
--- /dev/null
+++ b/server/src/entity/customers/pathfinding.rs
@@ -0,0 +1,98 @@
+/*
+    Hurry Curry! - a game about cooking
+    Copyright 2024 metamuffin
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU Affero General Public License as published by
+    the Free Software Foundation, version 3 of the License only.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU Affero General Public License for more details.
+
+    You should have received a copy of the GNU Affero General Public License
+    along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+*/
+use hurrycurry_protocol::glam::{IVec2, Vec2};
+use log::trace;
+use std::{
+    cmp::Ordering,
+    collections::{BinaryHeap, HashMap, HashSet},
+};
+
+#[derive(Debug, Clone)]
+pub struct Path(Vec<Vec2>);
+
+impl Path {
+    pub fn next_direction(&mut self, position: Vec2) -> Vec2 {
+        if let Some(next) = self.0.last().copied() {
+            trace!("next {next}");
+            if next.distance(position) < if self.0.len() == 1 { 0.1 } else { 0.6 } {
+                self.0.pop();
+            }
+            (next - position).normalize_or_zero() * 0.5
+        } else {
+            Vec2::ZERO
+        }
+    }
+    pub fn is_done(&self) -> bool {
+        self.0.is_empty()
+    }
+}
+
+pub fn find_path(walkable: &HashSet<IVec2>, from: IVec2, to: IVec2) -> Option<Path> {
+    #[derive(Debug, PartialEq, Eq)]
+    struct Open(i32, IVec2, IVec2, i32);
+    impl PartialOrd for Open {
+        fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+            self.0.partial_cmp(&other.0)
+        }
+    }
+    impl Ord for Open {
+        fn cmp(&self, other: &Self) -> Ordering {
+            self.0.cmp(&other.0)
+        }
+    }
+
+    let mut visited = HashMap::new();
+    let mut open = BinaryHeap::new();
+    open.push(Open(1, from, from, 0));
+
+    loop {
+        let Some(Open(_, pos, f, distance)) = open.pop() else {
+            return None;
+        };
+        if visited.contains_key(&pos) {
+            continue;
+        }
+        visited.insert(pos, f);
+        if pos == to {
+            break;
+        }
+        for dir in [IVec2::NEG_X, IVec2::NEG_Y, IVec2::X, IVec2::Y] {
+            let next = pos + dir;
+            if walkable.contains(&next) {
+                open.push(Open(
+                    -(distance + next.distance_squared(to).isqrt()),
+                    next,
+                    pos,
+                    distance + 1,
+                ));
+            }
+        }
+    }
+
+    let mut path = Vec::new();
+    let mut c = to;
+    loop {
+        path.push(c.as_vec2() + 0.5);
+        let cn = visited[&c];
+        if cn == c {
+            break;
+        }
+        c = cn
+    }
+    Some(Path(path))
+}