A*寻路算法学习及实现
- 算法简介
A*算法是游戏中常用的一种寻路算法，通过该算法可以求得节点化地图上起点到终点的一条最短路。
它将最短路算法（Dijkstra）和启发式算法（Greedy Best-First-Search）进行了结合。通过代价函数 F=G+Ｈ 进行寻找最短路（G为离起点最近的目标节点的距离；H为该目标节点到终点的距离，起到启发作用）；有效避免了Dijstra寻路过程中的搜索空间很大的缺点，又避免了简单启发式算法在复杂地形下找不到最优路径的问题。具体细节可以参考Stanford的A*简介。 算法流程
- 符号标记：
- 代价函数 F=G+Ｈ ；
- 开列表（open list）和闭列表（close list），openList用于存储当前已搜索过的节点的邻接节点，closeList用于存储已经搜索过的节点，当openList中的F值最小的节点被选中为当前搜索节点（curNode）后，将该节点放入closeList中;
- 父节点（parentNode），表示该节点到起点所需要经过的最短路中的第一个节点；
- 相邻节点间的距离（D）：垂直或水平相邻距离为10，斜对角相邻距离为14；
- 地图（map），起点（S），终点（D），当前搜索节点（curNode）；
- map是经节点化处理过的，这里将地图处理成网格，每个网格即为一个节点；
- 每个节点有四种状态：
普通状态（normal）表示该节点为可通过节点；
障碍状态（obstacle）表示该节点为障碍物，不可通过；
开状态（open）表示该节点在openList中；
闭状态（close）表示该节点在closeList中;
- 流程：
- 初始化：生成map；openList、closeList为空；curNode = S;
- while(curNode != D) —- 一直搜索到终点加入到openList结束
- closeList.push_back(curNode) —- 搜索过的节点放入闭集合；
- 更新当前搜索的节点curNode的邻接节点adjNodes状态：
- 对adjNodes中的normal节点（nd）：将其父节点设为curNode，并将该节点加入到openList中，然后计算该节点的代价值（G = curNode.G + nd.D, H = nd到终点的Manhattan距离，即从nd到终点D只进行横向和纵向移动所需要经过的距离，F = G+H）；
- 对adjNodes中的open节点（od）：比较其经过curNode到起点的代价(G’ = curNode.G + od.D)与该点原来到起点的代价(G)做比较: 如果 G’ < G,则更新od的父节点为curNode，并更新其代价函数值（G=G’,F = G + H）;否则不做更改；
- 对adjNode中的其他类型节点：不做处理;
- 查找当前openList中F值最小的节点，赋给curNode。然后将其保存到closeList中，并从openList中移除；
- 搜索到起点S 到终点 D的路径后，从终点D开始，逐一查找其父节点，直到起点S，便找到了起点到终点间的一条最短路;
- 符号标记：
寻路结果
- map：
- 寻路：
- 终点位置1,寻路结果
- 终点位置2寻路结果
- 终点位置1,寻路结果
- map：
- 代码实现
#include <iostream>
#include <vector>
#include <algorithm>
#include "DebugFunc.h"
typedef unsigned int uint;
typedef enum ENodeFlag
{
ENF_OPEN, //open
ENF_CLOSE, //close
ENF_NORMAL, //normal
ENF_OBSTACLE, //obstacle
}ENodeFlag;
typedef struct SPoint
{
int x; //地图节点的横坐标;
int y; //地图节点的纵坐标;
SPoint(int ax = -1, int ay = -1)
{
x = ax; y = ay;
}
const SPoint operator=(const SPoint &p)
{
x = p.x;
y = p.y;
return *this;
}
bool operator!=(const SPoint &p)
{
return (p.x != x) || (p.y != y);
}
}SPoint;
typedef struct SCost
{
int G; //起点到当前节点的cost;
int H; //当前节点到终点的预估cost;
int F; //总代价，F=G+H;
SCost()
{
G = H = F = 0;
}
}SCost;
typedef struct SMap
{
char m; //地图节点表示;
char f; //节点状态标记,open/close/normal;
SCost c; //路径代价;
SPoint p; //父节点坐标;
SMap()
{
m = '.'; f = ENF_NORMAL;
}
}SMap;
typedef std::vector<std::vector<SMap>> TArray; // 定义存储矩阵的类型;
const uint hoffset = 10; //水平移动单位距离;
const uint soffset = 14; //斜对角移动的单位距离;
const uint INFI = 0xFFFFFFF;//无穷值;
using namespace std;
void AStar(TArray &map, const SPoint &s, const SPoint &d);
int main(int argc, char **argv)
{
//生成地图;
const int mapSize = 30;
TArray map(mapSize, vector<SMap>(mapSize));
SPoint s, d;
s.x = 1; s.y = 9; //起点坐标;
d.x = 28; d.y = 17; //终点坐标;
map[s.x][s.y].m = 'S';
map[d.x][d.y].m = 'D';
for (int i = 2; i < mapSize - 5; ++i)
{
map[mapSize/2][i].m = '0';
map[mapSize/2][i].f = ENF_OBSTACLE;
}
for (int i = 1; i < 10; ++i)
{
map[i][mapSize/2].m = '0';
map[i][mapSize/2].f = ENF_OBSTACLE;
map[mapSize - i][d.y - 2].m = '0';
map[mapSize - i][d.y - 2].f = ENF_OBSTACLE;
}
for (int i = d.y - 2; i < d.y + 4; ++i)
{
map[d.x - 2][i].m = '0';
map[d.x - 2][i].f = ENF_OBSTACLE;
}
//打印地图;
PRINT("'.'--普通地面\t'0'--障碍物\t'S'--起点\n'D'--终点\t'o'--S到D的路径\n");
for (uint x = 0; x < mapSize; ++x)
{
for (uint y = 0; y < mapSize; ++y)
{
PRINT("%c ", map[x][y].m);
}
PRINT("\n");
}
AStar(map, s, d); //进行寻路;
//显示寻路结果;
SPoint curNode = map[d.x][d.y].p;
while (curNode != s)
{
map[curNode.x][curNode.y].m = 'o';
curNode = map[curNode.x][curNode.y].p;
}
PRINT("-----------------------------------------------------\n");
for (uint x = 0; x < mapSize; ++x)
{
for (uint y = 0; y < mapSize; ++y)
{
PRINT("%c ", map[x][y].m);
}
PRINT("\n");
}
system("pause");
return 0;
}
void UpdateOpenNode(TArray &map, const SPoint &curNode,
const SPoint &endNode, vector<SPoint> &openList)
{
int sX = 0, sY = 0, eX = 0, eY = 0;
sX = (curNode.x - 1) >= 0 ? curNode.x - 1 : 0;
eX = (curNode.x + 1) < map.size() ? curNode.x + 1 : map.size() - 1;
sY = (curNode.y - 1) >= 0 ? curNode.y - 1 : 0;
eY = (curNode.y + 1) < map.size() ? curNode.y + 1 : map.size() - 1;
for (int curX = sX; curX <= eX; ++curX)
{
for (int curY = sY; curY <= eY; ++curY)
{
if (map[curX][curY].f == ENF_NORMAL)
{
openList.push_back(SPoint(curX, curY));
map[curX][curY].p = curNode;
map[curX][curY].c.G = map[curNode.x][curNode.y].c.G
+ static_cast<int>(sqrt((float)(abs(curX - curNode.x)
+ abs(curY - curNode.y))) * 10);
map[curX][curY].c.H = (abs(curX - endNode.x) + abs(curY - endNode.y)) * 10;
map[curX][curY].c.F = map[curX][curY].c.H + map[curX][curY].c.G;
map[curX][curY].f = ENF_OPEN;
}
else if (map[curX][curY].f == ENF_OPEN)
{
int curG = map[curNode.x][curNode.y].c.G
+ static_cast<int>(sqrt((float)(abs(curX - curNode.x)
+ abs(curY - curNode.y))) * 10);
if (curG < map[curX][curY].c.G) //经过当前节点的路径更短;
{
map[curX][curY].c.G = curG;
map[curX][curY].c.F = curG +map[curX][curY].c.H;
map[curX][curY].p = curNode;
}
}
}
}
}
void AStar(TArray &map, const SPoint &s, const SPoint &d)
{
uint mapSize = map.size();
vector<SPoint> openList;
openList.reserve(mapSize);
SPoint curNode = s;
while(curNode != d)
{
map[curNode.x][curNode.y].f = ENF_CLOSE; //将当前节点加入闭节点列表;
UpdateOpenNode(map, curNode, d, openList); //更新当前节点周围的相邻节点到开列表中;
//查找当前开列表中cost最小的节点;这里可做优化，将cost信息保存进来，用堆结构加速查找过程;
int minCost = INFI;
vector<SPoint>::iterator itor = openList.begin(), minItor;
for (; itor != openList.end(); ++itor)
{
if (map[itor->x][itor->y].c.F < minCost)
{
minItor = itor;
curNode = *itor;
minCost = map[itor->x][itor->y].c.F;
}
}
openList.erase(minItor); //从openList中移除当前节点;
}
}
扫一扫
298
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
