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

最后结果如下：