数据结构学习第二十课(寻路算法之A星寻路)

A星寻路

#if 1
#include<stdio.h>
#include<string.h>
#include<vector>
using namespace std;
#define ROW 10
#define COL 10
#define zxprice 10
#define xxprice 14
struct Point
{
	int row;
	int col;
	int f;//代价
	int g;//已付出
	int h;//预计付出
	void getF()
	{
		f = g + h;
	}
};
struct treeNode
{
	Point pos;
	treeNode* pParent;
	vector<treeNode*> child;
};
enum direct { p_up, p_down, p_left, p_right, p_lup, p_ldown, p_rup, p_rdown };
treeNode* createNode(Point pos);
bool canWalk(int map[ROW][COL], bool isFind[ROW][COL], Point pos);
int getH(Point endPos, Point pos);
int main()
{
	int map[ROW][COL] =
	{
		{0,0,0,0,1,0,0,0,0,0},
		{0,0,0,0,1,0,0,0,0,0},
		{0,0,0,0,1,0,0,0,0,0},
		{0,0,0,0,1,0,0,0,0,0},
		{0,0,0,0,1,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,1,0,0,0,0,0},
		{0,0,0,0,1,0,0,0,0,0},
		{0,0,0,0,1,0,0,0,0,0}
	};
	bool isFind[ROW][COL] = { 0 };
	Point begPos = { 1,1 };
	Point endPos = { 7,6 };
	isFind[begPos.row][begPos.col] = true;
	treeNode* pRoot = createNode(begPos);
	vector<treeNode*> buff;
	vector<treeNode*>::iterator it;
	vector<treeNode*>::iterator itMin;
	treeNode* pChild = NULL;//新结点
	treeNode* pCurrent = pRoot;
	Point searchPos;
	bool isFindEnd = false;

	while (1)
	{
		//1 从当前点找出能走的几个点
		for (int i = 0; i < 8; i++)
		{
			searchPos = pCurrent->pos;
			switch (i)
			{
			case p_up:
				searchPos.row--;
				searchPos.g += zxprice;//计算g值
				break;
			case p_down:
				searchPos.row++;
				searchPos.g += zxprice;//计算g值
				break;
			case p_left:
				searchPos.col--;
				searchPos.g += zxprice;//计算g值
				break;
			case p_right:
				searchPos.col++;
				searchPos.g += zxprice;//计算g值
				break;
			case p_lup:
				searchPos.row--;
				searchPos.col--;
				searchPos.g += xxprice;//计算g值
				break;
			case p_ldown:
				searchPos.row++;
				searchPos.col--;
				searchPos.g += xxprice;//计算g值
				break;
			case p_rup:
				searchPos.row--;
				searchPos.col++;
				searchPos.g += xxprice;//计算g值
				break;
			case p_rdown:
				searchPos.col++;
				searchPos.row++;
				searchPos.g += xxprice;//计算g值
				break;
			}

			if (canWalk(map, isFind, searchPos))
			{
				pChild = createNode(searchPos);
				//1.1 计算g,h,f值
				pChild->pos.h = getH(endPos, searchPos);
				pChild->pos.getF();
				//1.2 入树
				pCurrent->child.push_back(pChild);//新结点成为当前点的孩子
				pChild->pParent = pCurrent;//当前点成为新结点的父
				//1.3 保存到buff数组中
				buff.push_back(pChild);
				printf("%d %d\n", pCurrent->pos.row, pCurrent->pos.col);
				printf("%d %d\n", searchPos.row, searchPos.col);
			}
		}
		//2 从buff数组中找出f值最小的点

		//for (int i = 0; i < buff.size(); i++)
		//{
		//	printf("%d %d f:%d\n", buff[i]->pos.row, buff[i]->pos.col, buff[i]->pos.f);
		//}
		itMin = buff.begin();
		for (it = buff.begin(); it != buff.end(); it++)
		{
			itMin = (((*itMin)->pos.f < (*it)->pos.f) ? itMin : it);
		}
		//3 要么是终点,循环结束,要么这个点成为当前点(从buff中删除)
		if ((*itMin)->pos.row == endPos.row &&
			(*itMin)->pos.col == endPos.col)
		{
			isFindEnd = true;
			break;
		}
		pCurrent = (*itMin);
		isFind[pCurrent->pos.row][pCurrent->pos.col] = true;
		buff.erase(itMin);
		//找不到终点
		if (0 == buff.size())
		{
			printf("wwww");
			break;
		}
	}
	if (isFindEnd)
	{
		printf("找到了:\n");
		while (pCurrent)
		{
			printf("[%d %d]", pCurrent->pos.row, pCurrent->pos.col);
			pCurrent = pCurrent->pParent;
		}
		printf("\n");
	}
	else
	{
		printf("没找到");
	}
	return 0;
}

treeNode* createNode(Point pos)
{
	treeNode* pNew = new treeNode;
	//memset(pNew, 0, sizeof(treeNode));//memset会破坏vector的结构,必须单独初始化
	pNew->child = { 0 };
	pNew->pParent = NULL;

	pNew->pos = pos;
	return pNew;
}
bool canWalk(int map[ROW][COL], bool isFind[ROW][COL], Point pos)
{
	if (pos.row >= ROW ||
		pos.col < 0 ||
		pos.row < 0 ||
		pos.col >= COL)
	{
		return false;
	}
	if (map[pos.row][pos.col])
	{
		return false;
	}
	if (isFind[pos.row][pos.col])
	{
		return false;
	}
	return true;
}
int getH(Point endPos, Point pos)
{
	//只计算直线距离 并无视障碍
	int x = (endPos.col > pos.col) ? (endPos.col - pos.col) : (pos.col - pos.col);
	int y = (endPos.row > pos.row) ? (endPos.row - pos.row) : (pos.row - endPos.row);
	return (x + y) * zxprice;
}
#endif