flatland-pathfinding 0.2.32

Shared grid A* pathfinding for Flatland3 clients and sim
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121

//! Z-level helpers for outdoor pathfinding.

use flatland_protocol::ZTransitionView;

use crate::grid::NavWorld;

pub const Z_LEVEL_TOLERANCE: f32 = 0.35;

pub fn walkable_levels_at(world: &NavWorld, x: f32, y: f32) -> Vec<f32> {
    let mut levels = vec![world.elevation_at(x, y)];
    for p in &world.z_platforms {
        if x >= p.x0 && x <= p.x1 && y >= p.y0 && y <= p.y1 {
            levels.push(p.z);
        }
    }
    for tr in &world.z_transitions {
        if x >= tr.x0 && x <= tr.x1 && y >= tr.y0 && y <= tr.y1 {
            levels.push(tr.z_from);
            levels.push(tr.z_to);
        }
    }
    levels.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
    levels.dedup_by(|a, b| (*a - *b).abs() < Z_LEVEL_TOLERANCE);
    levels
}

pub fn is_walkable_at_z(world: &NavWorld, x: f32, y: f32, z: f32) -> bool {
    walkable_levels_at(world, x, y)
        .iter()
        .any(|&l| (l - z).abs() <= Z_LEVEL_TOLERANCE)
}

fn nearest_level(levels: &[f32], current: f32) -> f32 {
    levels
        .iter()
        .min_by(|a, b| {
            (*a - current)
                .abs()
                .partial_cmp(&(*b - current).abs())
                .unwrap_or(std::cmp::Ordering::Equal)
        })
        .copied()
        .unwrap_or(current)
}

pub fn goal_z_for(world: &NavWorld, goal_x: f32, goal_y: f32, player_z: f32) -> f32 {
    nearest_level(&walkable_levels_at(world, goal_x, goal_y), player_z)
}

fn transition_at<'a>(world: &'a NavWorld, x: f32, y: f32) -> Option<&'a ZTransitionView> {
    world
        .z_transitions
        .iter()
        .find(|t| x >= t.x0 && x <= t.x1 && y >= t.y0 && y <= t.y1)
}

trait ZTransitionExt {
    fn z_min(&self) -> f32;
    fn z_max(&self) -> f32;
}

impl ZTransitionExt for ZTransitionView {
    fn z_min(&self) -> f32 {
        self.z_from.min(self.z_to)
    }

    fn z_max(&self) -> f32 {
        self.z_from.max(self.z_to)
    }
}

pub fn cell_walkable_for_path(
    world: &NavWorld,
    x: f32,
    y: f32,
    player_z: f32,
    goal_z: f32,
) -> bool {
    if is_walkable_at_z(world, x, y, player_z) {
        return true;
    }
    if let Some(tr) = transition_at(world, x, y) {
        let on_from = (player_z - tr.z_from).abs() <= Z_LEVEL_TOLERANCE;
        let on_to = (player_z - tr.z_to).abs() <= Z_LEVEL_TOLERANCE;
        let goal_on_from = (goal_z - tr.z_from).abs() <= Z_LEVEL_TOLERANCE;
        let goal_on_to = (goal_z - tr.z_to).abs() <= Z_LEVEL_TOLERANCE;
        if (on_from && goal_on_to) || (on_to && goal_on_from) {
            return true;
        }
    }
    false
}

/// Vertical intent while auto-walking (stairs).
pub fn transition_vertical(world: &NavWorld, px: f32, py: f32, pz: f32, goal_z: f32) -> f32 {
    let Some(tr) = transition_at(world, px, py) else {
        return 0.0;
    };
    if (pz - goal_z).abs() <= Z_LEVEL_TOLERANCE {
        return 0.0;
    }
    if goal_z > pz + Z_LEVEL_TOLERANCE
        && (pz - tr.z_from).abs() <= Z_LEVEL_TOLERANCE
        && tr.z_to > tr.z_from
    {
        return 1.0;
    }
    if goal_z < pz - Z_LEVEL_TOLERANCE
        && (pz - tr.z_to).abs() <= Z_LEVEL_TOLERANCE
        && tr.z_to > tr.z_from
    {
        return -1.0;
    }
    if goal_z > pz + Z_LEVEL_TOLERANCE && pz <= tr.z_min() + Z_LEVEL_TOLERANCE {
        return 1.0;
    }
    if goal_z < pz - Z_LEVEL_TOLERANCE && pz >= tr.z_max() - Z_LEVEL_TOLERANCE {
        return -1.0;
    }
    0.0
}