赞
踩
算法思想:从起点开始慢慢探索周围的点,直到看到终点。和dijkstra算法一样,整个图是慢慢被探索的,所以也需要定义两个集合,一个是已经处理(探索)过的点的集合,另一个是待处理的点的集合(“待处理的点的集合”并不等于“未处理过的点的集合”,前者指的是已经看到但未处理的点,后者还包括那些在远处尚未看到的点)。
A*算法的核心公式是F(起点到终点的距离)=G(起点到当前点的距离,即已经走过的路)+H(当前点到终点的距离,即还没走过的路)。最开始的时候,程序只能看到起点,此时将起点的G值设置为0,H值设置为起点到终点的距离,H值可以采用欧式距离,也可以采用曼哈顿距离。
与dijkstra算法不同的是,dijkstra每次需要从一个待处理的点的集合里找一个距离起点最近(distance值最小)的点出来进行处理,考虑的仅仅是G值,而A*算法不但要考虑G值,而且要考虑H值,于是有了F=G+H,每次从待处理的点的集合里找F值最小的点,进行处理。
这里的H值是一个估计值(因为当前点到终点之间可能有障碍物,距离到底是多少,暂时无法知道),所以这个算法被称为启发式搜索,和盲目搜索是有区别的。
数据结构如下,这里只做简单演示,实际应用程序的点的个数肯定不止这么多,要用malloc或者new动态申请内存。偷个懒,使用了一些全局变量,避免在函数之间传递参数。
- #include <windows.h>
- #include <gl/glut.h>
- #include <list>
- #include <stack>
- #include <iostream>
- using namespace std;
-
- #define MaxHeight 11
- #define MaxWidth 11
-
- #define PointOfBoundary 32
- #define PointOfWall 14
-
- struct Point
- {
- int x;
- int y;
- };
-
- struct Node
- {
- Point p;
- int F;
- int G;
- int H;
- Node* parent;
- char type;
- };
-
- Node node[MaxHeight][MaxWidth];
- 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} };
- 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} };
-
- Point startPoint = { 3,5 };
- Point targetPoint = { 6,3 };
主要函数如下:
- void InitAllNode();
- void SetBoundaryNode();
- void DrawBoundary();
- void SetWallNode();
- void DrawWall();
- void DrawPoint(Point point);
- void FindPath(Point p);
- bool Compare(const Node& node1, const Node& node2);
- void Display();
-
- int main(int argc, char* argv[])
- {
- glutInit(&argc, argv);
- glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB | GLUT_DEPTH);
- glutInitWindowPosition(100, 100);
- glutInitWindowSize(800, 800);
- glutCreateWindow("FindPath");
- glutDisplayFunc(Display);
- glutMainLoop();
- return 0;
- }
算法的主要过程如下,和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算法里那个比较的语句非常相似。
- if (node[currentNode.p.x][currentNode.p.y].G + deltaG < node[neighborNode.p.x][neighborNode.p.y].G)
- {
- node[neighborNode.p.x][neighborNode.p.y].parent = &node[currentNode.p.x][currentNode.p.y];
- node[neighborNode.p.x][neighborNode.p.y].G = node[currentNode.p.x][currentNode.p.y].G + deltaG;
- node[neighborNode.p.x][neighborNode.p.y].F = node[neighborNode.p.x][neighborNode.p.y].G + node[neighborNode.p.x][neighborNode.p.y].H;
- }
程序运行效果如下:
A寻路
下图中红色的点是障碍物,起点坐标是3,5,终点坐标是6,3。程序运行过程中open_list和closed_list的变化以及F、G、H值的计算过程如下,H值采用欧式距离,并且直接取整。另外,为了方便计算,所有距离值在原始距离值的基础上乘以了10。
最后结果如下:
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。