基于C++实现的A*算法

• AStar算法相对于Dijkstra算法而言升级的地方在于引入了启发距离，即H（当前点到终点的预计距离），因此在每次大循环中Dijkstra算法找的最短距离仅仅是G（起点到当前点的距离），而AStar算法找的是（G+H）即F的最小值，因此AStar算法可以更快的找到终点，从而在判断两点最短路径时候有着更快的结束时间即更高的效率

• 这两种算法本质上而言均是找起点到每一个点的最短距离，而AStar算法好就好在它引入了每次对终点的度量因而可以比Dijkstra算法更快的找到对终点的距离！

  其具体的原理步骤可以参见其他优秀的blog如：A* 寻路算法 - christanxw的专栏 - C++博客

基于c++的链表的A*算法代码如下：

AStar.h文件

#pragma once
#include<vector>
#include<list>
#include<memory>
#include<algorithm>
#include<iostream>
struct ANode
{
    ANode(int X, int Y, std::shared_ptr<ANode> p = nullptr) : x(X), y(Y), prev(p) {}
    int x; //点的x坐标
    int y; //点的y坐标
    int G=0; //起点到该点的欧拉距离
    int H=0;//该点到终点的曼哈顿距离
    int F=0;//G+H
    std::weak_ptr<ANode> prev;//指向的前一个节点!!!改成了weak_ptr不然数据多的时候析构报错
 
};
class AStar
{
public:
    AStar(std::vector<std::vector<int>> m ): maps(m) {}
    std::shared_ptr<ANode> findPath(std::shared_ptr<ANode> beg, std::shared_ptr<ANode> end);
    void PrintAStarPath(const std::pair<int, int> &, const std::pair<int, int> &);
    ~AStar()
    {
        openlist.clear();
        closeist.clear();
    }
private:
    void refreshOpenList(std::shared_ptr<ANode>, std::shared_ptr<ANode> end);
    int calculateH(std::shared_ptr<ANode>, std::shared_ptr<ANode>) const;
    int calculateF(std::shared_ptr<ANode>,std::shared_ptr<ANode>) const;

private:
    std::vector<std::vector<int>> maps;//地图
    std::list<std::shared_ptr<ANode>> openlist;//保存还未遍历过的节点
    std::list<std::shared_ptr<ANode>> closeist;//保存已经找到最短路径的节点
    const static int costLow;//上下位移的距离
    const static int costHigh;//斜向位移的距离
};
const int AStar::costLow = 10;
const int AStar::costHigh = 14;
int AStar::calculateH(std::shared_ptr<ANode> point, std::shared_ptr<ANode> end) const
{
    return costLow * (std::abs(point->x - end->x) + std::abs(point->y - end->y));
}
int AStar::calculateF(std::shared_ptr<ANode> point,std::shared_ptr<ANode> end) const
{
    return point->G + calculateH(point, end);
}
std::shared_ptr<ANode> AStar::findPath(std::shared_ptr<ANode> beg, std::shared_ptr<ANode> end)
{
    refreshOpenList(beg,end);
    while (!openlist.empty())
    {
        auto iter = std::min_element(openlist.cbegin(), openlist.cend(), [](std::shared_ptr<ANode> p1, std::shared_ptr<ANode> p2)
                                     { return p1->F <= p2->F;});
        closeist.push_back(*iter);
        std::shared_ptr<ANode> iter_temp = *iter;
        openlist.erase(iter);
        refreshOpenList(iter_temp, end);
        auto iter2 = std::find_if(openlist.cbegin(), openlist.cend(), [end](std::shared_ptr<ANode> sp)
                                  { return (sp->x == end->x) && (sp->y == end->y); });
        if (iter2 != openlist.end())
            return *iter2;
    }
    return nullptr;
}
void AStar::refreshOpenList(std::shared_ptr<ANode> point, std::shared_ptr<ANode> end)
{
    bool upIsWall = false;//表示当前点上有障碍物，即对应的斜向没法走
    bool downIsWall = false;
    bool leftIsWall = false;
    bool rightIsWall = false;
    if (point->x - 1 >= 0 && maps[point->x - 1][point->y] == 1)
        upIsWall = true;
    if (point->x + 1 < int(maps.size()) && maps[point->x + 1][point->y] == 1)
        downIsWall = true;
    if (point->y - 1 >= 0 && maps[point->x][point->y - 1] == 1)
        leftIsWall = true;
    if (point->y + 1 < int(maps.front().size()) && maps[point->x][point->y + 1] == 1)
        rightIsWall = true;
    //第一步初始化起点及其周围的节点
    if (openlist.empty() && closeist.empty())
    {
        closeist.push_back(point);
        for (int i = point->x - 1; i <= point->x + 1; ++i)
        {
            for (int j = point->y - 1; j <= point->y + 1; ++j)
            {
                if (i >= 0 && j >= 0 && i < int(maps.size()) && j < int(maps.front().size()) && (i != point->x || j != point->y) && !maps[i][j])
                {
                    if (i != point->x && j != point->y)
                    {
                        if (leftIsWall && j < point->y)
                            continue;
                        if (rightIsWall && j > point->y)
                            continue;
                        if (upIsWall && i < point->x)
                            continue;
                        if (downIsWall && i > point->x)
                            continue;                       
                    }
                    auto cur = std::make_shared<ANode>(i, j, point);
                    cur->G = (i != point->x && j != point->y) ? costHigh : costLow;
                    cur->H = calculateH(cur, end);
                    cur->F = calculateF(cur, end);
                    openlist.push_back(cur);
                }
            }
        }
    }
    //刷新每一次找到的起点到当前点的最短路径中的当前点的周围节点情况
    else
    {
        
        for (int i = point->x - 1; i <= point->x + 1; ++i)
        {
            for (int j = point->y - 1; j <= point->y + 1; ++j)
            {
                if (i >= 0 && j >= 0 && i < int(maps.size()) && j < int(maps.front().size()) && (i != point->x || j != point->y) && !maps[i][j])
                {
                    if (i != point->x && j != point->y)
                    {
                        if (leftIsWall && j < point->y)
                            continue;
                        if (rightIsWall && j > point->y)
                            continue;
                        if (upIsWall && i < point->x)
                            continue;
                        if (downIsWall && i > point->x)
                            continue;                       
                    }
                    auto cur = std::make_shared<ANode>(i, j, point);
                    cur->G = ((i != point->x && j != point->y) ? costHigh : costLow) + point->G;
                    cur->H = calculateH(cur, end);
                    cur->F = calculateF(cur, end);
                    auto iter_close=std::find_if(closeist.cbegin(), closeist.cend(), [i,j](std::shared_ptr<ANode> sp)
                                  { return (sp->x == i) && (sp->y == j); });
                    if (iter_close == closeist.end())
                    {
                        auto iter_open=std::find_if(openlist.cbegin(), openlist.cend(), [i,j](std::shared_ptr<ANode> sp)
                                  { return (sp->x == i) && (sp->y == j); });
                        if (iter_open != openlist.end())
                        {
                            if((*iter_open)->G > cur->G)
                            {
                                (*iter_open)->G = cur->G;
                                (*iter_open)->F = (*iter_open)->G + (*iter_open)->H;
                                (*iter_open)->prev = point;
                            }
                        }
                        else
                           openlist.push_back(cur); 
                    }
                }
            }
        }
    }
}
void AStar::PrintAStarPath(const std::pair<int, int>& start, const std::pair<int, int>& end)
{
    auto start_sp = std::make_shared<ANode>(start.first, start.second), end_sp = std::make_shared<ANode>(end.first, end.second);
    std::shared_ptr<ANode> final = findPath(start_sp, end_sp);
    if (!final)
        std::cout << "没有找到起点到终点路径" << std::endl;
    else
    {
        while (final)
        {
            maps[final->x][final->y] = '*';
            final = final->prev.lock();
        }
         for (const auto &i : maps)
         {
             for (const auto &j : i)
             {
                 if (j > 1)
                     std::cout << char(j) << " ";
                 else
                     std::cout << j << " ";
             }
             std::cout << std::endl;
         }
    }
}

