天天看點
傳回

資料結構和算法總結（三）：A* 尋路算法

前言

複習下尋路相關的東西，而且A star尋路在遊戲開發中應用挺多的，故記錄下。

正文

迪傑斯特拉算法

說起A*得先談談Dijkstra算法，它是在BFS基礎上的一種帶權值的兩點最短尋路貪心算法。

算法步驟

0.初始化圖，輸入起點，将所有點到起始點的距離設定為∞。

1.将起始點OriginNode記錄為已通路，并從OriginNode開始将周圍的點加入到待周遊清單中，更新到達這些點的距離，并将他們的父節點設定為起始點OriginNode。

2.如果待周遊清單不為空，則從清單中取出到達距離最小的點currentNode；反之則跳到第5步。

3.周遊currentNode鄰接的點neighbors:

         1）  如果已通路過，則周遊其他點。

         2） 如果鄰接的某點neighbor已經在待周遊清單中，則比較neighbor目前的距離distance(OriginNode,neighbor) 與  distance(OriginNode,currentNode) + distance(currentNode,neighbor), 如果後者更小，則将neighbor的距離值更新為該值并将父節點設定為currentNode。

         3） 如果不在待周遊清單，則将neighbor放入待周遊清單，并則将neighbor的距離值更新為  distance(OriginNode,currentNode) + distance(currentNode,neighbor), 并将父節點設定為currentNode。

4.重複第2步

5.輸入終點，從終點開始回溯父節點，列印路徑。

動态過程

資料結構和算法總結（三）：A* 尋路算法

A* 尋路算法

A*的思路其實與Dijkstra一緻，相比較Dijkstra，A*引入了一個啟發公式來衡量消耗 

其中 g(n) 代表從起始點到目前點的消耗，h(n) 代表終點到目前點的預估消耗（通常用曼哈頓距離或者歐拉距離衡量，這裡不贅述，參考）。

算法思路

/*
A*尋路算法

   帶權值的BFS
   每個點儲存達到該點的消耗F = G值（出發點到目前點的代價值） + H值（目标點到目前點的預估距離，常用曼哈頓距離或者歐拉距離）
   維護着兩個清單，開放清單和關閉清單

   邏輯:
   初始化清單Close,Open,Path,将StartNode放入Open清單中
   while(true)
   {
       從Open表中取得F值最小的節點CurrentNode.
       從Open表中删除CurrentNode
       将CurrentNode加入Close表中
       if ( CurrentNode 是 目标點targetNode)
       {
           從CurrentNode點回溯直到起點并将所有回溯的節點加入Path表
           列印Path表
           return;
       }
       for (CurrentNode 的每一個鄰接點 neighbor)
       {
           if(neighbor 在 Close表中 || neighbor 不可通行)
           {
               continue;
           }
           if(neighbor 在 Open表中)
           {
               if( CurrentNode 到達neighbor的消耗costG + node的G值 < neighbor的G值)
               {
                   更新neighbor的G值;
                   将neighbor的回溯節點fatherNode設定為CurrentNode;
               }
           }
           else
           {
               更新neighbor的G值;
               更新neighbor的H值;
               将neighbor的回溯節點fatherNode設定為CurrentNode;
               将neighbor放入Open清單;
           }
       }
   }
 */
資料結構和算法總結（三）：A* 尋路算法

代碼:

AstarNode的定義實作.

Astar.h

#pragma once
#include <vector>
#include <math.h>

using std::vector;

class AstarNode
{
private:
    int m_igValue;
    int m_ihValue;
    int m_ifValue;
    AstarNode* m_father;
public:
    int x;
    int y;
    bool isPass;
public:
    void   SetG(int iValue);
    int    GetG() const;
    int    GetH() const;
    int    GetF() const;
    void   CalculateH(const AstarNode* endNode);
    void   SetFather(AstarNode* node);
    AstarNode*   GetFather() const;
    bool isInList(const vector<AstarNode*>& nodeList) const;
public:
    AstarNode();
    AstarNode(int x,int y);
    AstarNode(const AstarNode& node);
    ~AstarNode();
    AstarNode& operator=(const AstarNode& node);
};

Astar.cpp

