A*寻路算法及优化

A*算法是基于树结构遍历过程的一个优化算法，A*其实是一种思想，采用启发式遍历来得到起始点到目标点的最优路径。

A*算法的基本思想是：f(n) = g(n) + h(n)

对于空间的每个点坐标，给它3个值，F,G,H值。这3个值的作用是：

G值表示从一个点移动到下一个点需要的花费，比如平面坐标上从(1,1)到(2,1)花费可以是1，(1,1)到(2,2)使用对角线距离的话G值就有可能增加2。这个数值是可以预先定义好的。

H值表示当前点到目标点的估算值。一般使用这两点的平方和来近似。

F值就表示该点的估价值了。F值使用G和H值的和来表示，F值越低代表该点越优。最好的路径就是一系列最优节点的集合。

算法的描述：（引用自A*)

1，把起始格添加到开启列表。
2，重复如下的工作：
a) 寻找开启列表中F值最低的格子。我们称它为当前格。
b) 把它切换到关闭列表。
c) 对相邻的8格中的每一个？
* 如果它不可通过或者已经在关闭列表中，略过它。反之如下。
* 如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。
* 如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。

d) 停止，当你
* 把目标格添加进了关闭列表，这时候路径被找到，或者
* 没有找到目标格，开启列表已经空了。这时候，路径不存在。
3.保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格。这就是你的路径。

AStarPathFinder.h

#ifndef _ASTARPATHFINDER_H_
#define _ASTARPATHFINDER_H_

#include <vector>
#include <set>
#include <map>
using namespace std;

struct PathPoint 
{
	PathPoint() : x(0), z(0)
	{

	}

	bool operator== (const PathPoint& PathPoint) const
	{
		return ((x == PathPoint.x) && (z == PathPoint.z));
	}

	int x, z;
};

struct PathNode
{
	PathNode() : mParent(NULL)
	{

	}

	PathPoint	mPathPoint;		// 节点所处的平面点
	float		mF;				// 节点估价值, f = g + h
	float		mG;				// g值, 与TILESIZE相关
	float		mH;				// h值，为当前点到目标点的距离
	PathNode*	mParent;		// 该节点的父节点
};

struct NodeSort
{
	bool operator() (const PathNode* node1, const PathNode* node2) const
	{
		return node1->mF < node2->mF;
	}
};

class AStarPathFinder 
{
public:
	typedef multiset<PathNode*, NodeSort> NODESET;
	typedef NODESET::iterator NODESETITER;
	typedef map<DWORD, PathNode*> NODEFINDMAP;
	typedef NODEFINDMAP::iterator NODEMAPITER;
	typedef vector<PathPoint> PATHVEC;

	AStarPathFinder();
	~AStarPathFinder();

	// 清理
	void clear();
	// 搜寻路径
	// @sx, sy 起始点坐标， @dx， dy目标点坐标
	bool findPath(int sx, int sz, int dx, int dz);
	// 得到路径容器
	PATHVEC getPath();

private:
	// 生成最优点的子节点
	// @bestNode 最优点
	void generateNodes(PathNode* bestNode, int dx, int dz);
	// 判断最优点，同时更新OPEN和CLOSED表
	void updateNode(PathNode* bestNode, int sx, int sz, int dx, int dz, float tileSize);
	// 是否是目标点
	bool isDestNode(PathNode* node);
	// 得到最优点，通过返回已排序的map结构的第一个节点获得
	PathNode* getBestNode();
	// 设置路径，通过获得目标点的连续父节点直到起始点，反过来就是路径
	void setPath(PathNode* node, int sx, int sz);

private:
	// OPEN表， 存储未遍历过的节点, 第一位是节点数据
	// 用于排序
	NODESET	mOpen;
	// CLOSED表，存储遍历过的节点，第一位是节点数据
	// 用于排序
	NODESET	mClosed;
	// 保存最优路径
	PATHVEC	mPath;
	// 展开节点时的参考步长，既向四方1个单位展开节点
	const static float TILESIZE;
	const static float TILESIZE2;
};


#endif

AStarPathFinder.cpp

AStarPathFinder::AStarPathFinder()
{
	clear();
}

AStarPathFinder::~AStarPathFinder()
{
	clear();
}

