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;
 }