flatland_pathfinding/
session.rs

1//! Path session — follow waypoints toward a goal (client auto-nav + worker travel steps).
2
3use crate::grid::NavWorld;
4use crate::mode::PathMode;
5use crate::path::find_path_with_goal_z;
6use crate::z_nav::{goal_z_for, transition_vertical, Z_LEVEL_TOLERANCE};
7
8pub const WAYPOINT_RADIUS_M: f32 = 0.85;
9pub const ARRIVE_RADIUS_M: f32 = 0.75;
10const SPRINT_LEG_M: f32 = 4.0;
11const STUCK_TICKS_BEFORE_REPLAN: u32 = 6;
12const MAX_REPLAN_ATTEMPTS: u32 = 5;
13
14/// Dexterity contribution to move speed (display DEX 1–100 scale).
15pub const DEX_MOVE_COEFF: f32 = 0.003;
16
17/// Effective horizontal speed (m/s) for travel ETA and worker movement.
18pub fn effective_move_speed_mps(base_speed: f32, dex_display: f32, encumbrance_mult: f32) -> f32 {
19    base_speed * encumbrance_mult * (1.0 + DEX_MOVE_COEFF * dex_display.max(0.0))
20}
21
22#[derive(Debug, Clone, Copy, PartialEq)]
23pub struct PathSteer {
24    pub forward: f32,
25    pub strafe: f32,
26    pub vertical: f32,
27    pub sprint: bool,
28}
29
30#[derive(Debug, Clone)]
31pub struct PathSession {
32    waypoints: Vec<(f32, f32)>,
33    waypoint_index: usize,
34    pub goal_x: f32,
35    pub goal_y: f32,
36    pub goal_z: f32,
37    pub mode: PathMode,
38    last_progress_x: f32,
39    last_progress_y: f32,
40    stuck_ticks: u32,
41    replan_attempts: u32,
42}
43
44impl PathSession {
45    pub fn plan(
46        world: &NavWorld,
47        from_x: f32,
48        from_y: f32,
49        from_z: f32,
50        goal_x: f32,
51        goal_y: f32,
52        mode: PathMode,
53    ) -> Option<Self> {
54        let goal_z = goal_z_for(world, goal_x, goal_y, from_z);
55        Self::plan_with_goal_z(world, from_x, from_y, from_z, goal_x, goal_y, goal_z, mode)
56    }
57
58    pub fn plan_with_goal_z(
59        world: &NavWorld,
60        from_x: f32,
61        from_y: f32,
62        from_z: f32,
63        goal_x: f32,
64        goal_y: f32,
65        goal_z: f32,
66        mode: PathMode,
67    ) -> Option<Self> {
68        let path =
69            find_path_with_goal_z(world, from_x, from_y, from_z, goal_x, goal_y, goal_z, mode)?;
70        if path.is_empty() {
71            return None;
72        }
73        // Final steer must not chase the raw click/container coord through walls —
74        // A* may snap the walkable cell; follow the path terminus instead.
75        let (approach_x, approach_y) = path.last().copied().unwrap_or((goal_x, goal_y));
76        Some(Self {
77            waypoints: path,
78            waypoint_index: 0,
79            goal_x: approach_x,
80            goal_y: approach_y,
81            goal_z,
82            mode,
83            last_progress_x: from_x,
84            last_progress_y: from_y,
85            stuck_ticks: 0,
86            replan_attempts: 0,
87        })
88    }
89
90    /// Single-waypoint steer (interior rooms / pocket houses). Collision handles walls.
91    pub fn direct_to(from_x: f32, from_y: f32, goal_x: f32, goal_y: f32, goal_z: f32) -> Self {
92        Self {
93            waypoints: vec![(goal_x, goal_y)],
94            waypoint_index: 0,
95            goal_x,
96            goal_y,
97            goal_z,
98            mode: PathMode::Fastest,
99            last_progress_x: from_x,
100            last_progress_y: from_y,
101            stuck_ticks: 0,
102            replan_attempts: 0,
103        }
104    }
105
106    pub fn active(&self) -> bool {
107        self.waypoint_index < self.waypoints.len()
108    }
109
110    pub fn waypoints(&self) -> &[(f32, f32)] {
111        &self.waypoints
112    }
113
114    pub fn replan(&mut self, world: &NavWorld, px: f32, py: f32, pz: f32) -> bool {
115        if let Some(path) = find_path_with_goal_z(
116            world,
117            px,
118            py,
119            pz,
120            self.goal_x,
121            self.goal_y,
122            self.goal_z,
123            self.mode,
124        ) {
125            if !path.is_empty() {
126                let changed = path != self.waypoints;
127                self.waypoints = path;
128                self.waypoint_index = 0;
129                self.stuck_ticks = 0;
130                self.last_progress_x = px;
131                self.last_progress_y = py;
132                if changed {
133                    self.replan_attempts = 0;
134                } else {
135                    self.replan_attempts = self.replan_attempts.saturating_add(1);
136                }
137                return self.replan_attempts < MAX_REPLAN_ATTEMPTS;
138            }
139        }
140        self.replan_attempts = self.replan_attempts.saturating_add(1);
141        self.replan_attempts < MAX_REPLAN_ATTEMPTS
142    }
143
144    /// True when `px, py` is far enough from the last counted stand to reset stuck.
145    pub fn would_count_progress(&self, px: f32, py: f32) -> bool {
146        (px - self.last_progress_x).hypot(py - self.last_progress_y) > 0.25
147    }
148
149    pub fn note_progress(&mut self, px: f32, py: f32) -> bool {
150        if self.would_count_progress(px, py) {
151            self.last_progress_x = px;
152            self.last_progress_y = py;
153            self.stuck_ticks = 0;
154            self.replan_attempts = 0;
155            return false;
156        }
157        self.stuck_ticks = self.stuck_ticks.saturating_add(1);
158        self.stuck_ticks >= STUCK_TICKS_BEFORE_REPLAN
159    }
160
161    pub fn steer(&mut self, px: f32, py: f32, pz: f32, world: &NavWorld) -> Option<PathSteer> {
162        let vertical_climb = transition_vertical(world, px, py, pz, self.goal_z);
163        if vertical_climb.abs() > f32::EPSILON {
164            return Some(PathSteer {
165                forward: 0.0,
166                strafe: 0.0,
167                vertical: vertical_climb,
168                sprint: false,
169            });
170        }
171        while self.waypoint_index < self.waypoints.len() {
172            let (wx, wy) = self.waypoints[self.waypoint_index];
173            let dx = wx - px;
174            let dy = wy - py;
175            let dist = (dx * dx + dy * dy).sqrt();
176            if dist < WAYPOINT_RADIUS_M {
177                self.waypoint_index += 1;
178                continue;
179            }
180            let forward = (dy / dist).clamp(-1.0, 1.0);
181            let strafe = (dx / dist).clamp(-1.0, 1.0);
182            let sprint = dist > SPRINT_LEG_M;
183            return Some(PathSteer {
184                forward,
185                strafe,
186                vertical: 0.0,
187                sprint,
188            });
189        }
190        let goal_dist = (self.goal_x - px).hypot(self.goal_y - py);
191        if goal_dist > ARRIVE_RADIUS_M || (pz - self.goal_z).abs() > Z_LEVEL_TOLERANCE {
192            if goal_dist > ARRIVE_RADIUS_M {
193                let forward = ((self.goal_y - py) / goal_dist).clamp(-1.0, 1.0);
194                let strafe = ((self.goal_x - px) / goal_dist).clamp(-1.0, 1.0);
195                return Some(PathSteer {
196                    forward,
197                    strafe,
198                    vertical: 0.0,
199                    sprint: goal_dist > SPRINT_LEG_M,
200                });
201            }
202            let vz = transition_vertical(world, px, py, pz, self.goal_z);
203            if vz.abs() > f32::EPSILON {
204                return Some(PathSteer {
205                    forward: 0.0,
206                    strafe: 0.0,
207                    vertical: vz,
208                    sprint: false,
209                });
210            }
211        }
212        None
213    }
214
215    /// Remaining path length in meters (rough estimate).
216    pub fn remaining_distance_m(&self, px: f32, py: f32) -> f32 {
217        let mut total = 0.0;
218        let mut last = (px, py);
219        if self.waypoint_index < self.waypoints.len() {
220            for &(wx, wy) in &self.waypoints[self.waypoint_index..] {
221                total += (wx - last.0).hypot(wy - last.1);
222                last = (wx, wy);
223            }
224        }
225        total + (self.goal_x - last.0).hypot(self.goal_y - last.1)
226    }
227}
228
229#[cfg(test)]
230mod tests {
231    use super::*;
232    use crate::grid::NavWorld;
233
234    fn open_world() -> NavWorld {
235        NavWorld::new(
236            64.0,
237            64.0,
238            vec![],
239            vec![],
240            vec![],
241            vec![],
242            vec![],
243            vec![],
244            crate::grid::TerrainNavTable::unit_test_defaults(),
245        )
246    }
247
248    #[test]
249    fn replan_gives_up_when_goal_becomes_unreachable() {
250        let world = open_world();
251        let mut session = PathSession::plan_with_goal_z(
252            &world,
253            10.0,
254            10.0,
255            0.0,
256            20.0,
257            10.0,
258            0.0,
259            PathMode::Fastest,
260        )
261        .expect("path");
262        // Wall off the goal after the first plan.
263        let mut blocked = open_world();
264        blocked
265            .terrain_zones
266            .push(flatland_protocol::TerrainZoneView {
267                id: "wall".into(),
268                x0: 14.0,
269                y0: 0.0,
270                x1: 18.0,
271                y1: 64.0,
272                kind: flatland_protocol::TerrainKindView::DeepWater,
273                elevation: 0.0,
274                glyph: None,
275                color: None,
276                tile_id: None,
277                z_order: 0,
278                channel_start_tick: None,
279                channel_end_tick: None,
280            });
281        let mut gave_up = false;
282        for _ in 0..MAX_REPLAN_ATTEMPTS + 2 {
283            if !session.replan(&blocked, 10.0, 10.0, 0.0) {
284                gave_up = true;
285                break;
286            }
287        }
288        assert!(
289            gave_up,
290            "replan must eventually return false for unreachable goals"
291        );
292    }
293}
flatland_pathfinding/session.rs

flatland_pathfinding/
session.rs
