光速AStar寻路算法（C++）

一、最终效果

可以看到，我创建了500个小动物，也有110的FPS。虽然它们并不是每一帧都在计算寻路，但是平均下来不卡顿也不错了。我是i7 6700 + GT 720，这个效果图虽然不能直观的说明效率问题，但我相信我的AStar比网上一般的例子快。

如果对绘图的DND库感兴趣，可以移步我的其他文章。

二、使用方法

针对不同的物体，肯定有不同的寻路逻辑，飞禽走兽水怪可通行路径和偏好都会不一致。假设我们要为陆地的怪物设置寻路逻辑，则先要继承基础的AStar类：

class AStarFoo : public AStar
{
public://相邻点之间的代价，可以选择忽略第一个点，从而只返回第二点的消耗virtual float GetCostNodeToNode(const PointU& source, const PointU& target){//这里通过target的坐标访问实际节点的信息        //如果目标节点可以通行，则返回代价值//例如，普通地返回 1.0//沼泽返回5（更难通行）//草丛返回 0.85（更加偏好隐蔽的行径）auto* node = island->GetNode(target);if(node->type == GRASS)return 0.85f;//……else if//返回0代表不可通行return 0;}AStarFoo() :AStar(true, 200 * 200) {  }};

继承AStar类，重写GetCostNodeToNode函数，定义了节点之间的自定义代价值，可以根据两个节点的信息处理，也可以只处理将要去的节点，如上面代码的例子。

而调用的基类构造函数中，true控制是否可以斜穿网格，200*200定义了最大访问的节点数，防止寻路失败时浪费大量计算时间，一旦寻路超过4万，就会返回失败。

接着如下代码寻路：

//创建一个Foo实例
AstarFoo foo;//用于存储返回的路径
list<PointU> list_path;//寻找坐标(100, 200) -> (300, 100)的路径
if (foo.Search(PointU(100, 200),PointU(300, 100), list_path)
{//处理返回的路径list_path
}

还可以额外传入一个链表指针，以返回相对最近的路径（失败寻路）：

//用于存储返回的路径
list<PointU> list_path;
//失败寻路的最近路径
list<PointU> list_fail_path;//寻找坐标(100, 200) -> (300, 100)的路径
if (foo.Search(PointU(100, 200),PointU(300, 100), list_path, &list_fail_path)
{//处理返回的路径list_path
}
else
{//处理失败返回的路径list_fail_path
}

三、基本原理

（甲）图的连接

网上有大把的关于A星寻路算法的原理，不过大部分都是基于2d网格的。实际上AStar算法，本质上是基于图的，只要用户定义了图的连接，和节点之间的代价，AStar就可以进行寻路。

只不过2d网格是一种特殊的图，并且一般来说是双向，如果用图来实现平铺网格的信息，就会变得很浪费很慢。例如从2d网格的(x, y)处出发，自然而然的就知道与它连接的节点是哪些：

右、下、左、上

如果可以斜穿，还包括右下，左下，左上，右上。

而自定义代价值，附加到节点上即可，不需要保存到连接的信息上。例如，以图实现，节点3到节点6的代价需要定义，节点6到节点3的代价也要定义，不过你也可以不定义，总之GetCostNodeToNode函数定义了两个节点之间的代价值。Foo类我们重载的GetCostNodeToNode函数直接设置为到任意节点，都是固定的代价值，只和目标节点有关。

（乙）代价值

F = G + H，即总代价 = 自定义代价值 + 估计代价值。前面我们反复提到的代价值即自定义代价值G，表示节点之间的实际行走代价。而估计代价值H则是估计两个节点的代价值……对于人类来说，一眼看到河对面，就知道走过去会不会太费力（取决于有没有桥，或者船）。但是电脑不会直接知道，哪里有没有桥或者船（如果遍历寻找，就失去了AStar的意义——快速的寻路）。其实这也不是人类的智慧多么伟大，只是由于信息的获知问题。假设人去一个从来没去过的地方，然后前面是一座很大的山，人也无法估计是否路径可行。

对于电脑（后面称AI了）来说，最直观的就是两点之间的距离（隐含在了PointU里面）。离目标点越近，则代价值越小，而AI就会先去往代价值小的点，但是AI并无法直接感知代价，只能先走着去，再实际判断我们设定的代价值（为0就不可通行了，无论估计值是多少）。

而总代价是两者之和，决定了哪个节点最先被AI考察。在考察的路途中，还包含了经过的节点代价。例如寻路经过10个节点，则下一个节点的代价需要加上第10个节点的代价，当然第10个的代价已经加上了第9个的代价，依次类推，当前节点的代价为整条路径的代价和。当待考察的点有更小的代价值时，就会从那个节点开始往周围寻路。更多细节问题，请见后面。

（丙）Open表和Closed表

open表表示应该被检查的节点，每次循环，我们取出总代价值最小的的那一个。如果是目标节点（寻路的目标位置），则返回成功，并按_parent指针，返回整个路径。如果不是，则移动到closed表，然后将与其相连的节点做以下处理：

1.如果在closed表，说明再访问这个节点没有了意义，因为只可能通过它相邻的节点达到目标。所以不做其他处理。

2.如果不在open表，则添加到open表，当然需要设置它的总代价值、自定义代价值、父节点指针三个属性（此操作保证表有序）。

3.如果在open表，且此节点新计算的总代价值比旧值小，说明新的路径更优，则更新为新值，且使用新的父节点（此操作改变了总代价值，也需要保证有序）。

四、具体实现

（甲）节点银行

首先我们需要表示一个节点类AStarNode，如下：

class AStarNode
{
public:PointU _xy;AStarNode* _parent;   //父节点（为空代表起点）float _cost;     //前往这个节点 的代价值float _total;      //总代价 （cost + 估计代价）bool _open;           //是否在 Open表bool _closed;        //是否在 Close表
};

其中成员表示的含义如下：

_xy	位置
_parent	父节点，当寻路成功时，按这个返回整条路径的链表
_cost	自定义代价
_total	总代价
_open	是否在open表
_closed	是否在close表

由于需要大量使用AStarNode，所以我们直接用数组（节点银行，简单的内存池）保存，反复使用，防止new和delete的开销，在AStart的构造函数就分配了内存：

//AStar成员
AStarNode* _arrNodeBank;//节点银行//AStar构造函数
AStar(bool sideling = true, UINT32 max_node = DND_ASTAR_SEARCH_MAX)
{_arrNodeBank = new AStarNode[max_node];//other……
}//析构函数
virtual ~AStar()
{delete[] _arrNodeBank;
}

（乙）优先队列（二叉堆）

由于我们需要反复从open表取出最小代价的节点，所以可以用优先队列来保存open表，也仅仅是保存指针，所有内存都在节点银行。其中又以二叉堆的速度最快，可以利用标准库的vector数组和堆的算法来实现一系列操作，代码如下：

inline bool fn_CompareAStarNode(AStarNode* n1, AStarNode* n2)
{return n1->_total > n2->_total;
}//优先队列
class AStarPriorityQueue
{
public:AStarNode* Pop(){AStarNode* node = this->_heap.front();pop_heap(this->_heap.begin(), this->_heap.end(), fn_CompareAStarNode);this->_heap.pop_back();return node;}void Push(AStarNode* node){this->_heap.push_back(node);push_heap(this->_heap.begin(), this->_heap.end(), fn_CompareAStarNode);}void UpdateNode(AStarNode* node){vector<AStarNode*>::iterator iter;for (iter = this->_heap.begin(); iter != this->_heap.end(); iter++){if ((*iter)->_xy == node->_xy){push_heap(this->_heap.begin(), iter + 1, fn_CompareAStarNode);return;}}}bool IsEmpty(){return this->_heap.empty();}void Clear(){_heap.clear();}
private:vector<AStarNode*> _heap;
};

原书是通过仿函数进行排序的，不过lambda表达式和全局函数都可以，至于仿函数和lambda表达式谁快，我感觉应该是全局函数，但没有实际实验。防止比较函数重定义，必须加上内联标记。部分成员函数的功能如下：

Pop	取出总代价值最小的节点
Push	按序插入一个节点
UpdateNode	修改节点总代价值后，再刷新顺序位置

（丙）估计值H的计算

前面说了估计值是通过几何距离（勾股定理）求得的，不过我这儿不一样，因为怪物只能斜45度和水平垂直移动，实际的路径如下：

所以说，默认的实现如下，这样计算量也比较低：

//根号2 减 1
const float DND_ASTAR_SQRT_2_SUB_1 = 0.4142135f;//返回到目标点的 估计值
virtual inline float GetNodeHeuristic(const PointU& source, const PointU& target)
{PointU d = target - source;return (d.x > d.y ? (d.x + DND_ASTAR_SQRT_2_SUB_1 * d.y) : (d.y + DND_ASTAR_SQRT_2_SUB_1 * d.x));
}

需要注意的是，PointU是无符号整型，在进行减法时，实际是求得二者之差绝对值。可以参考后面PointU类的实现。另外，这是一个虚函数，所以你也可以自己定义估计代价函数GetNodeHeuristic返回H值。另外高估斜着走的价值，可以使最终路径斜着走的更少（因为斜着走的估计代价值更高了）。

（丁）起始节点与成功返回

下面进入开始寻路的前要操作，首先放入起始点到open表，如下操作：

//起点插入Open表
AStarNode* node_start = _get_node(source);
node_start->_open = true;
node_start->_closed = false;
node_start->_cost = 0;
node_start->_total = GetNodeHeuristic(source, target);//_total = _cost + h
node_start->_parent = NULL;
_queueOpen.Push(node_start);

接着是循环的操作，直到open表为空（失败），或者best_node就是要寻路到的位置（成功）：

AStarNode* best_node = NULL;//代价值最小的节点
while (!_queueOpen.IsEmpty())
{//取出 消耗值 最小的节点best_node = _queueOpen.Pop();//是目标节点if (best_node->_xy == target){//构造路径（包含首尾节点） while (best_node){list_path.push_front(best_node->_xy);best_node = best_node->_parent;}return true;}//TODO: 如果不是目标节点的操作
}

（戊）添加相邻节点到open表

显然，一开始只有起点一个节点在open表。如果当前节点不是终点，就加入与它相邻的节点到open表。相邻节点的判断如上面所说的，可以自行修改代码实现图的连接，也可以用默认的2d网格：

//你可以修改这里 实现图 的连接，这里就不再实现了
//相邻点
_check_connecting(1, 0, DND_ASTAR_COST_MIN, best_node, target);
_check_connecting(-1, 0, DND_ASTAR_COST_MIN, best_node, target);
_check_connecting(0, 1, DND_ASTAR_COST_MIN, best_node, target);
_check_connecting(0, -1, DND_ASTAR_COST_MIN, best_node, target);
//是否斜穿节点
if (_bSideling)
{_check_connecting(1, 1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);_check_connecting(-1, 1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);_check_connecting(-1, -1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);_check_connecting(1, -1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);
}

其中水平垂直的相邻节点，代价传入了1.0作为单位距离的代价值系数（斜切的传入了根号2，由于斜着穿格子路程更长，所以自定义代价值也应该更大）：

//单位距离的代价值
const float DND_ASTAR_COST_MIN = 1.0f;
//根号2 倍单位距离 消耗
const float DND_ASTAR_COST_MIN_SQRT_2 = sqrt(2);

当加入相邻的节点后，此节点就可以加入close表了，此次寻路不会再访问到它：

best_node->_closed = true;

（己）相邻节点的处理

前面已经说到了相邻节点的处理，即_check_connecting函数的实现，首先判断自定义代价值G，如果是不存在的游戏节点（非AStarNode），或者不可通行的节点，则直接不处理：

//代价，new_xy是相邻节点的位置
float g = GetCostNodeToNode(best_node->_xy, new_xy);
//如果代价 等于0，说明此相邻节点不可通行
if (g == 0)return;

接着需要返回此位置的AStarNode数据，当然这个AStarNode第一次返回时，会从节点银行取得对象并进行初始化。之后再访问，则通过hash_map直接返回，当然它存的还是指针，并不实际拥有内存的释放权利。

AStarNode* _get_node(PointU xy)
{//如果在 主列表 直接返回，如果没有则从节点银行构造一个UINT64 index = xy.ToIndex2();//返回64位整型防止计算的索引越界AStarNode*& node = _hashMasterNode[index];if (node){return node;}else{node = &_arrNodeBank[_bankCur++];//用一个，则加一node->_xy = xy;node->_open = false;//初始的节点，不在closed表，也不在open表node->_closed = false;return node;}
}

返回位置的节点后，则按第三节（丙）的逻辑操作：

//在close表，则不作任何处理
if(actual_node->_closed == false)
{if (actual_node->_open){//在open表，则判断 总代价是否更小//上一个点代价 + 移到自己的代价 float new_cost = best_node->_cost + g*cost;//总代价，自己代价 + 估计代价float new_total = new_cost + GetNodeHeuristic(new_xy, target);//新的 代价更小 才刷新if (new_total < actual_node->_total){actual_node->_parent = best_node;actual_node->_cost = new_cost;actual_node->_total = new_total;_queueOpen.UpdateNode(actual_node);}}else{//不在open表，则加入actual_node->_parent = best_node;actual_node->_cost = best_node->_cost + g*cost;actual_node->_total = actual_node->_cost + GetNodeHeuristic(new_xy, target);_queueOpen.Push(actual_node);actual_node->_open = true;}
}

五、完整代码

其中报错代码，越界检测代码可自行修改或删除。

于2021-03-24更新

//
//name:     AStar
//author:   Lveyou
//date:     18-08-12//other:
//18-08-12: 网格二维地图 快速寻找最短路径 - Lveyou
//18-08-12: 本代码 借鉴、改写于 《游戏编程精粹1》 - Lveyou
//18-08-14: 如何使用：继承 AStar类，实现 GetCostNodeToNode函数即可（返回0代表此节点代价无限大）
//          重载 GetNodeHeuristic以自定义估价函数
//          网格大小只能是[0-65535]，如果要更大请修改 PointU类
//          _bSideling 控制是否可以斜穿，在AStar类的构造函数初始化
//          有 bug俺可不负责，但可以找上门锤我 - Lveyou
//20-04-15: 优化代码，且网格大小可以是UINT32值范围
//20-10-05: 优化返回节点的方法
//#pragma once//返回路径信息用
#include <list>
//二叉堆 用于Open表优先队列
#include <vector>
//哈希表 储存主节点表，所有寻路访问过的节点
#include <unordered_map>
//二叉堆相关操作
#include <algorithm>
using namespace std;//PointU定义
#define _DND_ASTAR_#ifdef _DND_ASTAR_#include "head.h"
#elsetypedef unsigned int        UINT32, *PUINT32;
typedef unsigned __int64    UINT64, *PUINT64;class PointU
{
public:UINT32 x, y;PointU() :x(0), y(0) {}PointU(UINT32 ix, UINT32 iy) :x(ix), y(iy) {}UINT64 ToIndex2(){return (UINT64(x) + UINT64(y) * UINT32_MAX);}bool operator==(const PointU& b) const{return (x == b.x) && (y == b.y);}bool operator!=(const PointU& b) const{return (x != b.x) || (y != b.y);}//无符号减法 PointU operator-(const PointU& b) const{return PointU(x >= b.x ? x - b.x : b.x - x, y >= b.y ? y - b.y : b.y - y);}
};
#endif // false//主节点表 最大节点数
const UINT32 DND_ASTAR_SEARCH_MAX = 10000;//以下常量不要修改//
//根号2 减 1
const float DND_ASTAR_SQRT_2_SUB_1 = 0.4142135f;
//根号2
const float DND_ASTAR_SQRT_2 = 1.4142135f;//单位距离的代价值
const float DND_ASTAR_COST_MIN = 1.0f;//根号2 倍单位距离 消耗
const float DND_ASTAR_COST_MIN_SQRT_2 = DND_ASTAR_COST_MIN * (DND_ASTAR_SQRT_2);//节点
class AStarNode
{
public:PointU _xy;AStarNode* _parent;   //父节点（为空代表起点）float _cost;     //前往这个节点 的代价值float _total;      //总代价 （cost + 估计代价）bool _open;           //是否在 Open表bool _closed;        //是否在 Close表
};inline bool fn_CompareAStarNode(AStarNode* n1, AStarNode* n2)
{return n1->_total > n2->_total;
}//优先队列
class AStarPriorityQueue
{
public:AStarNode* Pop(){AStarNode* node = this->_heap.front();pop_heap(this->_heap.begin(), this->_heap.end(), fn_CompareAStarNode);this->_heap.pop_back();return node;}void Push(AStarNode* node){this->_heap.push_back(node);push_heap(this->_heap.begin(), this->_heap.end(), fn_CompareAStarNode);}void UpdateNode(AStarNode* node){vector<AStarNode*>::iterator iter;for (iter = this->_heap.begin(); iter != this->_heap.end(); iter++){if ((*iter)->_xy == node->_xy){push_heap(this->_heap.begin(), iter + 1, fn_CompareAStarNode);return;}}}bool IsEmpty(){return this->_heap.empty();}void Clear(){_heap.clear();}
private:vector<AStarNode*> _heap;
};class AStar
{
public://返回到目标点的 估计值virtual inline float GetNodeHeuristic(const PointU& source, const PointU& target){PointU d = target - source;return (d.x > d.y ? (d.x + DND_ASTAR_SQRT_2_SUB_1 * d.y) : (d.y + DND_ASTAR_SQRT_2_SUB_1 * d.x));}//相邻点之间的代价，可以选择忽略第一个参数，从而只返回第二个点的代价virtual float GetCostNodeToNode(const PointU& source, const PointU& target) = 0;//搜索（传入失败路径，会返回相对最佳的路径）bool Search(const PointU& source, const PointU& target, list<PointU>& list_path, list<PointU>* fail_path = NULL){if (target.x > 100000|| target.y > 100000|| source.x > 100000|| source.y > 100000){debug_err(String::Format(256, L"DND: AStar传入的坐标值过大，请检查参数: [%u, %u] -> [%u, %u]",source.x, source.y,target.x, target.y));return false;}/*elsedebug_info(String::Format(256, L"DND: AStar传入的坐标为: [%u, %u] -> [%u, %u]",source.x, source.y,target.x, target.y).GetWcs());*///清空_bankCur = 0;_masterNode.clear();_queueOpen.Clear();//起点插入Open表AStarNode* node_start = _get_node(source);node_start->_open = true;node_start->_closed = false;node_start->_cost = 0;node_start->_total = GetNodeHeuristic(source, target);node_start->_parent = NULL;_queueOpen.Push(node_start);AStarNode* best_node = NULL;while (!_queueOpen.IsEmpty()){//取出 消耗值 最小的节点best_node = _queueOpen.Pop();//是目标节点if (best_node->_xy == target){//构造路径（包含首尾节点）   while (best_node){list_path.push_front(best_node->_xy);best_node = best_node->_parent;}return true;}//由于_check_connecting 会增加节点，所以提前判断，并预留一些if (_bankCur + 8 >= _maxNode){if (fail_path){//构造路径（包含首尾节点） while (best_node){fail_path->push_front(best_node->_xy);best_node = best_node->_parent;}}return false;}///你可以修改这里 实现图 的连接，这里就不再实现了//相邻点_check_connecting(1, 0, DND_ASTAR_COST_MIN, best_node, target);_check_connecting(-1, 0, DND_ASTAR_COST_MIN, best_node, target);_check_connecting(0, 1, DND_ASTAR_COST_MIN, best_node, target);_check_connecting(0, -1, DND_ASTAR_COST_MIN, best_node, target);//是否斜穿节点if (_bSideling){_check_connecting(1, 1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);_check_connecting(-1, 1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);_check_connecting(-1, -1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);_check_connecting(1, -1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);}best_node->_closed = true;}if (fail_path){//构造路径（包含首尾节点）   while (best_node){fail_path->push_front(best_node->_xy);best_node = best_node->_parent;}}return false;}AStar(bool sideling = true, UINT32 max_node = DND_ASTAR_SEARCH_MAX){_arrNodeBank = new AStarNode[max_node];_bSideling = sideling;_maxNode = max_node;}virtual ~AStar(){delete[] _arrNodeBank;}
private:AStarNode* _get_node(PointU xy){//如果在 主列表 直接返回，如果没有则从节点银行构造一个auto& iter_x = _masterNode[xy.x];AStarNode*& node = iter_x[xy.y];if (node){return node;}else{//此处会越界
#ifndef DND_NO_DEBUGif (_bankCur >= _maxNode){debug_err(L"DND: AStar寻路的节点超过上限！");return NULL;}
#endif          node = &_arrNodeBank[_bankCur++];node->_xy = xy;node->_open = false;node->_closed = false;return node;}}void _check_connecting(int dx, int dy, float cost, AStarNode* best_node, const PointU& target){PointU new_xy;new_xy.x = int(best_node->_xy.x) + dx;new_xy.y = int(best_node->_xy.y) + dy;//防止越界if (new_xy.x > 100000 || new_xy.y > 100000)return;//if (best_node->_parent == NULL ||best_node->_parent->_xy != new_xy){//代价float g = GetCostNodeToNode(best_node->_xy, new_xy);//如果代价 等于0，说明此节点不可通行if (g == 0)return;AStarNode* actual_node = _get_node(new_xy);//不在close表if(actual_node->_closed == false){if (actual_node->_open){//上一个点代价 + 移到自己的代价 float new_cost = best_node->_cost + g*cost;//总代价，自己代价 + 估计代价float new_total = new_cost + GetNodeHeuristic(new_xy, target);//新的 代价更小 才刷新if (new_total < actual_node->_total){actual_node->_parent = best_node;actual_node->_cost = new_cost;actual_node->_total = new_total;_queueOpen.UpdateNode(actual_node);}}else{//不在open表，则加入actual_node->_parent = best_node;actual_node->_cost = best_node->_cost + g*cost;actual_node->_total = actual_node->_cost + GetNodeHeuristic(new_xy, target);_queueOpen.Push(actual_node);actual_node->_open = true;}}}}//主列表map<UINT32, map<UINT32, AStarNode*>> _masterNode;//Open表AStarPriorityQueue _queueOpen;//节点银行AStarNode* _arrNodeBank;//每次搜索前置0UINT32 _bankCur;//能否斜穿bool _bSideling;//遍历节点上限UINT32 _maxNode;
};

六.示例

要实际使用就在AStar类上继承重写一下方法，在下面的例子，我通过判断地图节点是否可通行，如果可通行就返回0表示不可通行（而不是没有代价），不碰撞就返回1代表可通行且代价为1。返回2可以通行但代价会更高，例如沼泽、山地等。

其中构造函数第一个参数为是否可斜穿，第二个为节点遍历上限。

代码如下：

//
//name:     AStar Cow
//author:   Lveyou
//date:     19-03-06//other:
//19-03-06: 用于类似牛这种动物的寻路 - Lveyou
//#pragma once#include "F629.h"
#include "Island.h"
#include "AStar.h"const UINT32 GAME_ASTAR_COW_SIZE = 40;
class AStarCow : public AStar
{
public://相邻点之间的代价，可以选择忽略第一个点，从而只返回第二点的消耗virtual float GetCostNodeToNode(const PointU& source, const PointU& target){Point p = target;if (_f629->GetCurIsland()->IsNodeCollision(p, true))return 0;elsereturn 1;return 0;}void Init(F629* f629){_f629 = f629;}AStarCow() :AStar(true, GAME_ASTAR_COW_SIZE * GAME_ASTAR_COW_SIZE) {  }F629* _f629;
};

在怪物类，我稍微封装简化了一下使用方法，简单来说就是调用Search函数即可。

bool Monster::SearchWay(AStar* astar, const Point& target_xy, bool replace, bool add_begin /*= false*/, bool add_end /*= true*/)
{if (_searchCd > 0)return false;_searchCd = 1.0f;list<PointU> list_path;if (astar->Search(PointToPointU(_island->GetPoint(_coor->GetPosition())),PointToPointU(target_xy), list_path)){if (replace){_listWaypoint.clear();for (auto& iter : list_path){_listWaypoint.push_back(Waypoint(_island->GetVector2(iter) +Vector2(GAME_MAP_NODE_SIZE_HALF, GAME_MAP_NODE_SIZE_HALF)));}if (!add_begin)_listWaypoint.pop_front();if (!add_end&& _listWaypoint.size() != 0)_listWaypoint.pop_back();}return true;}return false;
}