Rust语言从入门到精通系列 - A*寻路算法

1
use std::collections::BinaryHeap;
2
use std::cmp::Ordering;
3

4
#[derive(Clone, Copy, Eq, PartialEq)]
5
struct Node {
6
    x: usize,
7
    y: usize,
8
    f: usize,
9
    g: usize,
10
    h: usize,
11
}
12

13
impl Ord for Node {
14
    fn cmp(&self, other: &Self) -> Ordering {
15
        other.f.cmp(&self.f)
16
    }
17
}
18

19
impl PartialOrd for Node {
20
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
21
        Some(self.cmp(other))
22
    }
23
}
24

25
fn a_star(map: &Vec<Vec<i32>>, start: (usize, usize), end: (usize, usize)) -> Option<Vec<(usize, usize)>> {
26
    let mut open_list = BinaryHeap::new();
27
    let mut close_list = vec![vec![false; map[0].len()]; map.len()];
28
    let mut parent = vec![vec![(0, 0); map[0].len()]; map.len()];
29
    let mut g_score = vec![vec![usize::MAX; map[0].len()]; map.len()];
30
    let mut f_score = vec![vec![usize::MAX; map[0].len()]; map.len()];
31
    let (start_x, start_y) = start;
32
    let (end_x, end_y) = end;
33

34
    g_score[start_x][start_y] = 0;
35
    f_score[start_x][start_y] = manhattan_distance(start_x, start_y, end_x, end_y);
36

37
    open_list.push(Node { x: start_x, y: start_y, f: f_score[start_x][start_y], g: 0, h: f_score[start_x][start_y] });
38

39
    while let Some(current) = open_list.pop() {
40
        if current.x == end_x && current.y == end_y {
41
            let mut path = vec![];
42
            let mut current = (end_x, end_y);
43
            while current != (start_x, start_y) {
44
                path.push(current);
45
                current = parent[current.0][current.1];
46
            }
47
            path.push((start_x, start_y));
48
            path.reverse();
49
            return Some(path);
50
        }
51

52
        close_list[current.x][current.y] = true;
53

54
        //    四方向坐标系寻路, 可以根据需求改写扩展为8方向
55
        for (dx, dy) in &[(-1, 0), (1, 0), (0, -1), (0, 1)] {
56
            let x = current.x as i32 + dx;
57
            let y = current.y as i32 + dy;
58

59
            //    判断坐标是否超出地图边界
60
            if x < 0 || x >= map.len() as i32 || y < 0 || y >= map[0].len() as i32 {
61
                continue;
62
            }
63

64
            let x = x as usize;
65
            let y = y as usize;
66

67
            //    判断是否可以通行，可以通过扩展类型实现更多的地图场景效果
68
            if map[x][y] == 1 || close_list[x][y] {
69
                continue;
70
            }
71

72
            let tentative_g_score = g_score[current.x][current.y] + 1;
73
            if tentative_g_score < g_score[x][y] {
74
                parent[x][y] = (current.x, current.y);
75
                g_score[x][y] = tentative_g_score;
76
                f_score[x][y] = tentative_g_score + manhattan_distance(x, y, end_x, end_y);
77
                if !open_list.iter().any(|node| node.x == x && node.y == y) {
78
                    open_list.push(Node { x: x, y: y, f: f_score[x][y], g: g_score[x][y], h: manhattan_distance(x, y, end_x, end_y) });
79
                }
80
            }
81
        }
82
    }
83

84
    None
85
}
86

87
//    曼哈顿距离算法
88
fn manhattan_distance(x1: usize, y1: usize, x2: usize, y2: usize) -> usize {
89
    let dx = if x1 > x2 { x1 - x2 } else { x2 - x1 };
90
    let dy = if y1 > y2 { y1 - y2 } else { y2 - y1 };
91
    (dx + dy) * 10
92
}
93

94
fn main() {
95
    let map = vec![
96
        vec![0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
97
        vec![0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
98
        vec![0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
99
        vec![0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
100
        vec![0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
101
        vec![0, 0, 0, 0, 1, 1, 1, 1, 1, 0],
102
        vec![0, 0, 0, 0, 1, 0, 0, 0, 1, 0],
103
        vec![0, 0, 0, 0, 1, 0, 0, 0, 1, 0],
104
        vec![0, 0, 0, 0, 1, 0, 0, 0, 1, 0],
105
        vec![0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
106
    ];
107

108
    let start = (6, 1);
109
    let end = (3, 8);
110

111
    if let Some(path) = a_star(&map, start, end) {
112
        for row in 0..map.len() {
113
            for col in 0..map[0].len() {
114
                if (row, col) == start {
115
                    print!("S");
116
                } else if (row, col) == end {
117
                    print!("E");
118
                } else if path.contains(&(row, col)) {
119
                    print!("*");
120
                } else if map[row][col] == 1 {
121
                    print!("X");
122
                } else {
123
                    print!(".");
124
                }
125
            }
126
            println!();
127
        }
128
    } else {
129
        println!("No path found!");
130
    }
131
}
132
//    输出结果：
133
// ..........
134
// ..........
135
// ..........
136
// .*******E.
137
// .*........
138
// .*..XXXXX.
139
// .S..X...X.
140
// ....X...X.
141
// ....X...X.
142
// ....X.....