void AStarPathFinder::clear()
{
	if ( !mOpen.empty() )
	{
		NODESETITER end = mOpen.end();
		for ( NODESETITER iter = mOpen.begin(); iter != end; ++iter )
		{
			SAFE_DELETE(*iter);
		}
	}
	mOpen.clear();

	if ( !mClosed.empty() )
	{
		NODESETITER end = mClosed.end();
		for ( NODESETITER iter = mClosed.begin(); iter != end; ++iter )
		{
			SAFE_DELETE(*iter);
		}
	}
	mClosed.clear();

	mPath.clear();
}

bool AStarPathFinder::findPath(int sx, int sz, int dx, int dz)
{
	//clear();
	PathNode* node = new PathNode;
	PathNode* bestNode = NULL;
	// 初始节点数据, g为0，h为平方距离
	node->mG = 0.0f;
	node->mH = static_cast<float>((dx - sx) * (dx - sx) + (dz - sz) * (dz - sz));
	node->mF = node->mG + node->mH;
	node->mPathPoint.x = sx;
	node->mPathPoint.z = sz;
	// OPEN表初始节点为起始节点
	mOpen.insert(node);
	while (!mOpen.empty())
	{
		bestNode = getBestNode();
		if ( bestNode == NULL )
		{
			return false;
		}
		// 把搜寻过的最优点从OPEN中删除，放入CLOSED中
		mOpen.erase(mOpen.begin());
		mClosed.insert(bestNode);
		// 如果得到的是目标点，那么循环结束
		if ( isDestNode(bestNode) )
		{
			break;
		}
		// 否则生成子节点
		generateNodes(bestNode, dx, dz);
	}

	setPath(bestNode, sx, sz);

	return true;
}

void AStarPathFinder::generateNodes(PathNode* bestNode, int dx, int dz)
{
	int x, z;
	//生成8个方向的子节点，跳过已经在CLOSED表中的和障碍物节点
	x = bestNode->mPathPoint.x - 1;
	z = bestNode->mPathPoint.z - 1;
	updateNode(bestNode, x, z, dx, dz, TILESIZE2);
	x = bestNode->mPathPoint.x;
	z = bestNode->mPathPoint.z - 1;
	updateNode(bestNode, x, z, dx, dz, TILESIZE);
	x = bestNode->mPathPoint.x + 1;
	z = bestNode->mPathPoint.z - 1;
	updateNode(bestNode, x, z, dx, dz, TILESIZE2);
	x = bestNode->mPathPoint.x + 1;
	z = bestNode->mPathPoint.z;
	updateNode(bestNode, x, z, dx, dz, TILESIZE);
	x = bestNode->mPathPoint.x + 1;
	z = bestNode->mPathPoint.z + 1;
	updateNode(bestNode, x, z, dx, dz, TILESIZE2);
	x = bestNode->mPathPoint.x;
	z = bestNode->mPathPoint.z + 1;
	updateNode(bestNode, x, z, dx, dz, TILESIZE);
	x = bestNode->mPathPoint.x - 1;
	z = bestNode->mPathPoint.z + 1;
	updateNode(bestNode, x, z, dx, dz, TILESIZE2);
	x = bestNode->mPathPoint.x - 1;
	z = bestNode->mPathPoint.z;
	updateNode(bestNode, x, z, dx, dz, TILESIZE);
}

void AStarPathFinder::updateNode(PathNode* bestNode, int sx, int sz, int dx, int dz, float tileSize)
{
	PathNode* child = new PathNode;
	child->mH = static_cast<float>((dx - sx) * (dx - sx) + (dz - sz) * (dz - sz));
	child->mG = bestNode->mG + tileSize;
	child->mF = child->mG + child->mH;
	child->mPathPoint.x = sx;
	child->mPathPoint.z = sz;
	// 跳过CLOSED表中的节点和不可通过的节点
	bool bInClosed = false;
	PathNode* node = NULL;
	if ( (node = findClosedNode(child)) != NULL )
	{
		if (child->mG < node->mG)
		{
			node->mG = child->mG;
			node->mF = child->mG + child->mH;
			node->mParent = bestNode;
		}
		SAFE_DELETE(child);
	}
	else if ( (node = findOpenNode(child)) == NULL )
	{
		// 如果不在OPEN和CLOSED表中，把这个节点的父节点设为当前节点
		child->mParent = bestNode;
		// 并且把这个节点放入OPEN表
		mOpen.insert(child);
	}
	else if ( node != NULL )
	{
		// 如果在OPEN表中，比较当前节点的G值+TILESIZE和此节点原来的G值，如果新G值小那么更新
		// G值，并且把这个节点的父节点设为当前节点
		if ( child->mG < node->mG )
		{
			node->mG = child->mG;
			node->mF = child->mG + child->mH;
			node->mParent = bestNode;
		}
		SAFE_DELETE(child);
	}
	else
	{
		SAFE_DELETE(child);
	}
}

