From 069b61be6d132bc2d9935bb1f846da4dc3d8b5d5 Mon Sep 17 00:00:00 2001
From: metamuffin <metamuffin@disroot.org>
Date: Sun, 14 Jul 2024 11:32:02 +0200
Subject: fix pathfinding

---
 server/src/customer/pathfinding.rs | 25 +++++++++++++++----------
 server/src/lib.rs                  |  2 +-
 2 files changed, 16 insertions(+), 11 deletions(-)

(limited to 'server/src')

diff --git a/server/src/customer/pathfinding.rs b/server/src/customer/pathfinding.rs
index 2fe938a9..2b9f98f4 100644
--- a/server/src/customer/pathfinding.rs
+++ b/server/src/customer/pathfinding.rs
@@ -57,7 +57,7 @@ impl Path {
 
 pub fn find_path(walkable: &HashSet<IVec2>, from: IVec2, to: IVec2) -> Option<Path> {
     #[derive(Debug, PartialEq, Eq)]
-    struct Open(i32, IVec2, IVec2);
+    struct Open(i32, IVec2, IVec2, i32);
     impl PartialOrd for Open {
         fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
             self.0.partial_cmp(&other.0)
@@ -71,23 +71,28 @@ pub fn find_path(walkable: &HashSet<IVec2>, from: IVec2, to: IVec2) -> Option<Pa
 
     let mut visited = HashMap::new();
     let mut open = BinaryHeap::new();
-    open.push(Open(1, from, from));
+    open.push(Open(1, from, from, 0));
 
     loop {
-        let Some(Open(_, p, f)) = open.pop() else {
+        let Some(Open(_, pos, f, distance)) = open.pop() else {
             return None;
         };
-        if visited.contains_key(&p) {
+        if visited.contains_key(&pos) {
             continue;
         }
-        visited.insert(p, f);
-        if p == to {
+        visited.insert(pos, f);
+        if pos == to {
             break;
         }
-        for d in [IVec2::NEG_X, IVec2::NEG_Y, IVec2::X, IVec2::Y] {
-            let n = p + d;
-            if walkable.contains(&n) {
-                open.push(Open(-d.distance_squared(to), n, p));
+        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,
+                ));
             }
         }
     }
diff --git a/server/src/lib.rs b/server/src/lib.rs
index 6d0d4e26..afd6f1db 100644
--- a/server/src/lib.rs
+++ b/server/src/lib.rs
@@ -15,7 +15,7 @@
     along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
 */
-#![feature(if_let_guard, map_many_mut, let_chains, iterator_try_collect)]
+#![feature(if_let_guard, map_many_mut, let_chains, iterator_try_collect, isqrt)]
 pub mod customer;
 pub mod data;
 pub mod entity;
-- 
cgit v1.2.3-70-g09d2