当前位置:   article > 正文

C++实现A*寻路算法_c a寻路算法

c a寻路算法

算法思想:从起点开始慢慢探索周围的点,直到看到终点。和dijkstra算法一样,整个图是慢慢被探索的,所以也需要定义两个集合,一个是已经处理(探索)过的点的集合,另一个是待处理的点的集合(“待处理的点的集合”并不等于“未处理过的点的集合”,前者指的是已经看到但未处理的点,后者还包括那些在远处尚未看到的点)。

A*算法的核心公式是F(起点到终点的距离)=G(起点到当前点的距离,即已经走过的路)+H(当前点到终点的距离,即还没走过的路)。最开始的时候,程序只能看到起点,此时将起点的G值设置为0,H值设置为起点到终点的距离,H值可以采用欧式距离,也可以采用曼哈顿距离。

与dijkstra算法不同的是,dijkstra每次需要从一个待处理的点的集合里找一个距离起点最近(distance值最小)的点出来进行处理,考虑的仅仅是G值,而A*算法不但要考虑G值,而且要考虑H值,于是有了F=G+H,每次从待处理的点的集合里找F值最小的点,进行处理。

这里的H值是一个估计值(因为当前点到终点之间可能有障碍物,距离到底是多少,暂时无法知道),所以这个算法被称为启发式搜索,和盲目搜索是有区别的。

数据结构如下,这里只做简单演示,实际应用程序的点的个数肯定不止这么多,要用malloc或者new动态申请内存。偷个懒,使用了一些全局变量,避免在函数之间传递参数。

  1. #include <windows.h>
  2. #include <gl/glut.h>
  3. #include <list>
  4. #include <stack>
  5. #include <iostream>
  6. using namespace std;
  7. #define MaxHeight 11
  8. #define MaxWidth 11
  9. #define PointOfBoundary 32
  10. #define PointOfWall 14
  11. struct Point
  12. {
  13. int x;
  14. int y;
  15. };
  16. struct Node
  17. {
  18. Point p;
  19. int F;
  20. int G;
  21. int H;
  22. Node* parent;
  23. char type;
  24. };
  25. Node node[MaxHeight][MaxWidth];
  26. Point boundaryPoint[] = { {0,0},{1,0},{2,0},{3,0},{4,0},{5,0},{6,0},{7,0},{8,0},{8,1},{8,2},{8,3},{8,4},{8,5},{8,6},{8,7},{8,8},{7,8},{6,8},{5,8},{4,8},{3,8},{2,8},{1,8},{0,8},{0,7},{0,6},{0,5},{0,4},{0,3},{0,2},{0,1} };
  27. Point wallPoint[] = { {2,4},{2,5},{2,6},{3,4},{4,4},{5,4},{6,4},{7,4},{5,2},{5,3},{3,6},{4,6},{5,6},{6,2} };
  28. Point startPoint = { 3,5 };
  29. Point targetPoint = { 6,3 };

主要函数如下:

  1. void InitAllNode();
  2. void SetBoundaryNode();
  3. void DrawBoundary();
  4. void SetWallNode();
  5. void DrawWall();
  6. void DrawPoint(Point point);
  7. void FindPath(Point p);
  8. bool Compare(const Node& node1, const Node& node2);
  9. void Display();
  10. int main(int argc, char* argv[])
  11. {
  12. glutInit(&argc, argv);
  13. glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB | GLUT_DEPTH);
  14. glutInitWindowPosition(100, 100);
  15. glutInitWindowSize(800, 800);
  16. glutCreateWindow("FindPath");
  17. glutDisplayFunc(Display);
  18. glutMainLoop();
  19. return 0;
  20. }

算法的主要过程如下,和dijkstra算法很相似。

循环之前,初始化起点的数据,把起点push到open_list中。

while(open_list非空)

{

    1.从open_list中取出F值最小的点进行处理(可以使用优先队列),并将该点放入closed_list中;

    2.将该点的8个邻接点(边界点、障碍物、已经处理过的点,这三者除外)放入open_list中,

    并计算每个点的G值和H值,然后根据F=G+H计算F值;

    当遇到了终点时,break;

}

计算G值时需要注意,对于某一个点,可能有多条路径可以到达这个点,该点的G值可能会不断发生变化,这一点和dijkstra算法一样,每个点的distance值可能会不断发生变化。代码如下,和dijkstra算法里那个比较的语句非常相似。

  1. if (node[currentNode.p.x][currentNode.p.y].G + deltaG < node[neighborNode.p.x][neighborNode.p.y].G)
  2. {
  3. node[neighborNode.p.x][neighborNode.p.y].parent = &node[currentNode.p.x][currentNode.p.y];
  4. node[neighborNode.p.x][neighborNode.p.y].G = node[currentNode.p.x][currentNode.p.y].G + deltaG;
  5. node[neighborNode.p.x][neighborNode.p.y].F = node[neighborNode.p.x][neighborNode.p.y].G + node[neighborNode.p.x][neighborNode.p.y].H;
  6. }

程序运行效果如下:

A寻路

下图中红色的点是障碍物,起点坐标是3,5,终点坐标是6,3。程序运行过程中open_list和closed_list的变化以及F、G、H值的计算过程如下,H值采用欧式距离,并且直接取整。另外,为了方便计算,所有距离值在原始距离值的基础上乘以了10。

最后结果如下:

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

闽ICP备14008679号