改进后用二叉堆的AStar主文件如下 

#pragma once
#include<vector>
#include<list>
#include<memory>
#include<algorithm>
#include<iostream>
struct ANode
{
    ANode(int X, int Y, std::shared_ptr<ANode> p = nullptr) : x(X), y(Y), prev(p) {}
    int x; //点的x坐标
    int y; //点的y坐标
    int G=0; //起点到该点的欧拉距离
    int H=0;//该点到终点的曼哈顿距离
    int F=0;//G+H
    std::weak_ptr<ANode> prev;//指向的前一个节点
};
class AStar
{
public:
    AStar(std::vector<std::vector<int>> m ): maps(m) {}
    std::shared_ptr<ANode> findPath(std::shared_ptr<ANode> beg, std::shared_ptr<ANode> end);
    void PrintAStarPath(const std::pair<int, int> &, const std::pair<int, int> &);
    ~AStar()
    {
        openlist.clear();
        closeist.clear();
    }
private:
    void refreshOpenList(std::shared_ptr<ANode>, std::shared_ptr<ANode> end);
    int calculateH(std::shared_ptr<ANode>, std::shared_ptr<ANode>) const;
    int calculateF(std::shared_ptr<ANode>,std::shared_ptr<ANode>) const;
    void HeapSort(int beg, int end);
    void HeapSort(int end);

private:
    std::vector<std::vector<int>> maps;//地图
    std::vector<std::shared_ptr<ANode>> openlist;//保存还未遍历过的节点
    std::list<std::shared_ptr<ANode>> closeist;//保存已经找到最短路径的节点
    const static int costLow;//上下位移的距离
    const static int costHigh;//斜向位移的距离
};
const int AStar::costLow = 10;
const int AStar::costHigh = 14;
int AStar::calculateH(std::shared_ptr<ANode> point, std::shared_ptr<ANode> end) const
{
    return costLow * (std::abs(point->x - end->x) + std::abs(point->y - end->y));
}
int AStar::calculateF(std::shared_ptr<ANode> point,std::shared_ptr<ANode> end) const
{
    return point->G + calculateH(point, end);
}
void AStar::HeapSort(int beg, int end)
{
    auto temp = openlist[beg];
    int pre = beg;
    for (int i = pre * 2 + 1; i <= end; i = i * 2 + 1)
    {
        if (i + 1 <= end && openlist[i + 1]->F < openlist[i]->F)
            ++i;
        if (openlist[i]->F <= temp->F)
            openlist[pre] = openlist[i];
        else
            break;
        pre = i;
    }
    openlist[pre] = temp;
}
void AStar::HeapSort(int end)
{
    int pre = int(end) - 1;
    int i = ((int(end) - 1) % 2 == 0) ? (int(end) - 1) / 2 - 1 : (int(openlist.size()) - 1) / 2;
    for (; i >= 0; i = (i % 2 == 0) ? i / 2 - 1 : i / 2)
    {
        if (openlist[i]->F >= openlist[pre]->F)
            std::swap(openlist[i], openlist[pre]);
        else
            break;
        pre = i;
    }
}
std::shared_ptr<ANode> AStar::findPath(std::shared_ptr<ANode> beg, std::shared_ptr<ANode> end)
{
    refreshOpenList(beg,end);
    while (!openlist.empty())
    {
        auto iter2 = std::find_if(openlist.cbegin(), openlist.cend(), [end](std::shared_ptr<ANode> sp)
                                  { return (sp->x == end->x) && (sp->y == end->y); });
        if (iter2 != openlist.end())
            return *iter2;
        auto iter_temp = openlist.front();
        std::swap(openlist[0], openlist[openlist.size() - 1]);
        openlist.pop_back();
        HeapSort(0,openlist.size()-1);
        closeist.push_back(iter_temp);
        refreshOpenList(iter_temp, end);
    }
    return nullptr;
}
void AStar::refreshOpenList(std::shared_ptr<ANode> point, std::shared_ptr<ANode> end)
{
    bool upIsWall = false;//表示当前点上有障碍物，即对应的斜向没法走
    bool downIsWall = false;
    bool leftIsWall = false;
    bool rightIsWall = false;
    if (point->x - 1 >= 0 && maps[point->x - 1][point->y] == 1)
        upIsWall = true;
    if (point->x + 1 < int(maps.size()) && maps[point->x + 1][point->y] == 1)
        downIsWall = true;
    if (point->y - 1 >= 0 && maps[point->x][point->y - 1] == 1)
        leftIsWall = true;
    if (point->y + 1 < int(maps.front().size()) && maps[point->x][point->y + 1] == 1)
        rightIsWall = true;
    //第一步初始化起点及其周围的节点
    if (openlist.empty() && closeist.empty())
    {
        closeist.push_back(point);
        for (int i = point->x - 1; i <= point->x + 1; ++i)
        {
            for (int j = point->y - 1; j <= point->y + 1; ++j)
            {
                if (i >= 0 && j >= 0 && i < int(maps.size()) && j < int(maps.front().size()) && (i != point->x || j != point->y) && !maps[i][j])
                {
                    if (i != point->x && j != point->y)
                    {
                        if (leftIsWall && j < point->y)
                            continue;
                        if (rightIsWall && j > point->y)
                            continue;
                        if (upIsWall && i < point->x)
                            continue;
                        if (downIsWall && i > point->x)
                            continue;                       
                    }
                    auto cur = std::make_shared<ANode>(i, j, point);
                    cur->G = (i != point->x && j != point->y) ? costHigh : costLow;
                    cur->H = calculateH(cur, end);
                    cur->F = calculateF(cur, end);
                    openlist.push_back(cur);
                }
            }
        }
        int i = ((int(openlist.size()) - 1) % 2 == 0) ? (int(openlist.size()) - 1) / 2 - 1 : (int(openlist.size()) - 1) / 2;
        for (; i >= 0; --i)
            HeapSort(i, openlist.size() - 1);
    }
    //刷新每一次找到的起点到当前点的最短路径中的当前点的周围节点情况
    else
    {
        
        for (int i = point->x - 1; i <= point->x + 1; ++i)
        {
            for (int j = point->y - 1; j <= point->y + 1; ++j)
            {
                if (i >= 0 && j >= 0 && i < int(maps.size()) && j < int(maps.front().size()) && (i != point->x || j != point->y) && !maps[i][j])
                {
                    if (i != point->x && j != point->y)
                    {
                        if (leftIsWall && j < point->y)
                            continue;
                        if (rightIsWall && j > point->y)
                            continue;
                        if (upIsWall && i < point->x)
                            continue;
                        if (downIsWall && i > point->x)
                            continue;                       
                    }
                    auto cur = std::make_shared<ANode>(i, j, point);
                    cur->G = ((i != point->x && j != point->y) ? costHigh : costLow) + point->G;
                    cur->H = calculateH(cur, end);
                    cur->F = calculateF(cur, end);
                    auto iter_close=std::find_if(closeist.cbegin(), closeist.cend(), [i,j](std::shared_ptr<ANode> sp)
                                  { return (sp->x == i) && (sp->y == j); });
                    if (iter_close == closeist.end())
                    {
                        auto iter_open=std::find_if(openlist.cbegin(), openlist.cend(), [i,j](std::shared_ptr<ANode> sp)
                                  { return (sp->x == i) && (sp->y == j); });
                        if (iter_open != openlist.end())
                        {
                            if((*iter_open)->G > cur->G)
                            {
                                (*iter_open)->G = cur->G;
                                (*iter_open)->F = (*iter_open)->G + (*iter_open)->H;
                                (*iter_open)->prev = point;
                                HeapSort(int(iter_open - openlist.begin()) + 1);
                            }
                        }
                        else
                        {
                           openlist.push_back(cur);
                           HeapSort(int(openlist.size()));
                        }
                    }
                }
            }
        }
    }
}
void AStar::PrintAStarPath(const std::pair<int, int>& start, const std::pair<int, int>& end)
{
    auto start_sp = std::make_shared<ANode>(start.first, start.second), end_sp = std::make_shared<ANode>(end.first, end.second);
    std::shared_ptr<ANode> cur = findPath(start_sp, end_sp);
    if (!cur)
        std::cout << "没有找到起点到终点路径" << std::endl;
    else
    {
        while (cur)
        {
            maps[cur->x][cur->y] = '*';
            cur = cur->prev.lock();
        }
         for (const auto &i : maps)
         {
             for (const auto &j : i)
             {
                 if (j > 1)
                     std::cout << char(j);
                 else
                     std::cout << j;
             }
             std::cout << std::endl;
         }
    }
}

主函数文件

#include"AStar.h"
int main()
{
    std::vector<std::vector<int>> map = {{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
                                         {1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1},
                                         {1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1},
                                         {1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},
                                         {1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},
                                         {1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},
                                         {1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},
                                         {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}};
​
    AStar star(map);
    star.PrintAStarPath({1, 1}, {6, 10});
    return 0;
}

​最终结果

小型地图：

其中1表示障碍物，0表示可通的道路

在大型地图例如6000*6000的地图，找(0,0)到(5999,5999)的路径时候：

        改进后的算法用时17545ms 改进前的为28566ms