C++下基于A*算法的三维航线规划

基于三维栅格空间的A*算法流程C++实现_格栅地图 a* 算法 c++_chase。的博客-CSDN博客

一、A*算法介绍

A*算法的原理是这样，首先，想象你有一块空白的地图，然后在地图上设置几个矩形的障碍物，地图可以用(x,y,z)表示每一块儿栅格，例如10*10*10的立体空间，划分成10*10*10=1000个小立方体，每个立方体表示一个栅格，立方体中心为(x,y,z)，如果障碍物在这个栅格内，则该栅格为1，空白为0。

要做的是，设置一个起点p1和终点p2，寻找一条从起点到终点的最短航线，航线要能绕过障碍物。

具体处理思路：

1）从起点开始，将起点作为当前栅格，获取当前点周围的相邻栅格，然后计算周围每个栅格的代价f，f = g + h，g表示栅格到起点的欧式距离，h表示栅格到终点的估计距离，一般也用欧式距离计算。

2）从周围栅格中选出一个代价f最小的栅格，作为新的当前栅格。将这个新的当前栅格的父栅格指向前一个栅格（此处是为了到达终点的时候，根据父栅格记录出一条完整路径），然后重复1）。

3）直到当前栅格到达终点，则将当前栅格的父节点输出到path路径中。

二、代码

三、结果