A*寻路C++ 代码

 

 

#include "AStar.h"

//准备一张 开标 和 闭表
std::vector<CAStar::_NODE>CAStar::open;
std::vector<CAStar::_NODE>CAStar::close;

#define _ABS(v) ((v) < 0 ? -(v):(v))
#define _H_V(c, e) (_ABS((c)%w-(e)%w)+_ABS((c)/w - (e)/w))

//找到最小的F值的下标
unsigned int CAStar::GetMinF()
{
	unsigned int i = 0;
	for(unsigned int j = 1; j < open.size(); ++j)
	{
		if(open[i]._g + open[i]._h > open[j]._g + open[j]._h)
		{
			i = j;
		}
	}
	return i;
}
//看下标的节点 是否在 开表中
int CAStar::InOpen(int index)
{
	for(unsigned int i = 0; i < open.size(); ++i)
	{
		if(index == open[i]._cur)
			return i;
	}
	return -1;
}

int CAStar::InClose(int index)
{
	for(unsigned int i = 0; i < close.size(); ++i)
	{
		if(index == close[i]._cur)
			return i;
	}
	return -1;
}

int CAStar::FindPath(const char* map,//传进来的地图
		int w, int h,	//地图宽高
		int b, int e,	//起始点 和 终点
		int* path)		//寻路路径
{
	if(!map || w < 1 || h < 1 || b < 0 || b >= w * h || e < 0 || e >= w * h || !path)
	{
		return -1;
	}

	//八方向 判断

	const int offset_x[] = {0,1,1,1,0,-1,-1,-1};
	const int offset_y[] = {-1,-1,0,1,1,1,0,-1};

	//清空表
	open.clear();
	close.clear();

	//把起点放入开表
	_NODE begin = {b, -1, 0, _H_V(b, e)};
	open.push_back(begin);

	//循环找路
	while(!open.empty())
	{
		//得到开表中 F值 最小的节点下标
		unsigned int min_f = GetMinF();
		//得到开表中F值最小的节点
		_NODE min_f_n = open[min_f];

		//将其中开表中删除
		open.erase(open.begin() + min_f);

		//放入闭表中
		close.push_back(min_f_n);

		//获取坐标
		int cx = min_f_n._cur % w;
		int cy = min_f_n._cur / w;

		//循环当前节点的 八方向 判断是否有通路
		for(int i = 0; i < 8; ++i)
		{
			int dx = cx + offset_x[i];
			int dy = cy + offset_y[i];

			//地图中判断
			if(dx >= 0 && dx < w && dy >= 0 && dy < h)
			{
				//找到终点了
				int di = dx + dy * w;
				if(di == e)
				{
					//终点入表
					_NODE node = {di, min_f_n._cur, 0, 0};
					close.push_back(node);

					//最后放入的节点下标
					int index = close.size() - 1;

					//循环得到路径
					int pathlen = 0;
					while (index != -1)
					{
						path[pathlen++] = close[index]._cur;
						int j = index;
						int k = close[index]._b;
						if(k == -1)
							break;
						//效率优化点
						while(1)
						{
							if(close[j]._cur == k)
								break;
							else
								--j;
						}
						index = j;
					}
					return pathlen;
				}
				else
				{
					//不是障碍
					if(map[di] > 0)
					{
						//不在闭表中
						if(!InClose(di))
						{
							//不在开表中
							int r = InOpen(di);
							int curG = min_f_n._g + (i % 2 == 1 ? 7 : 5);
							if(-1 == r)
							{
								_NODE node = {di, min_f_n._cur, curG, _H_V(di, e)};
								open.push_back(node);
							}
							else
							{
								//如果G值 小于 开表中的G 就替换
								if(open[r]._g < curG)
								{
									open[r]._b = min_f_n._cur;
									open[r]._g = curG;
								}
							}
						}
					}
				}
			}
		}
	}

	return 0;
}