A*寻路算法

        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去世的日子，谨以此文致敬。