A* 寻路算法

写在前面

再来填一个坑。之前一直说要写的A* 终于有空补上了，本篇博客首先会介绍最基础的A* 实现，在简单A* 的基础上，尝试实现稍复杂的A* 算法（带有高度信息的地形，允许对角线寻路等）。

A*算法简介

本博客不准备探讨A* 算法的原理，这里仅仅对A*算法做一个简单介绍，对具体原理感兴趣的同学请查阅相关资料。

A* 算法是一种启发式搜索算法。本质上来讲，可以算作是广度优先搜索算法的改进。我们知道，广度优先搜索总能找到路径最短的最优解，因为它每次新的一轮遍历永远是离起始点最近的位置，这样，当扫描到目标点时，可以保证目标点的距离是离起点距离最近的，也就是找到了寻路的最优解。

A* 算法的运行过程与广度优先搜索类似，不同的是，A* 除了考虑当前点离起始点的距离外，还考虑了当前点离目标点的距离，我们分别用g和h来表示这两个距离，由此我们有代价函数f = g+h。A*算法每次查找代价最低的点作为搜索点，并更新搜索节点列表。最终搜索到目标位置。可以看到，A* 算法得到的路径并不一定最优。

具体的算法设计

对于每一次搜索任务，我们维护两个列表：openList和closeList。 这两个列表分别存放待访问的节点和已访问过的节点。对于每一个循环，我们从openlist中取出f最小的点，并检查它的四邻域（上下左右四个点），若四邻域的点中有已经处于openlist中的点，则做更新操作（如果该点新计算的f值比旧的f值更小），若有点处于closeList中，则不做任何操作，若不在以上列表，则计算他们的f值，并放入openList。以此类推，直至到达最终点或openList为空（不存在路径）。

具体实现（C++11）

以下是用C++ 实现的A*寻路算法，地图上为1的点表示可达的点，-1则表示不可达。

#include <iostream>
#include <unordered_map>
#include<vector>
#include<limits.h>
const int WIDTH = 100;
const int HEIGHT = 100;
using namespace std;
struct Node
{
Node* parent;
int _x;
int _y;
int f;
int g;
int h;
Node(int x, int y):_x(x),_y(y),parent(nullptr){
g = 0;
h = 0;
f = g+h;
}
};

int calculateH(Node* cur,Node* end);
vector<Node*> find(Node* start,Node* end, unordered_map<int,unordered_map<int,int>>& mGraph);
void check(int x,int y,Node* cur,unordered_map<int,unordered_map<int,Node*>>& openList, unordered_map<int,unordered_map<int,Node*>>& endList,Node* end, unordered_map<int,unordered_map<int,int>>& mGraph);
Node* findMin(unordered_map<int,unordered_map<int,Node*>>& openList);
int main(int argc, char const *argv[])
{
unordered_map<int,unordered_map<int,int>> mGraph;

for (int i = 0; i < WIDTH; ++i)
{
for (int j = 0; j < HEIGHT; ++j)
{
mGraph[i][j] = 1;
}
}
for(int i = 0;i<25;++i) {
mGraph[i][2] = -1;
}

Node * start = new Node(0,0);
Node * end = new Node(10,10);
start->h = calculateH(start,end);
start->f = start->g+start->h;
auto ret = find(start,end,mGraph);
for(auto v:ret) {
cout<<v->_x<<"  "<<v->_y<<endl;
}
return 0;
}

vector<Node*> find(Node* start,Node* end, unordered_map<int,unordered_map<int,int>>& mGraph) {

if(start->_x == end->_x && start->_y == end->_y) return {};
if(mGraph[start->_x][start->_y]==-1||mGraph[end->_x][end->_y] == -1) {
// 起点不可达
return {};
}
unordered_map<int,unordered_map<int,Node*>> openList;
unordered_map<int,unordered_map<int,Node*>> endList;
openList[start->_x][start->_y]=start;
vector<pair<int,int>> dirs{{1,0},{-1,0},{0,1},{0,-1}};
bool isFind = false;
while(!isFind) {
// find min
auto minNode = findMin(openList);
// four directions
if(minNode==nullptr) break;
for(auto&val: dirs) {
auto nx = val.first+minNode->_x;
auto ny = val.second+minNode->_y;
if(nx == end->_x&&ny == end->_y) {
isFind = true;
end->parent = minNode;
break;
}
check(nx,ny,minNode,openList,endList,end,mGraph);
}
if(isFind) break;
endList[minNode->_x][minNode->_y] = minNode;
openList[minNode->_x].erase(openList[minNode->_x].find(minNode->_y));
}

if(!isFind) return {};
vector<Node*> retVec;
auto tmp = end;
while(tmp) {
retVec.push_back(tmp);
tmp = tmp->parent;
}
reverse(retVec.begin(),retVec.end());
return retVec;
}

void check(int x,int y,Node* cur,unordered_map<int,unordered_map<int,Node*>>& openList, unordered_map<int,unordered_map<int,Node*>> &endList,Node* end, unordered_map<int,unordered_map<int,int>>& mGraph) {
if (x<0||x>=HEIGHT||y<0||y>=WIDTH)
{
return;
}
if(mGraph[x][y]==-1) return;
if (endList.find(x)!=endList.end()&&endList[x].find(y)!=endList[x].end())
{
return;
}

if(openList.find(x)!=openList.end()&&openList[x].find(y)!=openList[x].end()) {
auto t = openList[x][y];
if (cur->g+1+t->h<t->f)
{
t->g = cur->g+1;
t->f = t->g+t->h;
t->parent = cur;
return;
}
}
Node * newNode = new Node(x,y);
newNode->g = cur->g+1;
newNode->h = calculateH(newNode,end);
newNode->f = newNode->g+newNode->h;
openList[x][y] = newNode;
newNode->parent = cur;
}

int calculateH(Node* cur,Node* end) {
return (abs(cur->_x-end->_x)+abs(cur->_y-end->_y));
}
Node* findMin(unordered_map<int,unordered_map<int,Node*>>& openList) {
int minVal = INT_MAX;
Node* n = nullptr;
for(auto &v:openList) {
for(auto&val:v.second) {
if(val.second!=nullptr&&val.second->f<minVal) {
minVal = val.second->f;
n = val.second;
}
}
}
return n;
}