flatland_pathfinding/
z_nav.rs

1//! Z-level helpers for outdoor pathfinding.
2
3use flatland_protocol::ZTransitionView;
4
5use crate::grid::NavWorld;
6
7pub const Z_LEVEL_TOLERANCE: f32 = 0.35;
8
9pub fn walkable_levels_at(world: &NavWorld, x: f32, y: f32) -> Vec<f32> {
10    let mut levels = vec![world.elevation_at(x, y)];
11    for p in &world.z_platforms {
12        if x >= p.x0 && x <= p.x1 && y >= p.y0 && y <= p.y1 {
13            levels.push(p.z);
14        }
15    }
16    for tr in &world.z_transitions {
17        if x >= tr.x0 && x <= tr.x1 && y >= tr.y0 && y <= tr.y1 {
18            levels.push(tr.z_from);
19            levels.push(tr.z_to);
20        }
21    }
22    levels.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
23    levels.dedup_by(|a, b| (*a - *b).abs() < Z_LEVEL_TOLERANCE);
24    levels
25}
26
27pub fn is_walkable_at_z(world: &NavWorld, x: f32, y: f32, z: f32) -> bool {
28    walkable_levels_at(world, x, y)
29        .iter()
30        .any(|&l| (l - z).abs() <= Z_LEVEL_TOLERANCE)
31}
32
33/// Like [`is_walkable_at_z`], but uses a precomputed ground elevation instead of
34/// scanning `terrain_zones` (critical for `build_grid`'s full-map z pass).
35pub fn is_walkable_at_z_with_ground(
36    world: &NavWorld,
37    x: f32,
38    y: f32,
39    z: f32,
40    ground_z: f32,
41) -> bool {
42    if (ground_z - z).abs() <= Z_LEVEL_TOLERANCE {
43        return true;
44    }
45    for p in &world.z_platforms {
46        if x >= p.x0 && x <= p.x1 && y >= p.y0 && y <= p.y1 && (p.z - z).abs() <= Z_LEVEL_TOLERANCE
47        {
48            return true;
49        }
50    }
51    for tr in &world.z_transitions {
52        if x >= tr.x0 && x <= tr.x1 && y >= tr.y0 && y <= tr.y1 {
53            if (tr.z_from - z).abs() <= Z_LEVEL_TOLERANCE
54                || (tr.z_to - z).abs() <= Z_LEVEL_TOLERANCE
55            {
56                return true;
57            }
58        }
59    }
60    false
61}
62
63fn nearest_level(levels: &[f32], current: f32) -> f32 {
64    levels
65        .iter()
66        .min_by(|a, b| {
67            (*a - current)
68                .abs()
69                .partial_cmp(&(*b - current).abs())
70                .unwrap_or(std::cmp::Ordering::Equal)
71        })
72        .copied()
73        .unwrap_or(current)
74}
75
76pub fn goal_z_for(world: &NavWorld, goal_x: f32, goal_y: f32, player_z: f32) -> f32 {
77    nearest_level(&walkable_levels_at(world, goal_x, goal_y), player_z)
78}
79
80fn transition_at<'a>(world: &'a NavWorld, x: f32, y: f32) -> Option<&'a ZTransitionView> {
81    world
82        .z_transitions
83        .iter()
84        .find(|t| x >= t.x0 && x <= t.x1 && y >= t.y0 && y <= t.y1)
85}
86
87trait ZTransitionExt {
88    fn z_min(&self) -> f32;
89    fn z_max(&self) -> f32;
90}
91
92impl ZTransitionExt for ZTransitionView {
93    fn z_min(&self) -> f32 {
94        self.z_from.min(self.z_to)
95    }
96
97    fn z_max(&self) -> f32 {
98        self.z_from.max(self.z_to)
99    }
100}
101
102pub fn cell_walkable_for_path(
103    world: &NavWorld,
104    x: f32,
105    y: f32,
106    player_z: f32,
107    goal_z: f32,
108) -> bool {
109    cell_walkable_for_path_with_ground(world, x, y, player_z, goal_z, world.elevation_at(x, y))
110}
111
112/// [`cell_walkable_for_path`] with caller-supplied ground z (avoids per-cell zone scans).
113pub fn cell_walkable_for_path_with_ground(
114    world: &NavWorld,
115    x: f32,
116    y: f32,
117    player_z: f32,
118    goal_z: f32,
119    ground_z: f32,
120) -> bool {
121    if is_walkable_at_z_with_ground(world, x, y, player_z, ground_z) {
122        return true;
123    }
124    if let Some(tr) = transition_at(world, x, y) {
125        let on_from = (player_z - tr.z_from).abs() <= Z_LEVEL_TOLERANCE;
126        let on_to = (player_z - tr.z_to).abs() <= Z_LEVEL_TOLERANCE;
127        let goal_on_from = (goal_z - tr.z_from).abs() <= Z_LEVEL_TOLERANCE;
128        let goal_on_to = (goal_z - tr.z_to).abs() <= Z_LEVEL_TOLERANCE;
129        if (on_from && goal_on_to) || (on_to && goal_on_from) {
130            return true;
131        }
132    }
133    false
134}
135
136/// Vertical intent while auto-walking (stairs).
137pub fn transition_vertical(world: &NavWorld, px: f32, py: f32, pz: f32, goal_z: f32) -> f32 {
138    let Some(tr) = transition_at(world, px, py) else {
139        return 0.0;
140    };
141    if (pz - goal_z).abs() <= Z_LEVEL_TOLERANCE {
142        return 0.0;
143    }
144    if goal_z > pz + Z_LEVEL_TOLERANCE
145        && (pz - tr.z_from).abs() <= Z_LEVEL_TOLERANCE
146        && tr.z_to > tr.z_from
147    {
148        return 1.0;
149    }
150    if goal_z < pz - Z_LEVEL_TOLERANCE
151        && (pz - tr.z_to).abs() <= Z_LEVEL_TOLERANCE
152        && tr.z_to > tr.z_from
153    {
154        return -1.0;
155    }
156    if goal_z > pz + Z_LEVEL_TOLERANCE && pz <= tr.z_min() + Z_LEVEL_TOLERANCE {
157        return 1.0;
158    }
159    if goal_z < pz - Z_LEVEL_TOLERANCE && pz >= tr.z_max() - Z_LEVEL_TOLERANCE {
160        return -1.0;
161    }
162    0.0
163}
flatland_pathfinding/z_nav.rs

flatland_pathfinding/
z_nav.rs
