广度寻路算法C++实现

红色方框是起点，蓝色方框是终点，深红色的区域代表障碍物无法通行。起点的坐标设为（0，0），那么起点的孩子结点就只有（0，1），也就是它下面那个方框，因为这个起点能到达的只有这一个地方。以此类推（0，1）的孩子结点就是（1，1）和（0，2）。
按照这个思路构建树的过程中，当我们遍历到终点时，就意味着起点到终点的路径出现了。我们的树结构中，每个结点都有指向自己父节点的指针，根据这个指针我们可以从终点找到回去的“路”。另外在树的遍历过程中，需要理解“层”的概念，因为我们是一层一层的进行遍历整颗树的，为了方便直接用一个容器vector来存放一个结点的所有子节点，这样做可以省去许多指针操作，毕竟每个结点的子节点数量并不相同。
实现代码如下：

// 广度寻路算法.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include <string.h>
#include <vector>
using namespace std;

#define ROWS   10
#define COLS   10

struct MyPoint{
	int row;
	int col;
};

struct pathNode{
	int		val;		//地图上的值
	bool	isFind;		//是否走过
};

//树节点
struct treeNode{
	MyPoint				pos;
	treeNode*			pParent;//指向父节点的指针
	vector<treeNode*>	child;	//存储指向所有孩子节点指针的容器
};

//能走返回true 否则返回false
bool canWalk(pathNode pathMap[ROWS][COLS], MyPoint pos);


int _tmain(int argc, _TCHAR* argv[])
{
	//1. 地图
	int map[ROWS][COLS] = {
		{ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
		{ 0, 1, 1, 1, 1, 1, 1, 1, 1, 0 },
		{ 0, 1, 1, 1, 1, 1, 1, 1, 1, 0 },
		{ 0, 1, 1, 1, 1, 1, 1, 1, 1, 0 },
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
		{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
		{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
		{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
		{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
	};

	//2 起点和终点
	MyPoint begPos = { 0, 0 };
	MyPoint endPos = { 0, 9 };

	//3 辅助地图
	pathNode pathMap[ROWS][COLS] = { 0 };
	for (int i = 0; i < ROWS; i++){
		for (int j = 0; j < COLS; j++){
			pathMap[i][j].val = map[i][j];
		}
	}

	//4 准备一颗树，起点成为树根节点
	treeNode* pRoot = NULL;//准备树

	pRoot = new treeNode;//开内存
#if 1
	memset(pRoot, 0, sizeof(treeNode));//全部置空
	pRoot->pos = begPos;
#else
	pRoot->pos = begPos;
	pRoot->pParent = NULL;
#endif
	//5 标记起点走过
	pathMap[begPos.row][begPos.col].isFind = true;

	//6 寻路
	MyPoint temp;
	//当前节点
	treeNode* pCurrent = NULL;
	//新节点
	treeNode* pChild = NULL;
	//当前层
	vector<treeNode*> currentBuff;
	
	//下一层
	vector<treeNode*> nextBuff;
	bool isFindEnd = false;

	currentBuff.push_back(pRoot);
	while (1)
	{//一层一层变化
		nextBuff.clear();//清空数组
		for (int i = 0; i < currentBuff.size(); i++){//遍历当前层
			pCurrent = currentBuff[i];
			for (int j = 0; j < 4; j++){//四个方向找相邻节点
				temp = pCurrent->pos;
				switch (j){
				case 0://上
					temp.row--;
					break;
				case 1://下
					temp.row++;
					break;
				case 2://左
					temp.col--;
					break;
				case 3://右
					temp.col++;
					break;
				}
				if (canWalk(pathMap,temp)){//能走
					//创建新节点
					pChild = new treeNode;
					memset(pChild, 0, sizeof(treeNode));
					pChild->pos = temp;
					//新节点入树
					pCurrent->child.push_back(pChild);	//新节点成为当前节点的孩子
					pChild->pParent = pCurrent;			//当前节点成为新节点的父
					//标记走过
					pathMap[temp.row][temp.col].isFind = true;
					//新节点进入下一层
					nextBuff.push_back(pChild);
					//是否是终点
					if (temp.row == endPos.row &&
						temp.col == endPos.col){
						isFindEnd = true;
						break;
					}
				}
			}
			if (isFindEnd) break;
		}
		if (isFindEnd) break;
		//整个地图都遍历完毕，也没有找到终点
		if (nextBuff.size() == 0) break;
		currentBuff = nextBuff;//当前层变化
	}

	if (isFindEnd){
		printf("找到终点啦!\n");
		printf("path:");
		while (pChild){
			printf("(%d,%d) ", pChild->pos.row, pChild->pos.col);
			pChild = pChild->pParent;
		}
		printf("\n");
	}

	while (1);
	return 0;
}

//能走返回true 否则返回false
bool canWalk(pathNode pathMap[ROWS][COLS], MyPoint pos){
	//不在地图范围内
	if (pos.row < 0 || pos.col >=COLS || pos.row >= ROWS || pos.col < 0) return false;

	//走过
	if (pathMap[pos.row][pos.col].isFind) return false;
	//是障碍
	if (pathMap[pos.row][pos.col].val) return false;

	return true;
}