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 || (tr.z_to - z).abs() <= Z_LEVEL_TOLERANCE
54            {
55                return true;
56            }
57        }
58    }
59    false
60}
61
62fn nearest_level(levels: &[f32], current: f32) -> f32 {
63    levels
64        .iter()
65        .min_by(|a, b| {
66            (*a - current)
67                .abs()
68                .partial_cmp(&(*b - current).abs())
69                .unwrap_or(std::cmp::Ordering::Equal)
70        })
71        .copied()
72        .unwrap_or(current)
73}
74
75pub fn goal_z_for(world: &NavWorld, goal_x: f32, goal_y: f32, player_z: f32) -> f32 {
76    nearest_level(&walkable_levels_at(world, goal_x, goal_y), player_z)
77}
78
79fn transition_at<'a>(world: &'a NavWorld, x: f32, y: f32) -> Option<&'a ZTransitionView> {
80    world
81        .z_transitions
82        .iter()
83        .find(|t| x >= t.x0 && x <= t.x1 && y >= t.y0 && y <= t.y1)
84}
85
86trait ZTransitionExt {
87    fn z_min(&self) -> f32;
88    fn z_max(&self) -> f32;
89}
90
91impl ZTransitionExt for ZTransitionView {
92    fn z_min(&self) -> f32 {
93        self.z_from.min(self.z_to)
94    }
95
96    fn z_max(&self) -> f32 {
97        self.z_from.max(self.z_to)
98    }
99}
100
101pub fn cell_walkable_for_path(
102    world: &NavWorld,
103    x: f32,
104    y: f32,
105    player_z: f32,
106    goal_z: f32,
107) -> bool {
108    cell_walkable_for_path_with_ground(
109        world,
110        x,
111        y,
112        player_z,
113        goal_z,
114        world.elevation_at(x, y),
115    )
116}
117
118/// [`cell_walkable_for_path`] with caller-supplied ground z (avoids per-cell zone scans).
119pub fn cell_walkable_for_path_with_ground(
120    world: &NavWorld,
121    x: f32,
122    y: f32,
123    player_z: f32,
124    goal_z: f32,
125    ground_z: f32,
126) -> bool {
127    if is_walkable_at_z_with_ground(world, x, y, player_z, ground_z) {
128        return true;
129    }
130    if let Some(tr) = transition_at(world, x, y) {
131        let on_from = (player_z - tr.z_from).abs() <= Z_LEVEL_TOLERANCE;
132        let on_to = (player_z - tr.z_to).abs() <= Z_LEVEL_TOLERANCE;
133        let goal_on_from = (goal_z - tr.z_from).abs() <= Z_LEVEL_TOLERANCE;
134        let goal_on_to = (goal_z - tr.z_to).abs() <= Z_LEVEL_TOLERANCE;
135        if (on_from && goal_on_to) || (on_to && goal_on_from) {
136            return true;
137        }
138    }
139    false
140}
141
142/// Vertical intent while auto-walking (stairs).
143pub fn transition_vertical(world: &NavWorld, px: f32, py: f32, pz: f32, goal_z: f32) -> f32 {
144    let Some(tr) = transition_at(world, px, py) else {
145        return 0.0;
146    };
147    if (pz - goal_z).abs() <= Z_LEVEL_TOLERANCE {
148        return 0.0;
149    }
150    if goal_z > pz + Z_LEVEL_TOLERANCE
151        && (pz - tr.z_from).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
157        && (pz - tr.z_to).abs() <= Z_LEVEL_TOLERANCE
158        && tr.z_to > tr.z_from
159    {
160        return -1.0;
161    }
162    if goal_z > pz + Z_LEVEL_TOLERANCE && pz <= tr.z_min() + Z_LEVEL_TOLERANCE {
163        return 1.0;
164    }
165    if goal_z < pz - Z_LEVEL_TOLERANCE && pz >= tr.z_max() - Z_LEVEL_TOLERANCE {
166        return -1.0;
167    }
168    0.0
169}
flatland_pathfinding/z_nav.rs

flatland_pathfinding/
z_nav.rs
