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    pub fn note_progress(&mut self, px: f32, py: f32) -> bool {
145        if (px - self.last_progress_x).hypot(py - self.last_progress_y) > 0.25 {
146            self.last_progress_x = px;
147            self.last_progress_y = py;
148            self.stuck_ticks = 0;
149            self.replan_attempts = 0;
150            return false;
151        }
152        self.stuck_ticks = self.stuck_ticks.saturating_add(1);
153        self.stuck_ticks >= STUCK_TICKS_BEFORE_REPLAN
154    }
155
156    pub fn steer(&mut self, px: f32, py: f32, pz: f32, world: &NavWorld) -> Option<PathSteer> {
157        let vertical_climb = transition_vertical(world, px, py, pz, self.goal_z);
158        if vertical_climb.abs() > f32::EPSILON {
159            return Some(PathSteer {
160                forward: 0.0,
161                strafe: 0.0,
162                vertical: vertical_climb,
163                sprint: false,
164            });
165        }
166        while self.waypoint_index < self.waypoints.len() {
167            let (wx, wy) = self.waypoints[self.waypoint_index];
168            let dx = wx - px;
169            let dy = wy - py;
170            let dist = (dx * dx + dy * dy).sqrt();
171            if dist < WAYPOINT_RADIUS_M {
172                self.waypoint_index += 1;
173                continue;
174            }
175            let forward = (dy / dist).clamp(-1.0, 1.0);
176            let strafe = (dx / dist).clamp(-1.0, 1.0);
177            let sprint = dist > SPRINT_LEG_M;
178            return Some(PathSteer {
179                forward,
180                strafe,
181                vertical: 0.0,
182                sprint,
183            });
184        }
185        let goal_dist = (self.goal_x - px).hypot(self.goal_y - py);
186        if goal_dist > ARRIVE_RADIUS_M || (pz - self.goal_z).abs() > Z_LEVEL_TOLERANCE {
187            if goal_dist > ARRIVE_RADIUS_M {
188                let forward = ((self.goal_y - py) / goal_dist).clamp(-1.0, 1.0);
189                let strafe = ((self.goal_x - px) / goal_dist).clamp(-1.0, 1.0);
190                return Some(PathSteer {
191                    forward,
192                    strafe,
193                    vertical: 0.0,
194                    sprint: goal_dist > SPRINT_LEG_M,
195                });
196            }
197            let vz = transition_vertical(world, px, py, pz, self.goal_z);
198            if vz.abs() > f32::EPSILON {
199                return Some(PathSteer {
200                    forward: 0.0,
201                    strafe: 0.0,
202                    vertical: vz,
203                    sprint: false,
204                });
205            }
206        }
207        None
208    }
209
210    /// Remaining path length in meters (rough estimate).
211    pub fn remaining_distance_m(&self, px: f32, py: f32) -> f32 {
212        let mut total = 0.0;
213        let mut last = (px, py);
214        if self.waypoint_index < self.waypoints.len() {
215            for &(wx, wy) in &self.waypoints[self.waypoint_index..] {
216                total += (wx - last.0).hypot(wy - last.1);
217                last = (wx, wy);
218            }
219        }
220        total + (self.goal_x - last.0).hypot(self.goal_y - last.1)
221    }
222}
223
224#[cfg(test)]
225mod tests {
226    use super::*;
227    use crate::grid::NavWorld;
228
229    fn open_world() -> NavWorld {
230        NavWorld::new(
231            64.0,
232            64.0,
233            vec![],
234            vec![],
235            vec![],
236            vec![],
237            vec![],
238            vec![],
239            crate::grid::TerrainNavTable::unit_test_defaults(),
240        )
241    }
242
243    #[test]
244    fn replan_gives_up_when_goal_becomes_unreachable() {
245        let world = open_world();
246        let mut session = PathSession::plan_with_goal_z(
247            &world,
248            10.0,
249            10.0,
250            0.0,
251            20.0,
252            10.0,
253            0.0,
254            PathMode::Fastest,
255        )
256        .expect("path");
257        // Wall off the goal after the first plan.
258        let mut blocked = open_world();
259        blocked.terrain_zones.push(flatland_protocol::TerrainZoneView {
260            id: "wall".into(),
261            x0: 14.0,
262            y0: 0.0,
263            x1: 18.0,
264            y1: 64.0,
265            kind: flatland_protocol::TerrainKindView::DeepWater,
266            elevation: 0.0,
267            glyph: None,
268            color: None,
269            tile_id: None,
270            z_order: 0,
271            channel_start_tick: None,
272            channel_end_tick: None,
273        });
274        let mut gave_up = false;
275        for _ in 0..MAX_REPLAN_ATTEMPTS + 2 {
276            if !session.replan(&blocked, 10.0, 10.0, 0.0) {
277                gave_up = true;
278                break;
279            }
280        }
281        assert!(gave_up, "replan must eventually return false for unreachable goals");
282    }
283}
flatland_pathfinding/session.rs

flatland_pathfinding/
session.rs