#include "Astar.h"

AstarNode::AstarNode()
{
    isPass = true;
    m_igValue = 0;
    m_ihValue = 0;
    m_ifValue = 0;
    m_father = nullptr;
}

AstarNode::AstarNode(int x,int y)
{
    isPass = true;
    m_igValue = 0;
    m_ihValue = 0;
    m_ifValue = 0;
    m_father = nullptr;
    this->x = x;
    this->y = y;
}

AstarNode::AstarNode(const AstarNode& node)
{
    isPass = node.isPass;
    m_igValue = node.GetG();
    m_ihValue = node.GetH();
    m_ifValue = node.GetF();
    m_father = node.GetFather();
    this->x = node.x;
    this->y = node.y;
}

AstarNode& AstarNode::operator=(const AstarNode& node)
 {
    isPass = node.isPass;
    m_igValue = node.GetG();
    m_ihValue = node.GetH();
    m_ifValue = node.GetF();
    m_father = node.GetFather();
    this->x = node.x;
    this->y = node.y;
    return *this;
 }

AstarNode::~AstarNode()
{

}

void AstarNode::SetG(int iValue)
{
    m_igValue = iValue;
}

int AstarNode::GetG() const
{
    return m_igValue;
}

int AstarNode::GetF() const
{
    return m_igValue + m_ihValue;
}

int AstarNode::GetH() const
{
    return m_ihValue;
}

void AstarNode::CalculateH(const AstarNode* endNode)
{
    m_ihValue = abs(endNode->x - x) + abs(endNode->y - y);
}

void AstarNode::SetFather(AstarNode* node)
{
    m_father = node;
}

AstarNode* AstarNode::GetFather() const
{
    return m_father;
}

bool AstarNode::isInList(const vector<AstarNode*>& nodeList) const
{
    for(auto iter = nodeList.begin();iter != nodeList.end();iter++)
    {
        if((*iter)->x == this->x && (*iter)->y == this->y)
              return true;
    }
    return false;
}

main.cpp

#include "Astar.cpp"
#include <algorithm>
using namespace std;

/*
   'e' 終點
   's' 起點
   '#' 障礙物
   '-' 可通行點
   'o' 回溯路徑節點
*/

vector<vector<char>> graph = {
    {'-', '-', '-', '-', '-', '-', '-'},
    {'-', '-', 'e', '#', '-', '-', '-'},
    {'-', '#', '#', '#', '#', '-', '-'},
    {'-', '-', '-', '-', '#', '#', '-'},
    {'-', '-', '#', '-', '#', '-', '-'},
    {'-', '-', '-', '#', '-', '-', '-'},
    {'-', '-', '-', '-', '-', '-', 's'},
};

void BuildGraph(vector<vector<AstarNode *>> &astarGraph, AstarNode &start, AstarNode &end)
{
    for (int i = 0; i < astarGraph.size(); i++)
    {
        auto row = astarGraph[i];
        for (int j = 0; j < row.size(); j++)
        {
            char mark = graph[i][j];
            if (mark == 's')
            {
                start.x = i;
                start.y = j;
                start.SetG(0);
                start.CalculateH(&end);
                astarGraph[i][j] = &start;
            }
            else if (mark == 'e')
            {
                end.x = i;
                end.y = j;
                astarGraph[i][j] = &end;
            }
            else if (mark == '#')
            {
                astarGraph[i][j] = new AstarNode(i, j);
                astarGraph[i][j]->isPass = false;
            }
            else
            {
                astarGraph[i][j] = new AstarNode(i, j);
            }
        }
    }
}

void printGraph()
{
    for (int i = 0; i < graph.size(); i++)
    {
        auto line = graph[i];
        for (int j = 0; j < line.size(); j++)
        {
            cout << line[j] << " ";
        }
        cout << endl;
    }
}

vector<AstarNode *>::iterator GetMinFNode(vector<AstarNode *> &openList)
{
    auto tmp = openList.begin();
    for (auto iter = openList.begin(); iter != openList.end(); iter++)
    {
        if ((*iter)->GetF() < (*tmp)->GetF())
        {
            tmp = iter;
        }
    }
    return tmp;
}

