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
#include
#include 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((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((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 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;
}此算法还有很多优化的地方，希望兄弟姐妹多多给予指点哈！