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动态申请内存。偷个懒,使用了一些全局变量,避免在函数之间传递参数。

#include 
#include 
#include 
#include 
#include 
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。

C++实现A*寻路算法_第1张图片

最后结果如下:

C++实现A*寻路算法_第2张图片

你可能感兴趣的:(算法,c++,数据结构,人工智能)