《算法九》（A星寻路算法）

A星寻路：
　　结构：N叉树
　　直线代价斜线代价：符合勾股定理
　　代价：每走一步,距离终点所付出的
　　计算公式：f = g + h + w;
　　f : 当前点到终点的代价
　　g : 起点到当前点的代价
　　h : 当前点到终点的预估代价（只计算直线距离，并忽略障碍）
　　w : 权重 路： 平地 上坡路 丘陵 沼泽 坑

 

代码演示：

#include <string.h>>
#include <iostream>
#include <vector>
#include <windows.h>
using namespace std;

//代价  ： 每走一步   距离终点所付出的

// f = g + h + w;
// f : 当前点到终点的代价
// g : 起点到当前点的代价
// h : 当前点到终点的预估代价（只计算直线距离，并忽略障碍）
// w : 权重  路：   平地    上坡路  丘陵  沼泽   坑 


//直线代价斜线代价：符合勾股定理 
//直线代价
#define ZXDJ  10
//斜线代价
#define XXDJ  14


//地图高  Y   竖着  行数  列  
#define ROWS  12
//地图宽  X   横着  列数  行
#define COLS  12



struct MyPoint{
	int row;//x 
	int col;//y 
	//f = g+h 
	int h;		//当前点到终点预估代价
	int f;		//当前点的代价
	int g;		//起点到当前点的代价

	void setF(){ f = g + h; }
};

//辅助地图 
struct pathNode{
	int		val;//值 
	bool	isFind;//有没有走过 
};

//方向枚举 ：上下左右/左上左下右上右下 
enum direct{p_up,p_down,p_left,p_right,lup,ldown,rup,rdown};


//树节点类型
struct treeNode{
	MyPoint				pos;
	treeNode*			pParent;
	vector<treeNode*>	child;
};
//创建一个树节点并返回节点首地址
//treeNode* CreateNode(const MyPoint& point);
treeNode* CreateNode(int row,int col);

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

//计算h值并返回
int getH(const MyPoint& currentPos, const MyPoint& endPos);

int main()
{
	int map[ROWS][COLS] = {
		{ 0, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
		{ 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0 },
		{ 0, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0 },
		{ 0, 1, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0 },
		{ 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0 },
		{ 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 }
	};

	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];
		}
	}
	MyPoint begPos = { 1, 1 };
	MyPoint endPos = { 4, 9 };
	//准备树结构
	treeNode* pRoot = NULL;
	//起点成为树根节点
	pRoot = CreateNode(begPos.row, begPos.col);
	//标记起点走过
	pathMap[begPos.row][begPos.col].isFind = true;


	//临时指针 指向当前节点
	treeNode* pTemp = pRoot;
	//临时指针 暂时保存当前点的孩子
	treeNode* pTempChild = NULL;
	//保存当前备选点的数组
	vector<treeNode*> buff;
	
	//准备两个迭代器

	//遍历整个容器
	vector<treeNode*>::iterator it;
	//定位容器中f值最小的元素
	vector<treeNode*>::iterator itMin;
	//临时点类型
	MyPoint tempPos;
	bool isFindEnd = false;
	
	
	//寻路
	while (1){
		
		//1 一个点压出八个点
		for (int i = 0; i < 8; i++){
			tempPos = pTemp->pos;
			//1.1 每个点做个临时的出来
			switch (i)
			{
			//上下左右   左上左下右上右下
			//同时需要改变g值 
			case p_up:tempPos.row--;tempPos.g += ZXDJ;break;
			case p_down:tempPos.row++;tempPos.g += ZXDJ;break;
			case p_left:tempPos.col--;tempPos.g += ZXDJ;break;
			case p_right:tempPos.col++;tempPos.g += ZXDJ;break;
			case lup:tempPos.row--;tempPos.col--;tempPos.g += XXDJ;break;
			case ldown:tempPos.row++;tempPos.col--;tempPos.g += XXDJ;break;
			case rup:tempPos.row--;tempPos.col++;tempPos.g += XXDJ;break;
			case rdown:tempPos.row++;tempPos.col++;tempPos.g += XXDJ;break;
			}
			//1.2 这个点可以走
			if (canWalk(pathMap, tempPos)){
				//1.2.1 创建树节点
				pTempChild = CreateNode(tempPos.row, tempPos.col);
				//1.2.2 计算节点的g值 h值 f值:f=g+h
				pTempChild->pos.g = tempPos.g;
				pTempChild->pos.h = getH(pTempChild->pos, endPos);//计算h值：到终点预估代价 
				pTempChild->pos.setF();//计算f 
				
				printf("(%d,%d) ",
					pTempChild->pos.row, pTempChild->pos.col);

				//1.2.3 新结点入树
				pTemp->child.push_back(pTempChild);
				pTempChild->pParent = pTemp;
				//1.2.4 新节点保存到数组中
				buff.push_back(pTempChild);
			}
			
		}
		printf("\n");
		
		//2 遍历buff数组，找出其中f值最小的一个
		itMin = buff.begin();
		for (it = buff.begin(); it != buff.end(); it++){
			itMin = ((*itMin)->pos.f > (*it)->pos.f) ? it : itMin;
		}

		//3 当前点变成这个点
		pTemp = *itMin;
		//标记当前点走过
		pathMap[pTemp->pos.row][pTemp->pos.col].isFind = true;
		
		//4 buff数组中删除这个点
		buff.erase(itMin);
		
		//5 判断是否找到终点
		if (pTemp->pos.row == endPos.row &&
			pTemp->pos.col == endPos.col){
			isFindEnd = true;
			break;
		}
		
		//6 判断地图是否没有出口
		if (0 == buff.size()) break;
	}
	
	

	
	// 打印路径
	if (isFindEnd){
		printf("找到终点啦！\n");
		printf("打印路径:终点-->起点\n");
		printf("\n");
		printf("\n");
		while (pTemp){
			printf("(%d,%d) ", pTemp->pos.row, pTemp->pos.col);
			pTemp = pTemp->pParent;
		}
		printf("\n");
	}
	return 0;
}

treeNode* CreateNode(int row, int col){
	treeNode* pNew = new treeNode;
	memset(pNew, 0, sizeof(treeNode));
	pNew->pos.row = row;
	pNew->pos.col = col;
	return pNew;
}


//判断pos点能不能走，能走返回true，否则返回false
bool canWalk(pathNode pathMap[ROWS][COLS], MyPoint pos){
	//不在地图范围内
	if (pos.row >= ROWS || pos.row < 0 || pos.col >= COLS || pos.col < 0)
		return false;
	//是障碍
	if (pathMap[pos.row][pos.col].val) return false;
	//走过
	if (pathMap[pos.row][pos.col].isFind) return false;
	return true;
}


//计算h值并返回
int getH(const MyPoint& currentPos, const MyPoint& endPos){
	int x = (currentPos.col > endPos.col) ? (currentPos.col - endPos.col) : (endPos.col - currentPos.col);
	int y = (currentPos.row > endPos.row) ? (currentPos.row - endPos.row) : (endPos.row - currentPos.row);
	return (x+y)*ZXDJ;
}