devbox
2023面试高手
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

基于C++实现的A*算法（链表和二叉堆实现）_a*算法是不是必须用到链表?

作者：2023面试高手 | 2024-05-02 21:30:12

赞

踩

a*算法是不是必须用到链表?

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

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

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

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

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

AStar.h文件

  1. #pragma once
  2. #include<vector>
  3. #include<list>
  4. #include<memory>
  5. #include<algorithm>
  6. #include<iostream>
  7. struct ANode
  8. {
  9.     ANode(int X, int Y, std::shared_ptr<ANode> p = nullptr) : x(X), y(Y), prev(p) {}
  10.     int x; //点的x坐标
  11.     int y; //点的y坐标
  12.     int G=0; //起点到该点的欧拉距离
  13.     int H=0;//该点到终点的曼哈顿距离
  14.     int F=0;//G+H
  15.     std::weak_ptr<ANode> prev;//指向的前一个节点!!!改成了weak_ptr不然数据多的时候析构报错
  16.  
  17. };
  18. class AStar
  19. {
  20. public:
  21.     AStar(std::vector<std::vector<int>> m ): maps(m) {}
  22.     std::shared_ptr<ANode> findPath(std::shared_ptr<ANode> beg, std::shared_ptr<ANode> end);
  23.     void PrintAStarPath(const std::pair<int, int> &, const std::pair<int, int> &);
  24.     ~AStar()
  25.     {
  26.         openlist.clear();
  27.         closeist.clear();
  28.     }
  29. private:
  30.     void refreshOpenList(std::shared_ptr<ANode>, std::shared_ptr<ANode> end);
  31.     int calculateH(std::shared_ptr<ANode>, std::shared_ptr<ANode>) const;
  32.     int calculateF(std::shared_ptr<ANode>,std::shared_ptr<ANode>) const;
  33.  
  34. private:
  35.     std::vector<std::vector<int>> maps;//地图
  36.     std::list<std::shared_ptr<ANode>> openlist;//保存还未遍历过的节点
  37.     std::list<std::shared_ptr<ANode>> closeist;//保存已经找到最短路径的节点
  38.     const static int costLow;//上下位移的距离
  39.     const static int costHigh;//斜向位移的距离
  40. };
  41. const int AStar::costLow = 10;
  42. const int AStar::costHigh = 14;
  43. int AStar::calculateH(std::shared_ptr<ANode> point, std::shared_ptr<ANode> end) const
  44. {
  45.     return costLow * (std::abs(point->x - end->x) + std::abs(point->y - end->y));
  46. }
  47. int AStar::calculateF(std::shared_ptr<ANode> point,std::shared_ptr<ANode> end) const
  48. {
  49.     return point->G + calculateH(point, end);
  50. }
  51. std::shared_ptr<ANode> AStar::findPath(std::shared_ptr<ANode> beg, std::shared_ptr<ANode> end)
  52. {
  53.     refreshOpenList(beg,end);
  54.     while (!openlist.empty())
  55.     {
  56.         auto iter = std::min_element(openlist.cbegin(), openlist.cend(), [](std::shared_ptr<ANode> p1, std::shared_ptr<ANode> p2)
  57.                                      { return p1->F <= p2->F;});
  58.         closeist.push_back(*iter);
  59.         std::shared_ptr<ANode> iter_temp = *iter;
  60.         openlist.erase(iter);
  61.         refreshOpenList(iter_temp, end);
  62.         auto iter2 = std::find_if(openlist.cbegin(), openlist.cend(), [end](std::shared_ptr<ANode> sp)
  63.                                   { return (sp->x == end->x) && (sp->y == end->y); });
  64.         if (iter2 != openlist.end())
  65.             return *iter2;
  66.     }
  67.     return nullptr;
  68. }
  69. void AStar::refreshOpenList(std::shared_ptr<ANode> point, std::shared_ptr<ANode> end)
  70. {
  71.     bool upIsWall = false;//表示当前点上有障碍物，即对应的斜向没法走
  72.     bool downIsWall = false;
  73.     bool leftIsWall = false;
  74.     bool rightIsWall = false;
  75.     if (point->x - 1 >= 0 && maps[point->x - 1][point->y] == 1)
  76.         upIsWall = true;
  77.     if (point->x + 1 < int(maps.size()) && maps[point->x + 1][point->y] == 1)
  78.         downIsWall = true;
  79.     if (point->y - 1 >= 0 && maps[point->x][point->y - 1] == 1)
  80.         leftIsWall = true;
  81.     if (point->y + 1 < int(maps.front().size()) && maps[point->x][point->y + 1] == 1)
  82.         rightIsWall = true;
  83.     //第一步初始化起点及其周围的节点
  84.     if (openlist.empty() && closeist.empty())
  85.     {
  86.         closeist.push_back(point);
  87.         for (int i = point->x - 1; i <= point->x + 1; ++i)
  88.         {
  89.             for (int j = point->y - 1; j <= point->y + 1; ++j)
  90.             {
  91.                 if (i >= 0 && j >= 0 && i < int(maps.size()) && j < int(maps.front().size()) && (i != point->x || j != point->y) && !maps[i][j])
  92.                 {
  93.                     if (i != point->x && j != point->y)
  94.                     {
  95.                         if (leftIsWall && j < point->y)
  96.                             continue;
  97.                         if (rightIsWall && j > point->y)
  98.                             continue;
  99.                         if (upIsWall && i < point->x)
  100.                             continue;
  101.                         if (downIsWall && i > point->x)
  102.                             continue;                       
  103.                     }
  104.                     auto cur = std::make_shared<ANode>(i, j, point);
  105.                     cur->G = (i != point->x && j != point->y) ? costHigh : costLow;
  106.                     cur->H = calculateH(cur, end);
  107.                     cur->F = calculateF(cur, end);
  108.                     openlist.push_back(cur);
  109.                 }
  110.             }
  111.         }
  112.     }
  113.     //刷新每一次找到的起点到当前点的最短路径中的当前点的周围节点情况
  114.     else
  115.     {
  116.         
  117.         for (int i = point->x - 1; i <= point->x + 1; ++i)
  118.         {
  119.             for (int j = point->y - 1; j <= point->y + 1; ++j)
  120.             {
  121.                 if (i >= 0 && j >= 0 && i < int(maps.size()) && j < int(maps.front().size()) && (i != point->x || j != point->y) && !maps[i][j])
  122.                 {
  123.                     if (i != point->x && j != point->y)
  124.                     {
  125.                         if (leftIsWall && j < point->y)
  126.                             continue;
  127.                         if (rightIsWall && j > point->y)
  128.                             continue;
  129.                         if (upIsWall && i < point->x)
  130.                             continue;
  131.                         if (downIsWall && i > point->x)
  132.                             continue;                       
  133.                     }
  134.                     auto cur = std::make_shared<ANode>(i, j, point);
  135.                     cur->G = ((i != point->x && j != point->y) ? costHigh : costLow) + point->G;
  136.                     cur->H = calculateH(cur, end);
  137.                     cur->F = calculateF(cur, end);
  138.                     auto iter_close=std::find_if(closeist.cbegin(), closeist.cend(), [i,j](std::shared_ptr<ANode> sp)
  139.                                   { return (sp->x == i) && (sp->y == j); });
  140.                     if (iter_close == closeist.end())
  141.                     {
  142.                         auto iter_open=std::find_if(openlist.cbegin(), openlist.cend(), [i,j](std::shared_ptr<ANode> sp)
  143.                                   { return (sp->x == i) && (sp->y == j); });
  144.                         if (iter_open != openlist.end())
  145.                         {
  146.                             if((*iter_open)->G > cur->G)
  147.                             {
  148.                                 (*iter_open)->G = cur->G;
  149.                                 (*iter_open)->F = (*iter_open)->G + (*iter_open)->H;
  150.                                 (*iter_open)->prev = point;
  151.                             }
  152.                         }
  153.                         else
  154.                            openlist.push_back(cur); 
  155.                     }
  156.                 }
  157.             }
  158.         }
  159.     }
  160. }
  161. void AStar::PrintAStarPath(const std::pair<int, int>& start, const std::pair<int, int>& end)
  162. {
  163.     auto start_sp = std::make_shared<ANode>(start.first, start.second), end_sp = std::make_shared<ANode>(end.first, end.second);
  164.     std::shared_ptr<ANode> final = findPath(start_sp, end_sp);
  165.     if (!final)
  166.         std::cout << "没有找到起点到终点路径" << std::endl;
  167.     else
  168.     {
  169.         while (final)
  170.         {
  171.             maps[final->x][final->y] = '*';
  172.             final = final->prev.lock();
  173.         }
  174.          for (const auto &i : maps)
  175.          {
  176.              for (const auto &j : i)
  177.              {
  178.                  if (j > 1)
  179.                      std::cout << char(j) << " ";
  180.                  else
  181.                      std::cout << j << " ";
  182.              }
  183.              std::cout << std::endl;
  184.          }
  185.     }
  186. }

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

  1. #pragma once
  2. #include<vector>
  3. #include<list>
  4. #include<memory>
  5. #include<algorithm>
  6. #include<iostream>
  7. struct ANode
  8. {
  9.     ANode(int X, int Y, std::shared_ptr<ANode> p = nullptr) : x(X), y(Y), prev(p) {}
  10.     int x; //点的x坐标
  11.     int y; //点的y坐标
  12.     int G=0; //起点到该点的欧拉距离
  13.     int H=0;//该点到终点的曼哈顿距离
  14.     int F=0;//G+H
  15.     std::weak_ptr<ANode> prev;//指向的前一个节点
  16. };
  17. class AStar
  18. {
  19. public:
  20.     AStar(std::vector<std::vector<int>> m ): maps(m) {}
  21.     std::shared_ptr<ANode> findPath(std::shared_ptr<ANode> beg, std::shared_ptr<ANode> end);
  22.     void PrintAStarPath(const std::pair<int, int> &, const std::pair<int, int> &);
  23.     ~AStar()
  24.     {
  25.         openlist.clear();
  26.         closeist.clear();
  27.     }
  28. private:
  29.     void refreshOpenList(std::shared_ptr<ANode>, std::shared_ptr<ANode> end);
  30.     int calculateH(std::shared_ptr<ANode>, std::shared_ptr<ANode>) const;
  31.     int calculateF(std::shared_ptr<ANode>,std::shared_ptr<ANode>) const;
  32.     void HeapSort(int beg, int end);
  33.     void HeapSort(int end);
  34.  
  35. private:
  36.     std::vector<std::vector<int>> maps;//地图
  37.     std::vector<std::shared_ptr<ANode>> openlist;//保存还未遍历过的节点
  38.     std::list<std::shared_ptr<ANode>> closeist;//保存已经找到最短路径的节点
  39.     const static int costLow;//上下位移的距离
  40.     const static int costHigh;//斜向位移的距离
  41. };
  42. const int AStar::costLow = 10;
  43. const int AStar::costHigh = 14;
  44. int AStar::calculateH(std::shared_ptr<ANode> point, std::shared_ptr<ANode> end) const
  45. {
  46.     return costLow * (std::abs(point->x - end->x) + std::abs(point->y - end->y));
  47. }
  48. int AStar::calculateF(std::shared_ptr<ANode> point,std::shared_ptr<ANode> end) const
  49. {
  50.     return point->G + calculateH(point, end);
  51. }
  52. void AStar::HeapSort(int beg, int end)
  53. {
  54.     auto temp = openlist[beg];
  55.     int pre = beg;
  56.     for (int i = pre * 2 + 1; i <= end; i = i * 2 + 1)
  57.     {
  58.         if (i + 1 <= end && openlist[i + 1]->F < openlist[i]->F)
  59.             ++i;
  60.         if (openlist[i]->F <= temp->F)
  61.             openlist[pre] = openlist[i];
  62.         else
  63.             break;
  64.         pre = i;
  65.     }
  66.     openlist[pre] = temp;
  67. }
  68. void AStar::HeapSort(int end)
  69. {
  70.     int pre = int(end) - 1;
  71.     int i = ((int(end) - 1) % 2 == 0) ? (int(end) - 1) / 2 - 1 : (int(openlist.size()) - 1) / 2;
  72.     for (; i >= 0; i = (i % 2 == 0) ? i / 2 - 1 : i / 2)
  73.     {
  74.         if (openlist[i]->F >= openlist[pre]->F)
  75.             std::swap(openlist[i], openlist[pre]);
  76.         else
  77.             break;
  78.         pre = i;
  79.     }
  80. }
  81. std::shared_ptr<ANode> AStar::findPath(std::shared_ptr<ANode> beg, std::shared_ptr<ANode> end)
  82. {
  83.     refreshOpenList(beg,end);
  84.     while (!openlist.empty())
  85.     {
  86.         auto iter2 = std::find_if(openlist.cbegin(), openlist.cend(), [end](std::shared_ptr<ANode> sp)
  87.                                   { return (sp->x == end->x) && (sp->y == end->y); });
  88.         if (iter2 != openlist.end())
  89.             return *iter2;
  90.         auto iter_temp = openlist.front();
  91.         std::swap(openlist[0], openlist[openlist.size() - 1]);
  92.         openlist.pop_back();
  93.         HeapSort(0,openlist.size()-1);
  94.         closeist.push_back(iter_temp);
  95.         refreshOpenList(iter_temp, end);
  96.     }
  97.     return nullptr;
  98. }
  99. void AStar::refreshOpenList(std::shared_ptr<ANode> point, std::shared_ptr<ANode> end)
  100. {
  101.     bool upIsWall = false;//表示当前点上有障碍物，即对应的斜向没法走
  102.     bool downIsWall = false;
  103.     bool leftIsWall = false;
  104.     bool rightIsWall = false;
  105.     if (point->x - 1 >= 0 && maps[point->x - 1][point->y] == 1)
  106.         upIsWall = true;
  107.     if (point->x + 1 < int(maps.size()) && maps[point->x + 1][point->y] == 1)
  108.         downIsWall = true;
  109.     if (point->y - 1 >= 0 && maps[point->x][point->y - 1] == 1)
  110.         leftIsWall = true;
  111.     if (point->y + 1 < int(maps.front().size()) && maps[point->x][point->y + 1] == 1)
  112.         rightIsWall = true;
  113.     //第一步初始化起点及其周围的节点
  114.     if (openlist.empty() && closeist.empty())
  115.     {
  116.         closeist.push_back(point);
  117.         for (int i = point->x - 1; i <= point->x + 1; ++i)
  118.         {
  119.             for (int j = point->y - 1; j <= point->y + 1; ++j)
  120.             {
  121.                 if (i >= 0 && j >= 0 && i < int(maps.size()) && j < int(maps.front().size()) && (i != point->x || j != point->y) && !maps[i][j])
  122.                 {
  123.                     if (i != point->x && j != point->y)
  124.                     {
  125.                         if (leftIsWall && j < point->y)
  126.                             continue;
  127.                         if (rightIsWall && j > point->y)
  128.                             continue;
  129.                         if (upIsWall && i < point->x)
  130.                             continue;
  131.                         if (downIsWall && i > point->x)
  132.                             continue;                       
  133.                     }
  134.                     auto cur = std::make_shared<ANode>(i, j, point);
  135.                     cur->G = (i != point->x && j != point->y) ? costHigh : costLow;
  136.                     cur->H = calculateH(cur, end);
  137.                     cur->F = calculateF(cur, end);
  138.                     openlist.push_back(cur);
  139.                 }
  140.             }
  141.         }
  142.         int i = ((int(openlist.size()) - 1) % 2 == 0) ? (int(openlist.size()) - 1) / 2 - 1 : (int(openlist.size()) - 1) / 2;
  143.         for (; i >= 0; --i)
  144.             HeapSort(i, openlist.size() - 1);
  145.     }
  146.     //刷新每一次找到的起点到当前点的最短路径中的当前点的周围节点情况
  147.     else
  148.     {
  149.         
  150.         for (int i = point->x - 1; i <= point->x + 1; ++i)
  151.         {
  152.             for (int j = point->y - 1; j <= point->y + 1; ++j)
  153.             {
  154.                 if (i >= 0 && j >= 0 && i < int(maps.size()) && j < int(maps.front().size()) && (i != point->x || j != point->y) && !maps[i][j])
  155.                 {
  156.                     if (i != point->x && j != point->y)
  157.                     {
  158.                         if (leftIsWall && j < point->y)
  159.                             continue;
  160.                         if (rightIsWall && j > point->y)
  161.                             continue;
  162.                         if (upIsWall && i < point->x)
  163.                             continue;
  164.                         if (downIsWall && i > point->x)
  165.                             continue;                       
  166.                     }
  167.                     auto cur = std::make_shared<ANode>(i, j, point);
  168.                     cur->G = ((i != point->x && j != point->y) ? costHigh : costLow) + point->G;
  169.                     cur->H = calculateH(cur, end);
  170.                     cur->F = calculateF(cur, end);
  171.                     auto iter_close=std::find_if(closeist.cbegin(), closeist.cend(), [i,j](std::shared_ptr<ANode> sp)
  172.                                   { return (sp->x == i) && (sp->y == j); });
  173.                     if (iter_close == closeist.end())
  174.                     {
  175.                         auto iter_open=std::find_if(openlist.cbegin(), openlist.cend(), [i,j](std::shared_ptr<ANode> sp)
  176.                                   { return (sp->x == i) && (sp->y == j); });
  177.                         if (iter_open != openlist.end())
  178.                         {
  179.                             if((*iter_open)->G > cur->G)
  180.                             {
  181.                                 (*iter_open)->G = cur->G;
  182.                                 (*iter_open)->F = (*iter_open)->G + (*iter_open)->H;
  183.                                 (*iter_open)->prev = point;
  184.                                 HeapSort(int(iter_open - openlist.begin()) + 1);
  185.                             }
  186.                         }
  187.                         else
  188.                         {
  189.                            openlist.push_back(cur);
  190.                            HeapSort(int(openlist.size()));
  191.                         }
  192.                     }
  193.                 }
  194.             }
  195.         }
  196.     }
  197. }
  198. void AStar::PrintAStarPath(const std::pair<int, int>& start, const std::pair<int, int>& end)
  199. {
  200.     auto start_sp = std::make_shared<ANode>(start.first, start.second), end_sp = std::make_shared<ANode>(end.first, end.second);
  201.     std::shared_ptr<ANode> cur = findPath(start_sp, end_sp);
  202.     if (!cur)
  203.         std::cout << "没有找到起点到终点路径" << std::endl;
  204.     else
  205.     {
  206.         while (cur)
  207.         {
  208.             maps[cur->x][cur->y] = '*';
  209.             cur = cur->prev.lock();
  210.         }
  211.          for (const auto &i : maps)
  212.          {
  213.              for (const auto &j : i)
  214.              {
  215.                  if (j > 1)
  216.                      std::cout << char(j);
  217.                  else
  218.                      std::cout << j;
  219.              }
  220.              std::cout << std::endl;
  221.          }
  222.     }
  223. }

主函数文件

  1. #include"AStar.h"
  2. int main()
  3. {
  4.     std::vector<std::vector<int>> map = {{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
  5.                                          {1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1},
  6.                                          {1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1},
  7.                                          {1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},
  8.                                          {1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},
  9.                                          {1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},
  10.                                          {1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},
  11.                                          {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}};
  12. ​
  13.     AStar star(map);
  14.     star.PrintAStarPath({1, 1}, {6, 10});
  15.     return 0;
  16. }

​最终结果

小型地图：

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

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

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/2023面试高手/article/detail/526210
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号