pi_path_finding/
finder.rs

1//!
2//! A*寻路算法--抽象实现
3//! 给定一组代表全图的通用节点`nodes`，以及寻路的起始节点在`nodes`的索引`start` 和目的节点在`nodes`索引`end`进行A* 寻路；
4//! 如果寻路到终点 或 寻路的结果节点数目超过`max_number`，则终止寻路并返回结果；
5//! 注意：如果 from-to 的代价不对称，那么A*算法的结果可能不是最优的
6//! 传统A*算法中，open堆很容易过大， 放入节点和弹出节点都会很慢
7//! 优化版的实现，增加一个节点邻居NodeNeighbors，这个节点邻居代表一个节点的全部邻居，可以从中取出最优邻居节点， 将NodeNeighbors放入open表，这样可以大大缩小open表的大小。
8//! 寻路时，步骤1、创建起点的节点邻居nn。 步骤2、弹出nn中的最优邻居节点node，判断是否到达，如果没有到达，则获取该node的节点邻居nn1，如果open表不为空，从表头取最优的节点邻居nn2，比较nn2、nn1和nn，选择放入nn1或nn， 获得最优的新的nn。 重复该步骤2。
9//! 支持双端寻路，双端寻路一般比单端寻路速度快。如果 from-to 的代价不对称，则应该使用从起点出发的单端寻路
10
11use num_traits::Zero;
12use pi_null::Null;
13use std::{
14    cmp::{Ordering, Reverse},
15    collections::BinaryHeap,
16    fmt::Debug,
17    mem,
18};
19use std::ops::Deref;
20
21/// A*节点索引
22#[derive(PartialEq, Eq, PartialOrd, Ord, Clone, Copy, Default, Debug)]
23pub struct NodeIndex(pub usize);
24
25impl Deref for NodeIndex {
26    type Target = usize;
27
28    fn deref(&self) -> &Self::Target {
29        &self.0
30    }
31}
32
33
34impl Null for NodeIndex {
35    fn is_null(&self) -> bool {
36        self.0.is_null()
37    }
38
39    fn null() -> Self {
40        NodeIndex(usize::null())
41    }
42}
43/// A*节点状态
44#[derive(PartialEq, Eq, Clone)]
45pub enum NodeState {
46    /// 还未使用
47    None,
48    /// 在from的open表中
49    FromOpen,
50    /// 在to的open表中
51    ToOpen,
52    /// 在from的已搜索
53    FromClose,
54    /// 在to的已搜索
55    ToClose,
56}
57/// A*算法的返回类型
58#[derive(Debug)]
59pub enum AStarResult {
60    /// 找到了路径
61    Found,
62    /// 没找到路径
63    NotFound(NodeIndex),
64    /// 被max_number限制，没找到路径，返回最接近的节点
65    LimitNotFound(NodeIndex),
66}
67
68// A*寻路的节点条目
69pub trait NodeEntry<N: PartialOrd + Zero + Copy + Debug> {
70    fn g(&self) -> N;
71    fn h(&self) -> N;
72    fn state(&self) -> NodeState;
73    fn parent(&self) -> NodeIndex;
74    fn clear(&mut self, version: usize);
75}
76
77// A*搜索器， 记录每个节点的搜索过程中的信息
78pub struct Finder<N: PartialOrd + Zero + Copy + Debug, E: NodeEntry<N> + Default> {
79    // 节点数组
80    pub nodes: Vec<E>,
81    // 节点邻居的邻居表
82    pub neighbors: Vec<FNode<N>>,
83    /// 当前的搜索版本，每次搜索加1
84    pub version: usize,
85}
86
87///
88/// ## A* 寻路的堆
89/// ### 对`N`的约束，数字集合
90/// + 算术运算，可拷贝，可偏序比较；
91/// + 实际使用的时候就是数字类型，比如：i8/u8/i32/u32/f32/f64；
92pub struct AStar<N: PartialOrd + Zero + Copy + Debug, E: NodeEntry<N> + Default> {
93    // 从from搜to的Open表，最小堆
94    from_open: BinaryHeap<Reverse<NodeNeighbors<N>>>,
95    // A*搜索器
96    finder: Finder<N, E>,
97}
98
99impl<N: PartialOrd + Zero + Copy + Debug, E: NodeEntry<N> + Default> AStar<N, E> {
100    /// 实例化A*寻路算法的结构体
101    /// 必须传入最大可能的全图节点容量，可选传入调用A*算法时可能搜索的节点数量以提前初始化节约性能，但也可为0
102    pub fn with_capacity(map_capacity: usize, node_number: usize) -> Self {
103        let mut vec = Vec::with_capacity(map_capacity);
104        vec.resize_with(map_capacity, || E::default());
105        AStar {
106            from_open: BinaryHeap::with_capacity(node_number),
107            finder: Finder {
108                nodes: vec,
109                neighbors: Vec::with_capacity(node_number),
110                version: 0,
111            },
112        }
113    }
114    /// 重新设置地图节点数组的大小
115    pub fn resize(&mut self, map_capacity: usize) {
116        self.finder.nodes.resize_with(map_capacity, || E::default())
117    }
118    /// 从起点向终点的A*寻路算法
119    /// 给定一组代表全图的通用节点`nodes`，以及寻路的起始节点在`nodes`的索引`start` 和目的节点在`nodes`索引`end`进行A* 寻路；
120    /// 如果 寻路到终点 或 寻路的结果节点数目超过`max_number`，则终止寻路并返回结果；
121    ///
122    /// 返回结果：是否成功寻路至目标点
123    ///
124    /// 大概流程：
125    ///
126    /// + 1. 创建起点的节点邻居nn。
127    ///
128    /// + 2、弹出nn中的最优邻居节点node，判断是否到达，如果没有到达，则获取该node的节点邻居nn1，如果open表不为空，从表头取最优的节点邻居nn2，比较nn2、nn1和nn，选择放入nn1或nn， 获得最优的新的nn。 重复该步骤2。
129    ///
130    /// + 3、如果open表为空，则返回NotFound。
131    ///
132    pub fn find<Arg>(
133        &mut self,
134        start: NodeIndex,
135        end: NodeIndex,
136        max_number: usize,
137        arg: &mut Arg,
138        make_neighbors: fn(&mut Arg, NodeIndex, NodeIndex, &mut Finder<N, E>) -> NodeNeighbors<N>,
139    ) -> AStarResult {
140        self.from_open.clear();
141        self.finder.neighbors.clear();
142        self.finder.version += 1;
143        self.finder.nodes[start.0].clear(self.finder.version);
144        // 创建起点的节点邻居
145        let mut nn = make_neighbors(arg, start, end, &mut self.finder);
146        let mut near = nn.node;
147        let mut near_h = nn.f;
148        println!("start: {:?}, end: {:?}, h0: {:?}", start, end, near_h);
149        loop {
150            // 弹出nn中的最优邻居节点NodeIndex，弹出后，nn代价大小会变
151            let node = nn.pop(&self.finder, NodeState::FromOpen);
152            // 判断节点是否无效
153            if node.is_null() {
154                // 无效则继续弹出新的nn
155                if let Some(n) = self.from_open.pop() {
156                    nn = n.0;
157                } else {
158                    // 如果open表空， 则返回NotFound
159                    return AStarResult::NotFound(near);
160                }
161                continue;
162            }
163            // 判断是否已经找到路径
164            if node == end {
165                return AStarResult::Found;
166            }
167            // 如果nn有效
168            if nn.start < nn.end {
169                // 创建该点的节点邻居
170                let mut nn1 = make_neighbors(arg, node, end, &mut self.finder);
171                let h = self.finder.nodes[node.0].h();
172                // println!("======= h1: {:?}, node: {:?}", h, node);
173                if h < near_h  {
174                    near = node;
175                    near_h = h;
176                }
177                // 如果该节点邻居可用
178                if nn1.start < nn1.end {
179                    // 如果nn1优于nn， 则交换两者
180                    if nn1 < nn {
181                        nn1 = mem::replace(&mut nn, nn1);
182                    }
183                    // 将nn1放入堆上
184                    self.from_open.push(Reverse(nn1));
185                    // 如果超过max_number，则返回LimitNotFound
186                    if self.from_open.len() > max_number {
187                        return AStarResult::LimitNotFound(node);
188                    }
189                }
190            } else {
191                // 如果nn无效则用新的nn
192                nn = make_neighbors(arg, node, end, &mut self.finder);
193                let h = self.finder.nodes[node.0].h();
194                // println!("======= h2: {:?}, node: {:?}", h, node);
195                if h < near_h  {
196                    near = node;
197                    near_h = h;
198                }
199                continue;
200            }
201            // 用堆上的nn2和nn比较
202            if let Some(n) = self.from_open.peek() {
203                // 如果堆上的nn2优于nn
204                if n.0 < nn {
205                    // 取出nn2并交换两者
206                    let nn2 = self.from_open.pop().unwrap().0;
207                    // 将nn放入到堆上
208                    self.from_open.push(Reverse(nn));
209                    nn = nn2;
210                }
211            }
212        }
213    }
214    /// 返回结果的迭代器
215    pub fn result_iter<'a>(&'a self, node: NodeIndex) -> ResultIterator<'a, N, E> {
216        ResultIterator {
217            finder: &self.finder,
218            node,
219        }
220    }
221}
222
223// 结果的迭代器
224#[derive(Clone)]
225pub struct ResultIterator<'a, N: PartialOrd + Zero + Copy + Debug, E: NodeEntry<N> + Default> {
226    pub finder: &'a Finder<N, E>,
227    pub node: NodeIndex,
228}
229
230impl<'a, N: PartialOrd + Zero + Copy + Debug, E: NodeEntry<N> + Default> Iterator
231    for ResultIterator<'a, N, E>
232{
233    type Item = NodeIndex;
234    fn next(&mut self) -> Option<Self::Item> {
235        if self.node.is_null() {
236            return None;
237        }
238        let old = self.node;
239        let e = &self.finder.nodes[self.node.0];
240        self.node = e.parent();
241        Some(old)
242    }
243}
244
245/// 节点邻居
246#[derive(Debug, Clone)]
247pub struct NodeNeighbors<N: PartialOrd + Zero + Copy + Debug> {
248    pub f: N,
249    pub node: NodeIndex,
250    pub start: usize,
251    pub end: usize,
252}
253
254impl<N: PartialOrd + Zero + Copy + Debug> NodeNeighbors<N> {
255    /// 从指定的节点邻居上获取其的最优邻居节点
256    fn pop<E: NodeEntry<N> + Default>(
257        &mut self,
258        finder: &Finder<N, E>,
259        state: NodeState,
260    ) -> NodeIndex {
261        while self.start < self.end {
262            let n = &finder.neighbors[self.start];
263            let e = &finder.nodes[n.node.0];
264            self.start += 1;
265            // 要求该节点的parnet必须是节点邻居上的节点
266            if e.state() == state && e.parent() == self.node {
267                // 更新节点邻居的f值
268                while self.start < self.end {
269                    let s = &finder.neighbors[self.start];
270                    let e = &finder.nodes[s.node.0];
271                    if e.state() == state && e.parent() == self.node {
272                        self.f = e.g() + e.h();
273                        break;
274                    }
275                    self.start += 1;
276                }
277                return n.node;
278            }
279        }
280        NodeIndex(usize::MAX)
281    }
282}
283// Ord trait所需
284impl<N: PartialOrd + Zero + Copy + Debug> Eq for NodeNeighbors<N> {}
285// Ord trait所需
286impl<N: PartialOrd + Zero + Copy + Debug> PartialEq for NodeNeighbors<N> {
287    fn eq(&self, other: &Self) -> bool {
288        self.f.eq(&other.f)
289    }
290}
291// Ord trait所需
292impl<N: PartialOrd + Zero + Copy + Debug> PartialOrd for NodeNeighbors<N> {
293    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
294        self.f.partial_cmp(&other.f)
295    }
296}
297// 通过f的比较实现Ord trait，以能在堆中排序
298impl<N: PartialOrd + Zero + Copy + Debug> Ord for NodeNeighbors<N> {
299    fn cmp(&self, other: &Self) -> Ordering {
300        match self.f.partial_cmp(&other.f) {
301            None => self.node.cmp(&other.node),
302            Some(r) => r,
303        }
304    }
305}
306
307/// 排序节点
308#[derive(Clone)]
309pub struct FNode<N: PartialOrd + Zero + Copy + Debug> {
310    pub f: N,
311    pub node: NodeIndex,
312}
313
314// Ord trait所需
315impl<N: PartialOrd + Zero + Copy + Debug> Eq for FNode<N> {}
316// Ord trait所需
317impl<N: PartialOrd + Zero + Copy + Debug> PartialEq for FNode<N> {
318    fn eq(&self, other: &Self) -> bool {
319        self.f.eq(&other.f)
320    }
321}
322// Ord trait所需
323impl<N: PartialOrd + Zero + Copy + Debug> PartialOrd for FNode<N> {
324    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
325        self.f.partial_cmp(&other.f)
326    }
327}
328// 通过f的比较实现Ord trait，以能在数组中排序
329impl<N: PartialOrd + Zero + Copy + Debug> Ord for FNode<N> {
330    fn cmp(&self, other: &Self) -> Ordering {
331        match self.f.partial_cmp(&other.f) {
332            None => self.node.cmp(&other.node),
333            Some(r) => r,
334        }
335    }
336}
337
338///
339/// ## 双端 A* 寻路的堆
340/// ### 对`N`的约束，数字集合
341/// + 算术运算，可拷贝，可偏序比较；
342/// + 实际使用的时候就是数字类型，比如：i8/u8/i32/u32/f32/f64；
343pub struct DualAStar<N: PartialOrd + Zero + Copy + Debug, E: NodeEntry<N> + Default> {
344    // 从from搜to的AStar
345    pub astar: AStar<N, E>,
346    // 从to搜from的Open表，最小堆
347    pub to_open: BinaryHeap<NodeNeighbors<N>>,
348}
pi_path_finding/finder.rs

pi_path_finding/
finder.rs
