1 files changed, 83 insertions, 0 deletions
diff --git a/server/src/customer/pathfinding.rs b/server/src/customer/pathfinding.rs
new file mode 100644
index 00000000..d25c6913
--- /dev/null
+++ b/server/src/customer/pathfinding.rs
@@ -0,0 +1,83 @@
+use super::movement::MovementBase;
+use crate::protocol::PacketS;
+use glam::{IVec2, Vec2};
+use log::debug;
+use std::{
+    cmp::Ordering,
+    collections::{BinaryHeap, HashMap, HashSet},
+};
+
+pub struct Path(Vec<Vec2>);
+
+impl Path {
+    pub fn execute_tick(
+        &mut self,
+        customer: &mut MovementBase,
+        walkable: &HashSet<IVec2>,
+        dt: f32,
+    ) -> PacketS {
+        if let Some(next) = self.0.last().copied() {
+            debug!("next {next}");
+            if next.distance(customer.position) < if self.0.len() == 1 { 0.1 } else { 0.6 } {
+                self.0.pop();
+            }
+            customer.update(&walkable, next - customer.position, dt)
+        } else {
+            customer.update(&walkable, Vec2::ZERO, dt)
+        }
+    }
+    pub fn is_done(&self) -> bool {
+        self.0.is_empty()
+    }
+}
+
+pub fn find_path(map: &HashSet<IVec2>, from: IVec2, to: IVec2) -> Option<Path> {
+    #[derive(Debug, PartialEq, Eq)]
+    struct Open(i32, IVec2, IVec2);
+    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));
+
+    loop {
+        let Some(Open(_, p, f)) = open.pop() else {
+            eprintln!("{visited:?}");
+            return None;
+        };
+        if visited.contains_key(&p) {
+            continue;
+        }
+        visited.insert(p, f);
+        if p == to {
+            break;
+        }
+        for d in [IVec2::NEG_X, IVec2::NEG_Y, IVec2::X, IVec2::Y] {
+            let n = p + d;
+            if map.contains(&n) {
+                open.push(Open(-d.distance_squared(to), n, p));
+            }
+        }
+    }
+
+    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))
+}