vimp_engine_core/nav/
pathfinder.rs

1use std::collections::HashMap;
2
3/// Ребро навигационного графа.
4#[derive(Clone, Copy, serde::Serialize, serde::Deserialize)]
5pub struct Edge {
6    pub node: usize,
7    pub weight: f32,
8}
9
10fn heuristic(a: [f32; 2], b: [f32; 2]) -> f32 {
11    let dx = a[0] - b[0];
12    let dy = a[1] - b[1];
13
14    (dx * dx + dy * dy).sqrt()
15}
16
17/// A* по графу (порт src/server/modules/bots/Pathfinder.js).
18/// Возвращает индексы узлов пути или None.
19pub fn find_path(
20    start_node: usize,
21    end_node: usize,
22    nodes: &[[f32; 2]],
23    edges: &[Vec<Edge>],
24) -> Option<Vec<usize>> {
25    let mut open_set = vec![start_node];
26    let mut came_from: HashMap<usize, usize> = HashMap::new();
27
28    let mut g_score: HashMap<usize, f32> = HashMap::new();
29    let mut f_score: HashMap<usize, f32> = HashMap::new();
30
31    g_score.insert(start_node, 0.0);
32    f_score.insert(start_node, heuristic(nodes[start_node], nodes[end_node]));
33
34    while !open_set.is_empty() {
35        // узел с наименьшим fScore
36        let mut current = open_set[0];
37
38        for &candidate in open_set.iter().skip(1) {
39            if f_score.get(&candidate).copied().unwrap_or(f32::INFINITY)
40                < f_score.get(&current).copied().unwrap_or(f32::INFINITY)
41            {
42                current = candidate;
43            }
44        }
45
46        if current == end_node {
47            return Some(reconstruct_path(&came_from, current));
48        }
49
50        let index = open_set.iter().position(|&n| n == current).unwrap();
51
52        open_set.remove(index);
53
54        for edge in &edges[current] {
55            let tentative = g_score[&current] + edge.weight;
56            // отсутствующий gScore — бесконечность (см. комментарий в JS-версии
57            // о ловушке `|| Infinity` для нулевого значения)
58            let neighbor_score = g_score.get(&edge.node).copied().unwrap_or(f32::INFINITY);
59
60            if tentative < neighbor_score {
61                came_from.insert(edge.node, current);
62                g_score.insert(edge.node, tentative);
63                f_score.insert(
64                    edge.node,
65                    tentative + heuristic(nodes[edge.node], nodes[end_node]),
66                );
67
68                if !open_set.contains(&edge.node) {
69                    open_set.push(edge.node);
70                }
71            }
72        }
73    }
74
75    None
76}
77
78fn reconstruct_path(came_from: &HashMap<usize, usize>, mut current: usize) -> Vec<usize> {
79    let mut total_path = vec![current];
80
81    while let Some(&previous) = came_from.get(&current) {
82        current = previous;
83        total_path.insert(0, current);
84    }
85
86    total_path
87}
88
89#[cfg(test)]
90mod tests {
91    use super::*;
92
93    #[test]
94    fn finds_shortest_path_in_simple_graph() {
95        // 0 -- 1 -- 2, и обход 0 -- 3 -- 2 длиннее
96        let nodes = [[0.0, 0.0], [1.0, 0.0], [2.0, 0.0], [1.0, 5.0]];
97        let edges = vec![
98            vec![Edge { node: 1, weight: 1.0 }, Edge { node: 3, weight: 5.0 }],
99            vec![Edge { node: 0, weight: 1.0 }, Edge { node: 2, weight: 1.0 }],
100            vec![Edge { node: 1, weight: 1.0 }, Edge { node: 3, weight: 5.0 }],
101            vec![Edge { node: 0, weight: 5.0 }, Edge { node: 2, weight: 5.0 }],
102        ];
103
104        let path = find_path(0, 2, &nodes, &edges).unwrap();
105
106        assert_eq!(path, vec![0, 1, 2]);
107    }
108
109    #[test]
110    fn returns_none_when_unreachable() {
111        let nodes = [[0.0, 0.0], [1.0, 0.0]];
112        let edges = vec![vec![], vec![]];
113
114        assert!(find_path(0, 1, &nodes, &edges).is_none());
115    }
116}
vimp_engine_core/nav/pathfinder.rs

vimp_engine_core/nav/
pathfinder.rs