bool AStarPathFinder::isDestNode(PathNode* node)
{
	return node->mH == 0;
}

PathNode* AStarPathFinder::getBestNode()
{
	NODESETITER iter = mOpen.begin();
	if ( iter != mOpen.end() )
	{
		return (*iter);
	}

	return NULL;
}

void AStarPathFinder::setPath(PathNode* node, int sx, int sz)
{
	PathPoint point;
	mPath.clear();
	while ( ( node->mPathPoint.x != sx ) || ( node->mPathPoint.z != sz ) )
	{
		point.x = node->mPathPoint.x;
		point.z = node->mPathPoint.z;
		mPath.push_back(point);
		node = node->mParent;
	}
}

std::vector<PathPoint> AStarPathFinder::getPath()
{
	return mPath;
}

使用multiset的原因是：使用set的话，会基于排序准则进行过滤，从而把不是相同点但f值相同的点过滤掉。 比如(1,2)和（2，1）会把其中一个点过滤掉。

寻路优化：

在实际项目中，当远距离寻路时会出现短暂卡的情况，我通过调试分析发现，在每次寻路时都会存在大量的链表查询，所以我将查询放给了Map里，效果果然不错。

代码如下：

// 用于查询
NODEFINDMAP mFindOpen;
NODEFINDMAP mFindClosed;

#define CREATENODEID(col, row)	(col * 10000 + row)

void AStarPathFinder::insertNodeInOpen(int sx, int sz, PathNode* node)
{
	if (sx >= 0 && sz >= 0)
	{
		DWORD nodeId = CREATENODEID(sx, sz);
		mFindOpen[nodeId] = node;
	}
}

void AStarPathFinder::deleteNodeFromeOpen(int sx, int sz)
{
	if (sx >= 0 && sz >= 0)
	{
		DWORD nodeId = CREATENODEID(sx, sz);
		NODEMAPITER iter = mFindOpen.find(nodeId);
		if (iter != mFindOpen.end())
		{
			mFindOpen.erase(iter);
		}
	}
}

void AStarPathFinder::insertNodeInClosed(int sx, int sz, PathNode* node)
{
	if (sx >= 0 && sz >= 0)
	{
		DWORD nodeId = CREATENODEID(sx, sz);
		mFindClosed[nodeId] = node;
	}
}

void AStarPathFinder::deleteNodeFromeClosed(int sx, int sz)
{
	if (sx >= 0 && sz >= 0)
	{
		DWORD nodeId = CREATENODEID(sx, sz);
		NODEMAPITER iter = mFindClosed.find(nodeId);
		if (iter != mFindClosed.end())
		{
			mFindClosed.erase(iter);
		}
	}
}

PathNode* AStarPathFinder::findOpenNode(PathNode* node)
{
	DWORD nodeId = CREATENODEID(node->mPathPoint.x, node->mPathPoint.z);
	NODEMAPITER iter = mFindOpen.find(nodeId);
	if (iter != mFindOpen.end())
	{
		return iter->second;
	}

	return NULL;
}

PathNode* AStarPathFinder::findClosedNode(PathNode* node)
{
	DWORD nodeId = CREATENODEID(node->mPathPoint.x, node->mPathPoint.z);
	NODEMAPITER iter = mFindClosed.find(nodeId);
	if (iter != mFindClosed.end())
	{
		return iter->second;
	}

	return NULL;
}

此算法还有很多优化的地方，希望兄弟姐妹多多给予指点哈！