A*算法的实现（c++优先队列）_priority_queue a*

A*算法的理解

对于A*算法，网上已经有了很详尽的描述，这里不再重复，如果想去看的话，我看了很多博客，最后看到这个博客的时候成功实现了A*算法。
因为详细的说明别人有讲，这里只谈一下实现。
在我看来，A*算法的实质其实就是BFS，只不过BFS的过程中加入了一个变量f，每次选出f最小的点进行BFS，BFS决定了我们能找到最优解，变量f能让我们贪心的更快的找到最优解。

struct node {
    int x, y;
    int f, g, h;// f = g + h;
    int type;
    bool is_open;
    bool is_close;
    node* parent = NULL;
    void print() {
        printf("%d   %d: f: %d  g: %d  h: %d   type: %d\n", x, y, f, g, h, type);
    }
    bool operator < (const node& cmp) const {
        return f > cmp.f;
    }
}map[15][15];
1
2
3
4
5
6
7
8
9
10
11
12
13
14

在实现A*算法前，我们需要定义一个结构体node(名字随意起)表示地图的一个点包含的信息，其中x,y表示点坐标，f、g和h是实现A*算法的关键，用来判断BFS的方向，type表示地图当前位置的类型，比如起点，终点或者墙之类的,is_open表示是否在bfs的open优先队列里面,is_close表示是否访问过，不重复访问一个节点。
这时候我们来讲一下怎么计算f,g,h，我们在一开始的时候把起点加入open优先队列里面，这时起点的g=0，起点会向八个方向扩展(当然你也可以写成向四个方向扩展)，这时你就需要计算扩展的节点的g，如果是上下左右扩展就+1，向斜扩展就加√2，g的扩展如下:

|g+√2 | g+1 | g+√2|
|  g+1 |  g  | g+1 |
|g+√2 | g+1 | g+√2|
1
2
3

我的话直接将fgh的值扩大10倍取整，就是上下左右+10，斜着+14。h的距离就是扩展的节点与终点之间的哈曼顿距离，假设扩展节点为u，终点为end，这时的h的计算方法为

h = abs(end.x-u.x) + abs(end.y-u.y);
1

最后计算f = g + h;
我们最后把每次扩展的节点加入open优先队列，并标记节点，不断扩展，直到遇到终点或者open优先队列为空，下面是代码：

#include <cstdio>
#include <iostream>
#include <queue>
#include <vector>
#include <time.h>
#include <algorithm>
using namespace std;
//地图
int g[15][15] = {
    { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
    { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 2, 0, 0, 0, 1, 0, 3, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
    { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 },
    { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
};
enum Type {
    Road,Wall,Start,End
    //路，墙，起点，终点
};
int endX = 0, endY = 0;
struct node {
    int x, y;
    int f, g, h;// f = g + h;
    int type;
    bool is_open;
    bool is_close;
    node* parent = NULL;
    void print() {
        printf("%d   %d: f: %d  g: %d  h: %d   type: %d\n", x, y, f, g, h, type);
    }
    bool operator < (const node& cmp) const {
    //这里是给优先队列排序用的，优先队列默认大的在前面，所以我们写<的时候判断用>号
        return f > cmp.f;
    }
}map[15][15];
priority_queue<node> open;
bool inside(int x,int y,int n, int m) {
    return x >= 0 && y >= 0 && x < n && y < m;
}
void A_() {
    while (!open.empty()) {
        node x = open.top();
        printf("%d   %d\n", x.x, x.y);//打印bfs的路径，没有实际作用
        open.pop();
        map[x.x][x.y].is_close = true;
        if (map[endX][endY].is_close) {//从终点走回起点
            int nx = endX, ny = endY;
            while (map[nx][ny].type != Start) {
                node* x = map[nx][ny].parent;
                nx = x->x;
                ny = x->y;
                if (map[nx][ny].type != Start)g[nx][ny] = 6;
            }
            break;
        }
        for (int i = -1; i <= 1; i++)for (int j = -1; j <= 1;j++) {
            if (i == j && i == 0)continue;
            if (!inside(x.x + i, x.y + j, 15, 15))continue;
            if (map[x.x + i][x.y + j].is_close || map[x.x + i][x.y + j].is_open) continue;
            if (map[x.x + i][x.y + j].type == Wall)continue;

            node& u = map[x.x + i][x.y + j];
            u.parent = &(map[x.x][x.y]);
            u.g = map[x.x][x.y].g + ((i&&j) ? 14 : 10);
            u.h = abs(u.x - map[endX][endY].x) + abs(u.y - map[endX][endY].y);
            u.f = u.g + u.f;
            u.is_open = true;

            open.push(map[x.x + i][x.y + j]);
        }
    }
}
void CreateMap() {
    for (int i = 0; i < 15; i++)for (int j = 0; j < 15; j++) {
        map[i][j].x = i;
        map[i][j].y = j;
        map[i][j].type = g[i][j];
        map[i][j].f = map[i][j].g = map[i][j].h = 0;
        map[i][j].is_open = map[i][j].is_close = false;
        if (map[i][j].type == Start) {
            map[i][j].is_open = true;
            open.push(map[i][j]);
        }
        if (map[i][j].type == End) {
            endX = i;
            endY = j;
        }
    }

}
int main()
{

    CreateMap();
    A_();
    for (int i = 0; i < 15; i++) {
        for (int j = 0; j < 15; j++) {
            printf("%d ", g[i][j]);
        }
        puts("");
    }
    return 0;
}
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