广度优先寻路算法

        广度优先遍历，是一种非启发式、不用预处理的寻路算法。通常会将整个图全部遍历，意图寻找终点。从起点开始，每次所有符合的点全部遍历，类似于扩散的样子。

       一般情况，广度优先遍历算法都是基于队列结构，先进先出的遍历所有节点。

      该寻路算法，通常先将起点加入队列结构，然后将该点周边符合要求（未加入队列结构并且可以到达的点）的点全部再次加入队列结构中，意图下一次遍历。一直循环该方法。直至全部遍历或者找到终点。

 

代码：

///////////////////////////////////////////////////////////////////////
//广度遍历寻路算法
struct stInfo
{
	int iPosx = -1;
	int iPosy = -1;
	int iFPosx = -1;
	int iFPosy = -1;
};
stInfo buildStInfo(int iPosx, int iPosy, int iFPosx = -1, int iFPosy = -1)
{
	stInfo stTmp;
	stTmp.iPosx = iPosx;
	stTmp.iPosy = iPosy;
	stTmp.iFPosx = iFPosx;
	stTmp.iFPosy = iFPosy;
	cout << "posx:" << iPosx << " posy:" << iPosy << " fposx:" << iFPosx << " fposy:" << iFPosy << endl;
	return stTmp;
}
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);
}
bool isInMapList(std::vector<stInfo> &mapList, int iPosx, int iPosy)
{
	for (auto &it : mapList)
	{
		if (it.iPosx == iPosx && it.iPosy == iPosy)
			return true;
	}
	return false;
}
void insertListAblePos(std::vector<stInfo> &vecList, std::pair<int, int> &posNow, std::pair<int, int> &posEnd, int **pstAr)
{
	if (isLegalPos(pstAr, posNow.first - 1, posNow.second) && !isInMapList(vecList, posNow.first-1, posNow.second))
	{
		stInfo stTmp = buildStInfo(posNow.first-1, posNow.second, posNow.first, posNow.second);
		vecList.push_back(stTmp);
	}
	if (isLegalPos(pstAr, posNow.first, posNow.second-1) && !isInMapList(vecList, posNow.first, posNow.second-1))
	{
		stInfo stTmp = buildStInfo(posNow.first, posNow.second-1, posNow.first, posNow.second);
		vecList.push_back(stTmp);
	}
	if (isLegalPos(pstAr, posNow.first + 1, posNow.second) && !isInMapList(vecList, posNow.first + 1, posNow.second))
	{
		stInfo stTmp = buildStInfo(posNow.first + 1, posNow.second, posNow.first, posNow.second);
		vecList.push_back(stTmp);
	}
	if (isLegalPos(pstAr, posNow.first, posNow.second+1) && !isInMapList(vecList, posNow.first, posNow.second+1))
	{
		stInfo stTmp = buildStInfo(posNow.first, posNow.second+1, posNow.first, posNow.second);
		vecList.push_back(stTmp);
	}
}
void printRoad(std::vector<stInfo> &mapList, std::pair<int, int> posEnd)
{
	while (!mapList.empty())
	{
		cout << posEnd.first << " " << posEnd.second << " -> ";
		for(auto it = mapList.begin(); it != mapList.end(); ++it)
		{
			if (it->iPosx == posEnd.first && it->iPosy == posEnd.second)
			{
				posEnd = std::make_pair(it->iFPosx, it->iFPosy);
				break;
			}
		}
		if (posEnd.first == -1 && posEnd.second == -1)
			break;
	}
}
void findRoadBFS(int **pstAr, std::pair<int,int> &posBegin, std::pair<int,int> &posEnd)
{
	std::vector<stInfo> vecList;
	std::pair<int, int> posNow = posBegin;

	stInfo stTmp = buildStInfo(posNow.first, posNow.second);
	vecList.push_back(stTmp);
	int iPosMove = 0;
	while (posNow != posEnd)
	{
		insertListAblePos(vecList, posNow, posEnd, pstAr);

		if (vecList.size() <= iPosMove)
			break;
		stInfo stTmp = vecList[iPosMove];

		posNow = std::make_pair(stTmp.iPosx, stTmp.iPosy);
		iPosMove++;
	}
	if (posNow == posEnd)
	{
		cout << "find success" << endl;
		printRoad(vecList, posEnd);
	}
	else
	{
		cout << "find error" << endl;
	}
}
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[1][2] = 1;
	pstAr[2][2] = 1;
	pstAr[3][2] = 1;
	pstAr[0][2] = 1;
	pstAr[4][2] = 1;
	pstAr[1][1] = 1;
	pstAr[3][0] = 1;

	findRoadBFS(pstAr, std::make_pair(1,0), std::make_pair(4,4));
	return 0;
}