inline int GetCost(int xDiff, int yDiff)
{
    if (xDiff == 0 || yDiff == 0)
        return 10;
    return 14;
}

void SearchOneNode(AstarNode &currentNode, AstarNode &neighbor, AstarNode &startNode, AstarNode &endNode, vector<AstarNode *> &openList, vector<AstarNode *> &closeList)
{
    if (neighbor.isInList(closeList) || !neighbor.isPass)
        return;
    int gCost = GetCost(currentNode.x - neighbor.x, currentNode.y - neighbor.y);
    if (neighbor.isInList(openList))
    {
        if (currentNode.GetG() + gCost < neighbor.GetG())
        {
            neighbor.SetG(currentNode.GetG() + gCost);
            neighbor.SetFather(&currentNode);
        }
    }
    else
    {
        neighbor.SetG(currentNode.GetG() + gCost);
        neighbor.SetFather(&currentNode);
        neighbor.CalculateH(&endNode);
        openList.push_back(&neighbor);
    }
}

void Astar()
{
    vector<AstarNode *> OpenList;
    vector<AstarNode *> CloseList;
    vector<AstarNode *> Path;
    size_t len = graph.size();
    vector<vector<AstarNode *>> astarGraph(len, vector<AstarNode *>(len));

    AstarNode startNode;
    AstarNode endNode;

    BuildGraph(astarGraph, startNode, endNode);
    OpenList.push_back(&startNode);
    while (!OpenList.empty())
    {
        auto it = GetMinFNode(OpenList);
        AstarNode *currentNode = *it;
        OpenList.erase(it);
        CloseList.push_back(currentNode);
        if (currentNode->x == endNode.x && currentNode->y == endNode.y)
        {
            while (currentNode->GetFather())
            {
                graph[currentNode->x][currentNode->y] = graph[currentNode->x][currentNode->y] == '-' ? 'o' : 'e';
                Path.push_back(currentNode);
                currentNode = currentNode->GetFather();
            }
            printGraph();
            break;
        }
        int curX = currentNode->x;
        int curY = currentNode->y;
        int row = graph.size();
        int column = row;
        if (curX + 1 < row)
            SearchOneNode(*currentNode, *astarGraph[curX + 1][curY], startNode, endNode, OpenList, CloseList);
        if (curX - 1 >= 0)
            SearchOneNode(*currentNode, *astarGraph[curX - 1][curY], startNode, endNode, OpenList, CloseList);
        if (curY + 1 < column)
            SearchOneNode(*currentNode, *astarGraph[curX][curY + 1], startNode, endNode, OpenList, CloseList);
        if (curY - 1 >= 0)
            SearchOneNode(*currentNode, *astarGraph[curX][curY - 1], startNode, endNode, OpenList, CloseList);
        if (curX - 1 >= 0 && curY - 1 >= 0)
            SearchOneNode(*currentNode, *astarGraph[curX - 1][curY - 1], startNode, endNode, OpenList, CloseList);
        if (curX - 1 >= 0 && curY + 1 < column)
            SearchOneNode(*currentNode, *astarGraph[curX - 1][curY + 1], startNode, endNode, OpenList, CloseList);
        if (curX + 1 < row && curY - 1 >= 0)
            SearchOneNode(*currentNode, *astarGraph[curX + 1][curY - 1], startNode, endNode, OpenList, CloseList);
        if (curX + 1 < row && curY + 1 < row)
            SearchOneNode(*currentNode, *astarGraph[curX + 1][curY + 1], startNode, endNode, OpenList, CloseList);
    }
    cout << endl;
}

int main()
{
    auto start = chrono::steady_clock::now();
    Astar();
    auto end = chrono::steady_clock::now();
    cout << chrono::duration<double, milli>(end - start).count() << " ms" << endl;
    getchar();
}

參考資料

歐式距離與曼哈頓距離       作者:大魚-瓶邪

A* 搜尋算法（Python ）     作者: 漫步_9378

A* Pathfinding (需要梯子)   作者: Sebastian Lague

VisualGO 算法可視化

上一篇: Unity3D學習（十一）：關于UI銷毀後圖集仍然無法釋放問題的解決辦法
下一篇: 配置環境變量