寻路算法A-Star(A*)的C++实现
2019-06-20 本文已影响13人
crossous
前言
在游戏中，一个单位从一个地点移动到另一个地点总是复杂的，需要考虑的因素有很多，不过今天只实现最简单的寻路算法。
地图的组织往往是一个图的数据结构，例如地形贴图，单通道的值会直接储存地形的高度信息。
我们希望通过寻路算法(Pathfinding Algorithm)，找到两点之间的最短路径。
作为一个地图，两点之间的路径必然是有很多的，最笨的方法是将所有路径都检测出来，取其长度最小的路径。
更好的算法还有迪杰斯特拉(Dijkstra)最短路径算法，以及BFS(Breadth First Search)广度优先算法，都能找到最短路径。
A-Star算法采用了一种“启发式”的方法，会比这些寻路算法更快。
基本原理
首先，这个算法包含一张基础的地图，包含以下的元素：
- 一张二维网格组成的地图
- 绿色的起始点
- 红色的终结点
- 灰色的障碍
我们需要找到一个从绿色起始点到达红色终结点的路径。寻路算法根据需求不同，走法并不统一，例如行走方向是8方向还是4方向（“米”字还是“十”字），会不会像象棋那样被“别住”马腿（例如2行4列的方格是否能直接斜走到1行5列的方格中）。
在此声明：文章默认实现8方向，无阻塞的寻路。如果有需求，也很容易改变。
在A*算法中，我们不会记录路径；实现方法是：每个节点（方格）都会记住走到自己方格的上一个方格是谁；假如我们的算法走到终点，就代表我们成功了，我们只需要不断找上一个方格，就能还原出这一个最短路径。
每一个方格还有其他属性。首先，是当前节点的行列坐标。其次，是三个变量，我们命名为F、G、H。
其中G的含义为：从原点出发，沿着路径走到当前节点所需要的“代价”。
从一个格子到另一个格子的“花销”往往是不同的，比如我们做了一个2D RPG游戏，同样是一格子的距离，假如路面有“泥泞”属性，就会导致时间花销变大；假如地面有“坑坑洼洼”属性，就会导致耐久变低等等，这些“花销”可以通过一些评价方法（会直接规定）统一为一种“花销”指标，而这个指标可看做路径的权值。
我们的实现不用那么复杂，先简单定义一种开销：长度开销，从一个格子水平或垂直到达另一个格子花销为10，而斜向到达花销为14（近似√(10+10)=14.14）。
H的含义为：从当前节点出发，大致到达终点的“开销”。
这个变量H就是体现A*算法“启发式”的地方，这个变量H的“开销”并非是精准的，我们可以用一些合理的方法来测算它，最简单的方法是比较直接距离，例如用沟谷定理直接算欧式距离，不过这样会出小数，而我们的“开销”暂时定义为整形，因此采用“曼哈顿距离”（也称街道距离，就是算两者横纵坐标绝对值之和）。
F是用来评估路径的变量，能帮助我们最快接近最短路径，最简单的方式：F=G+H，这样一来，F的含义就是：从起始点出发走到当前的花费，加上预计到达终点的花费，也就是预计的总花费。或许F变量可以采用一些加权平均的计算方式，有兴趣可以试试。
现在举个例子，坐标(4, 3)（非数组方式，而是坐标系方式，3行4列或x=4,y=3），起始点右侧的格子。从起始点只需要一步，因此G=10，曼哈顿距离到达终点H=30，因此F=40。
根据上方的定义，我们写出了格子的结构体：
struct Node {
int x;
int y;
int G;
int H;
int F;
Node* prev = nullptr;//路径的前一个格子
bool visiable;//可访问的
};
接下来再用到两个列表，openlist与closelist；openlist代表已知但未探索过的节点，closelist代表探索完成的节点。
可以想象为：我们站在一个格子中，我们的视线能看透周围8个格子，每看到新的格子，便加入到openlist中，每从openlist中探索（走过）完一个格子，便将其从openlist中去除，并加入到closelist中。
如下图，走过的格子在closelist中(淡蓝色)，未走过，但已知的格子在openlist中(绿色)：
算法步骤
伪代码：
起点G=0，H=曼哈顿距离，F=G+H
其他格子暂时将F和G设为无限大
起点加入openlist
loop(openlist不为空){
找到openlist中F值最小的的格子，将其设为当前处理的格子
if 这个格子就是end格子:
return true
计算当前格子周围8个格子（如果是超出边界，或者是障碍物，或者已经探索过（处于closelist中），则不予处理，）：
if (当前格子的G+当前格子到达周围格子的开销(10或14)) < 周围格子本身的G，则更新周围格子的G和F(因为路径更短，H不会变)
if 这个周围格子不在openlist，也不再closelist中，则加入openlist
将当前格子取出openlist并加入closelist中
}
return false(能找到的都找遍了，说明找不到路径)
思路大致清晰，接下来是代码的编写
代码实现
由于涉及可视化比较麻烦，因此采用打印到控制台输出。
在这个程序中，我希望将地图信息以字符数组的方式传入，程序应该告诉我们是否能找到路径，并将找到的最短路径打印出来。
//以这种方式
int main() {
//地图可视化参照上方图片
//O代表可行路径
//X代表障碍物
//S代表起点
//E代表终点
std::vector<std::vector<char>> map = {
{'O', 'O','O','O','O','O','O','O'},
{'O', 'O','O','O','X','O','O','O'},
{'O', 'O','S','O','X','O','E','O'},
{'O', 'O','O','O','X','O','O','O'},
{'O', 'O','O','O','O','O','O','O'},
{'O', 'O','O','O','O','O','O','O'}
};
A_Star problem(map);
if (problem.solve()) {
problem.print_map();
}
}
我们需要实现的就是A_Star类，针对它的功能，我们对其做如下声明：
class A_Star {
public:
A_Star(const std::vector<std::vector<char>>& map);
~A_Star();
bool solve();//解决方案
void print_path();//追溯路径，打印在控制台上
void print_map();//直接打印寻找的地图
Node* m_map = nullptr;//地图用一维数组数组存储
int m_map_height;//高和宽
int m_map_weight;
Node* m_start = nullptr;//开始和结束格子
Node* m_end = nullptr;
}
对于如果有拷贝需求，拷贝构造函数还是很需要的，按需写即可。
对于构造方法，希望它能将字符串数组转化为格子结构，并初始化属性。
A_Star::A_Star(const std::vector<std::vector<char>>& map) {
m_map_height = map.size();
m_map_weight = map[0].size();
if (m_map_height == 0 || m_map_weight == 0) {
throw std::exception("map format error");
}
m_map = new Node[m_map_height * m_map_weight];
for (int i = 0; i < m_map_height; ++i) {
for (int j = 0; j < m_map_weight; ++j) {
Node& curr_node = m_map[i*m_map_weight + j];
curr_node.x = j;
curr_node.y = i;
curr_node.G = std::numeric_limits<int>::max();//先全部设置为无限大
curr_node.F = std::numeric_limits<int>::max();
switch (map[i][j]) {
case 'O':
curr_node.visiable = true;
break;
case 'X':
curr_node.visiable = false;
break;
case 'S':
curr_node.visiable = true;
m_start = &curr_node;
break;
case 'E':
curr_node.visiable = true;
m_end = &curr_node;
break;
default:
throw std::exception("illegal identifier exist");
}
}
}
if (m_start == nullptr) {
throw std::exception("start point is not exist");
}
if (m_end == nullptr) {
throw std::exception("end point is not exist");
}
for (int i = 0; i < m_map_height; ++i) {
for (int j = 0; j < m_map_weight; ++j) {
Node& curr_node = m_map[i*m_map_weight + j];
curr_node.H = (std::abs(curr_node.x - m_end->x) + std::abs(curr_node.y - m_end->y)) * 10;//计算曼哈顿距离
}
}
}
析构方法很简单，我们只动态申请了地图，释放掉就好了。
A_Star::~A_Star() {
delete[] m_map;
}
对于solve方法，我希望能根据初始化的数据来找到一条最短路径，如果找到就返回true，否则返回false，并将寻找的路径信息存在Node结构中。
代码相对较长，但处理周围8个节点中有很多重复代码
bool A_Star::solve() {
std::vector<Node*> openlist;
std::vector<Node*> closelist;
openlist.push_back(m_start);//开始节点加入openlist
m_start->G = 0;//开始节点的G置零并计算F
m_start->F = m_start->H;
while (openlist.size() != 0) {//如果openlist不为空
Node* curr_node = nullptr;//找到F值最小的节点作为当前处理节点
for (std::vector<Node*>::iterator node = openlist.begin(); node != openlist.end(); node++) {
if (curr_node == nullptr) {
curr_node = *node;
continue;
}
else if ((*node)->F <= curr_node->F) {
curr_node = *node;
}
}
if (curr_node == m_end) {//如果这个节点刚好是终点，直接返回true退出
return true;
}
bool has_prev_x = curr_node->x - 1 >= 0;
bool has_next_x = curr_node->x + 1 < m_map_weight;
bool has_prev_y = curr_node->y - 1 >= 0;
bool has_next_y = curr_node->y + 1 < m_map_height;
if (has_prev_x) {//存在左侧
Node* left = &m_map[curr_node->y * m_map_weight + curr_node->x - 1];
if (left->visiable && std::find(closelist.begin(), closelist.end(), left) == closelist.end()) {//如果此路径可用并且不在closelist中
if (curr_node->G + 10 < left->G) {//自身G走10到达left节点比left原来的G小，则给left_up换条路径，并重新计算G和F
left->prev = curr_node;
left->G = curr_node->G + 10;
left->F = left->G + left->H;
if (std::find(openlist.begin(), openlist.end(), left) == openlist.end()) {//如果不在openlist中，加入openlist
openlist.push_back(left);
}
}
}
if (has_prev_y) {//存在左上
Node* left_up = &m_map[(curr_node->y - 1)*m_map_weight + curr_node->x - 1];
if (left_up->visiable && std::find(closelist.begin(), closelist.end(), left_up) == closelist.end()) {
if (curr_node->G + 14 < left_up->G) {//自身G走14到达left_up节点比left_up原来的G小，则换条路径
left_up->prev = curr_node;
left_up->G = curr_node->G + 14;
left_up->F = left_up->G + left_up->H;
if (std::find(openlist.begin(), openlist.end(), left_up) == openlist.end()) {//如果左侧节点不再openlist中，则添加进去
openlist.push_back(left_up);
}
}
}
}
if (has_next_y) {//存在左下
Node* left_down = &m_map[(curr_node->y + 1)*m_map_weight + curr_node->x - 1];
if (left_down->visiable && std::find(closelist.begin(), closelist.end(), left_down) == closelist.end()) {
if (curr_node->G + 14 < left_down->G) {//自身G走14到达left_down节点比left_down原来的G小，则换条路径
left_down->prev = curr_node;
left_down->G = curr_node->G + 14;
left_down->F = left_down->G + left_down->H;
if (std::find(openlist.begin(), openlist.end(), left_down) == openlist.end()) {
openlist.push_back(left_down);
}
}
}
}
}
if (has_next_x) {//存在右侧
Node* right = &m_map[curr_node->y * m_map_weight + curr_node->x + 1];
if (right->visiable && std::find(closelist.begin(), closelist.end(), right) == closelist.end()) {
if (curr_node->G + 10 < right->G) {
right->prev = curr_node;
right->G = curr_node->G + 10;
right->F = right->G + right->H;
if (std::find(openlist.begin(), openlist.end(), right) == openlist.end()) {
openlist.push_back(right);
}
}
}
if (has_prev_y) {//存在右上
Node* right_up = &m_map[(curr_node->y - 1)*m_map_weight + curr_node->x + 1];
if (right_up->visiable && std::find(closelist.begin(), closelist.end(), right_up) == closelist.end()) {
if (curr_node->G + 14 < right_up->G) {
right_up->prev = curr_node;
right_up->G = curr_node->G + 14;
right_up->F = right_up->G + right_up->H;
if (std::find(openlist.begin(), openlist.end(), right_up) == openlist.end()) {
openlist.push_back(right_up);
}
}
}
}
if (has_next_y) {//存在右下
Node* right_down = &m_map[(curr_node->y + 1)*m_map_weight + curr_node->x + 1];
if (right_down->visiable && std::find(closelist.begin(), closelist.end(), right_down) == closelist.end()) {
if (curr_node->G + 14 < right_down->G) {
right_down->prev = curr_node;
right_down->G = curr_node->G + 14;
right_down->F = right_down->G + right_down->H;
if (std::find(openlist.begin(), openlist.end(), right_down) == openlist.end()) {
openlist.push_back(right_down);
}
}
}
}
}
if (has_prev_y) {//存在上侧
Node* up = &m_map[(curr_node->y - 1)*m_map_weight + curr_node->x];
if (up->visiable && std::find(closelist.begin(), closelist.end(), up) == closelist.end()) {
if (curr_node->G + 10 < up->G) {
up->prev = curr_node;
up->G = curr_node->G + 10;
up->F = up->G + up->H;
if (std::find(openlist.begin(), openlist.end(), up) == openlist.end()) {
openlist.push_back(up);
}
}
}
}
if (has_next_y) {//存在下侧
Node* down = &m_map[(curr_node->y + 1)*m_map_weight + curr_node->x];
if (down->visiable && std::find(closelist.begin(), closelist.end(), down) == closelist.end()) {
if (curr_node->G + 10 < down->G) {
down->prev = curr_node;
down->G = curr_node->G + 10;
down->F = down->G + down->H;
if (std::find(openlist.begin(), openlist.end(), down) == openlist.end()) {
openlist.push_back(down);
}
}
}
}
openlist.erase(std::find(openlist.begin(), openlist.end(), curr_node));//把当前节点从openlist中拿出来
closelist.push_back(curr_node);//放到closelist中
}
return false;//没找到路径，直接返回
}
剩下的代码就是用来可视化的：
void A_Star::print_path() {//从终点往回追溯路径并打印
Node* curr_node = m_end;
while (curr_node != nullptr) {
std::cout << "(" << curr_node->x << ", " << curr_node->y << ")" << std::endl;
curr_node = curr_node->prev;
}
}
void A_Star::print_map() {
char* map = new char[m_map_height * m_map_weight];//还原输入地图的标记点
for (int i = 0; i < m_map_height; ++i) {
for (int j = 0; j < m_map_weight; ++j) {
if (m_map[i*m_map_weight + j].visiable)
map[i*m_map_weight + j] = 'O';
else
map[i*m_map_weight + j] = 'X';
}
}
map[m_start->y*m_map_weight + m_start->x] = 'S';
map[m_end->y*m_map_weight + m_end->x] = 'E';
Node* curr_node = m_end->prev;//将除了起点和终点的路径标为'*'
while (curr_node->prev != nullptr) {
map[curr_node->y * m_map_weight + curr_node->x] = '*';
curr_node = curr_node->prev;
}
for (int i = 0; i < m_map_height; ++i) {//打印输出到控制台
for (int j = 0; j < m_map_weight; ++j) {
std::cout << map[i*m_map_weight + j] << " ";
}
std::cout << std::endl;
}
delete[] map;
}
测试
按照上面一开始放出的代码，输出：
O O O O * O O O
O O O * X * O O
O O S O X O E O
O O O O X O O O
O O O O O O O O
O O O O O O O O
和图中基本一致，还可以试试别的：
std::vector<std::vector<char>> map = {
{'O', 'X','X','X','X','O','O','O'},
{'O', 'X','O','O','X','O','O','O'},
{'O', 'O','S','O','X','O','E','X'},
{'O', 'O','O','O','X','X','X','O'},
{'O', 'O','O','X','O','X','O','O'},
{'O', 'O','O','O','O','O','O','O'}
};
outout:
O X X X X O O O
O X O O X O O O
O O S O X O E X
O O O * X X X *
O O O X * X * O
O O O O O * O O
这个略有不同，我们代码实现的是斜向完全无阻塞，可视化网站实现的是单边遮挡无阻塞，双边遮挡阻塞，因此略有不同，不过研究明白算法，更改还是很容易的。
脑洞
最基础的A*寻路算法就这样实现了，可以看出算法的灵活性，在很多地方都可以加以定制。
例如，我们需要移动的角色有“闪烁技能”，能瞬移一面墙的距离，那么我们的已知范围就可以从周围8格扩大到24格，同时为了防止人物肆意妄为的“闪烁”，可以针对H变量动手，使得闪烁的代价增大，致使F变量增大，在openlist寻找最小F的过程中，就不会优先选择闪烁。
除此之外，前面提到的不同路径的不同花销也是可以做一些小动作，使得AI能根据地形选择不同的移动方式。
期待早日能应用到实践当中！
引用
A*算法理解及代码实现 思路参考自这里，python代码以及pygame可视化
PathFinding.js一个JS库的可视化演示，里面还包括很多其他寻路算法的展示
