A*寻路算法是一种用于静态位置高效搜索算法。他是在平面中两点之间去寻找一条可以到达的最短路径算法。
二维平面之中两个点，寻找可以抵达的最短路径。首先明确三个概念，H值，目前点到终点的曼哈顿距离（曼哈顿距离，就是两个位置长度差值和高度差值的和），G值，目前点到起点的消耗代价值，如果只是寻找路径，可以将该值也看成是这两点的曼哈顿距离，F值，H值和G值的和。
今天我这里讨论的寻路算法，暂时不考虑八个方向，仅仅考虑某个点所对应的上下左右四个方向可以移动。G值处理为距离起点的曼哈顿距离。接下来可能有点绕，但是是该算法的内容核心。
首先定义两个集合，openlist和closelist。openlist用于存放接下来可能会去的点的位置，closelist用于存放已经去过，不会再去的点的位置。然后，选定起点，将起点周边（上下左右）符合要求的点加入openlist，符合的要求需要可以达到，即没有阻挡物并且不存在于closelist中的点。在所有的openlist中找到F值最小的点，然后以该点为下一步行进点，在继续查找该行进点的上下左右符合要求的点。重复操作。结束条件，当openlist为空，没有找到终点，则不存在可以到达的路线，当行进点为终点时，则找到了最小路径。
结合下图，屌丝找女朋友的图来看下。
首先定义openlist和closelist。因为openlist每次需要查找最小F值，所以使用multimap结构，将F值作为键值。
struct stInfo
{
int iF = 0;
int iG = 0; //到起点
int iH = 0;
int iPosx = -1;
int iPosy = -1;
int iFPosx = -1;
int iFPosy = -1;
};
std::multimap<int, stInfo> mapOpenList;
closelist因为需要查找某个位置是否存在于closelist中，如果存在则不能在使用该点，所以以x坐标和y坐标用作键值：std::map<std::pair<int, int>, stInfo> mapCloseList;
查找过程类似于一棵树的广度和深度遍历的集合，在屌丝的行进点的时候，先进行广度遍历，将所有可能的结果存放到openlist中，然后进行深度遍历，找到F值最小的一个分支，作为下一个该屌丝的行进点。同时将上一个行进点加入closelist中，以后将不再考虑该点。至于最后怎么将整条路径打印出来，上边的结构体中定义两个变量iFPosx, iFPosy，他们存放父节点的位置，比如行进点的子节点有四个，那么他们四个的父节点都是该行进点。这里可以将上边的屌丝找女朋友的图，理解成为一棵四叉树。最后结束条件：一种情况是openlist为空，那么该屌丝已经将所有的路都完了，还没有找到女朋友，即该屌丝永远找不到女朋友。另一种情况是行进点是女朋友的点，那么该屌丝找到了女朋友。
代码：
#include <iostream>
#include <map>
#include <vector>
#include <iterator>
#include <set>
#include <cmath>
#include <unordered_map>
#define _CRT_SECURE_NO_WARNINGS
using namespace std;
struct stInfo
{
int iF = 0;
int iG = 0; //到起点
int iH = 0;
int iPosx = -1;
int iPosy = -1;
int iFPosx = -1;
int iFPosy = -1;
};
/*
* brief 获取两个点的曼哈顿距离
*/
int getDistanceTwoPoint(int iPosxFrom, int iPosyFrom, int iPosxTo, int iPosyTo)
{
return abs(iPosxFrom - iPosxTo) + abs(iPosyFrom - iPosyTo);
}
/*
* brief 构建stInfo
*/
void buildStInfo(stInfo &stTmp, int iPosx, int iPosy, std::pair<int, int> posBegin, std::pair<int, int> posEnd, int iFPosx = -1, int iFPosy = -1)
{
stTmp.iPosx = iPosx;
stTmp.iPosy = iPosy;
stTmp.iFPosx = iFPosx;
stTmp.iFPosy = iFPosy;
stTmp.iG = getDistanceTwoPoint(iPosx, iPosy, posBegin.first, posBegin.second);
stTmp.iH = getDistanceTwoPoint(iPosx, iPosy, posEnd.first, posEnd.second);
stTmp.iF = stTmp.iG + stTmp.iH;
//cout << "iposx:" << iPosx << " iposy:" << iPosy << " iG:" << stTmp.iG << " iH:" << stTmp.iH << " iF:" << stTmp.iF << endl;
}
/*
* brief 检查位置合法
*/
bool isLegalPos(int **pstAr, int iPosx, int iPosy)
{
//cout << "isLegalPos " << iPosx << " " << iPosy << endl;
return (iPosx >= 0 && iPosx < 5 && iPosy >= 0 && iPosy < 5) && (pstAr[iPosx][iPosy] != 1);
}
/*
* brief 检查某个点不在openlist
*/
bool notInOpenList(int iPosx, int iPosy, int iF, std::multimap<int, stInfo> &mapOpenList)
{
int iCount = mapOpenList.count(iF);
auto it = mapOpenList.find(iF);
while (it != mapOpenList.end() && iCount)
{
if (it->second.iPosx == iPosx && it->second.iPosy == iPosy)
return false;
++it;
--iCount;
}
return true;
}
/*
* brief 某个位置周围四个符合要求的点插入openlist
*/
bool insertPointToOpenList(int **pstAr, const std::pair<int, int> &posNow, std::multimap<int, stInfo> &mapOpenList, std::map<std::pair<int, int>, stInfo> &mapCloseList,
const std::pair<int, int> &posBegin, const std::pair<int, int> &posEnd)
{
if (isLegalPos(pstAr, posNow.first - 1, posNow.second) && mapCloseList.find(std::make_pair(posNow.first - 1, posNow.second)) == mapCloseList.end())
{
stInfo stTmp;
buildStInfo(stTmp, posNow.first-1, posNow.second, posBegin, posEnd, posNow.first, posNow.second);
if (notInOpenList(posNow.first - 1, posNow.second, stTmp.iF, mapOpenList))
{
mapOpenList.insert(std::make_pair(stTmp.iF, stTmp));
if (stTmp.iH == 0)
return true;
}
}
if (isLegalPos(pstAr, posNow.first, posNow.second-1) && mapCloseList.find(std::make_pair(posNow.first, posNow.second-1)) == mapCloseList.end())
{
stInfo stTmp;
buildStInfo(stTmp, posNow.first, posNow.second-1, posBegin, posEnd, posNow.first, posNow.second);
if (notInOpenList(posNow.first, posNow.second-1, stTmp.iF, mapOpenList))
{
mapOpenList.insert(std::make_pair(stTmp.iF, stTmp));
if (stTmp.iH == 0)
return true;
}
}
if (isLegalPos(pstAr, posNow.first + 1, posNow.second) && mapCloseList.find(std::make_pair(posNow.first + 1, posNow.second)) == mapCloseList.end())
{
stInfo stTmp;
buildStInfo(stTmp, posNow.first+1, posNow.second, posBegin, posEnd, posNow.first, posNow.second);
if (notInOpenList(posNow.first+1, posNow.second, stTmp.iF, mapOpenList))
{
mapOpenList.insert(std::make_pair(stTmp.iF, stTmp));
if (stTmp.iH == 0)
return true;
}
}
if (isLegalPos(pstAr, posNow.first, posNow.second+1) && mapCloseList.find(std::make_pair(posNow.first, posNow.second+1)) == mapCloseList.end())
{
stInfo stTmp;
buildStInfo(stTmp, posNow.first, posNow.second+1, posBegin, posEnd, posNow.first, posNow.second);
if (notInOpenList(posNow.first, posNow.second+1, stTmp.iF, mapOpenList))
{
mapOpenList.insert(std::make_pair(stTmp.iF, stTmp));
if (stTmp.iH == 0)
return true;
}
}
return false;
}
/*
* brief A*寻路
*/
void findRoad(int **pstAr, std::pair<int,int> posBegin, std::pair<int,int> posEnd)
{
std::multimap<int, stInfo> mapOpenList;
std::map<std::pair<int, int>, stInfo> mapCloseList;
std::pair<int, int> posNow = posBegin;
//起点加入Openlist
stInfo stTmp;
buildStInfo(stTmp, posNow.first, posNow.second, posBegin, posEnd);
mapOpenList.insert(std::make_pair(stTmp.iF, stTmp));
while (1)
{
auto itopen = mapOpenList.begin();
if (itopen == mapOpenList.end()) //openlist为空，无路径 如果不为空，取第一个就是iF最小值
{
cout << "无路径" << endl;
return;
}
posNow = std::make_pair(itopen->second.iPosx, itopen->second.iPosy);
//cout << "====" << posNow.first << " " << posNow.second << endl;
stInfo stTmp = itopen->second;
mapOpenList.erase(itopen);
mapCloseList.insert(std::make_pair(std::make_pair(stTmp.iPosx, stTmp.iPosy), stTmp));
if (stTmp.iH == 0)
{
cout << "找到了" << endl;
break;
}
insertPointToOpenList(pstAr, posNow, mapOpenList, mapCloseList, posBegin, posEnd);
//if (insertPointToOpenList(pstAr, posNow, mapOpenList, mapCloseList, posBegin, posEnd)) //周围坐标加进去
{
// mapCloseList.insert(std::make_pair(std::make_pair(posEnd.first, posEnd.second),stInfo()));
// cout << "找到了" << endl;
// break;
}
}
posNow = std::make_pair(posEnd.first, posEnd.second);
while (1)
{
auto it = mapCloseList.find(std::make_pair(posNow.first, posNow.second));
if (it == mapCloseList.end())
break;
if (it->first.first == -1 && it->first.second == -1)
break;
cout << it->first.first << " " << it->first.second << " -> ";
posNow = std::make_pair(it->second.iFPosx, it->second.iFPosy);
}
}
int main()
{
int **pstAr = new int *[5];
for (int i = 0; i < 5; ++i)
pstAr[i] = new int[5];
for (int i = 0; i < 5; ++i)
for (int j = 0; j < 5; ++j)
pstAr[i][j] = 0;
//pstAr[0][2] = 1;
pstAr[1][2] = 1;
pstAr[2][2] = 1;
pstAr[3][2] = 1;
//pstAr[4][2] = 1;
pstAr[1][1] = 1;
pstAr[3][0] = 1;
pstAr[4][0] = 1;
findRoad(pstAr, std::pair<int,int>(1,0), std::pair<int,int>(4,4));
//findRoad(pstAr, std::pair<int, int>(1, 0), std::pair<int, int>(0, 3));
return 0;
}
以上这份代码里的屌丝，最终找到了女朋友。
昨天是A*寻路算法发明人Nilsson去世的日子，谨以此文致敬。
扫一扫
993
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
