customer pathfinding

1 files changed, 43 insertions, 26 deletions

diff --git a/server/src/customer.rs b/server/src/customer.rs
index 8d843fda..403b891e 100644
--- a/server/src/customer.rs
+++ b/server/src/customer.rs
@@ -4,10 +4,10 @@ use crate::{
     protocol::{PacketC, PacketS, PlayerID},
 };
 use glam::{IVec2, Vec2};
-use log::error;
+use log::{error, info};
 use std::{
     cmp::Ordering,
-    collections::{BinaryHeap, HashMap, HashSet, VecDeque},
+    collections::{BinaryHeap, HashMap, HashSet},
     sync::Arc,
     time::Duration,
 };
@@ -24,7 +24,7 @@ struct DemandState {
 }
 
 enum CustomerState {
-    WalkingToChair { target: Vec2 },
+    WalkingToChair { path: Vec<Vec2> },
     Waiting,
 }
 
@@ -84,7 +84,6 @@ pub async fn customer(game: Arc<RwLock<Game>>, mut grx: broadcast::Receiver<Pack
     }
 }
 
-impl DemandState {}
 impl DemandState {
     pub fn tick(&mut self, packets_out: &mut Vec<(PlayerID, PacketS)>, dt: f32) {
         if self.customers.is_empty() {
@@ -95,25 +94,32 @@ impl DemandState {
                     character: 0,
                 },
             ));
+            let path = find_path(
+                &self.walkable,
+                self.data.customer_spawn.as_ivec2(),
+                IVec2::new(20, 1),
+            )
+            .expect("no path");
             self.customers.push(Customer {
                 id: -1,
                 position: self.data.customer_spawn,
                 facing: Vec2::X,
                 vel: Vec2::ZERO,
-                state: CustomerState::WalkingToChair {
-                    target: Vec2::new(2., 2.),
-                },
+                state: CustomerState::WalkingToChair { path },
             });
         }
 
         for p in &mut self.customers {
-            match p.state {
-                CustomerState::WalkingToChair { target } => {
-                    packets_out.push((
-                        p.id,
-                        move_player(p, &self.walkable, target - p.position, dt),
-                    ));
-                    if target.distance(p.position) < 0.5 {
+            match &mut p.state {
+                CustomerState::WalkingToChair { path } => {
+                    if let Some(next) = path.last().copied() {
+                        info!("next {next}");
+                        if next.distance(p.position) < 0.6 {
+                            path.pop();
+                        }
+                        packets_out
+                            .push((p.id, move_player(p, &self.walkable, next - p.position, dt)));
+                    } else {
                         p.state = CustomerState::Waiting;
                     }
                 }
@@ -123,7 +129,7 @@ impl DemandState {
     }
 }
 
-pub fn find_path(map: &HashSet<IVec2>, from: IVec2, to: IVec2) -> VecDeque<IVec2> {
+pub fn find_path(map: &HashSet<IVec2>, from: IVec2, to: IVec2) -> Option<Vec<Vec2>> {
     #[derive(Debug, PartialEq, Eq)]
     struct Open(i32, IVec2, IVec2);
     impl PartialOrd for Open {
@@ -141,8 +147,12 @@ pub fn find_path(map: &HashSet<IVec2>, from: IVec2, to: IVec2) -> VecDeque<IVec2
     let mut open = BinaryHeap::new();
     open.push(Open(1, from, from));
 
-    while let Some(Open(_, p, f)) = open.pop() {
-        if !visited.contains_key(&p) {
+    loop {
+        let Some(Open(_, p, f)) = open.pop() else {
+            eprintln!("{visited:?}");
+            return None;
+        };
+        if visited.contains_key(&p) {
             continue;
         }
         visited.insert(p, f);
@@ -150,22 +160,29 @@ pub fn find_path(map: &HashSet<IVec2>, from: IVec2, to: IVec2) -> VecDeque<IVec2
             break;
         }
         for d in [IVec2::NEG_X, IVec2::NEG_Y, IVec2::X, IVec2::Y] {
-            open.push(Open(d.distance_squared(to), p + d, p));
+            let n = p + d;
+            if map.contains(&n) {
+                open.push(Open(-d.distance_squared(to), n, p));
+            }
         }
     }
 
-    let mut path = VecDeque::new();
-    path.push_back(to);
-
-    // loop {
-    //     path.
-    // }
+    let mut path = Vec::new();
+    let mut c = to;
+    loop {
+        let cn = visited[&c];
+        path.push(cn.as_vec2() + 0.5);
+        if cn == c {
+            break;
+        }
+        c = cn
+    }
 
-    path
+    Some(path)
 }
 
 fn move_player(p: &mut Customer, map: &HashSet<IVec2>, direction: Vec2, dt: f32) -> PacketS {
-    let direction = direction.normalize();
+    let direction = direction.normalize_or_zero();
     if direction.length() > 0.1 {
         p.facing = direction + (p.facing - direction) * (-dt * 10.).exp();
     }