寫在前面

再來填一個坑。之前一直說要寫的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;
 }