UE 补完计划（四） 导航系统-5

前言：A* 寻路算法实现

UE4的导航使用的是RecastDetour组件，这是一个开源组件，主要支持3D场景的导航网格导出和寻路，或者有一个更流行的名字叫做NavMesh。不管是Unity还是UE都使用了这一套组件。

Github上有更为详细的源码、Demo和说明：https://github.com/recastnavigation/recastnavigation

本文使用的UE4源码版本为4.26.1，2021年2月2日的版本。

RecastDetour其实是两个模块，Recast负责生成导航网格，而Detour负责利用导航网格来寻路。

在前几篇文章中，阐述了UE4如何构建导航数据，并如何将Recast数据组织成寻路用的Detour数据的。

这一篇将会介绍UE4如何利用这些导航数据，来实际进行寻路，以及是如何支持垂直方向的3D寻路的。

A* 算法简介

A* 算法有非常多的非常好的文章详细介绍。这里我自己非常推荐这两篇，详细列举了 A* 算法的原理、常见问题和优化方案。

1. 堪称最好最全的A*算法详解（译文）
2. 径规划之 A* 算法

这里我还是简单介绍一下A*算法对比其它寻路算法的优劣，以及常见的一些优化方案。

A* 的概念

相信大家对Dijkstra算法和BFS算法都还有印象，在数据结构与算法课程一般也会讲这两种算法。我在这里简单介绍一下这两种算法的特点：

Dijkstra算法：其原则是一定要找到最短路径。会递归遍历所有的路径，并计算起始点到目标点的最短路径。

BFS算法：其原则是基于贪心算法，一定会去寻找下一个与目标距离最短的格子。

如果用代码描述，类似于这两者一个是广度遍历，一个是深度遍历。具体区别可以看下图：

BFS的问题在于，如果路径不是一马平川，而是有障碍（例如有一个死胡同），那么由于贪心思想的局限性，运行往往有问题。就像下面这张图：

Dijkstra、BFS和A*的本质

分析一下两种算法的行为，Dijkstra算法目的是找到总路径最短的，选择下一个格子的时候，就只能将所有周围的格子都找一遍，时间也更慢。而BFS算法目的是找到时间最短的，因此会默认选择离终点最近的，但无法保证路径最短。

A*算法就是结合了这两种思想。

对于A* 算法来说，下一个格子的选择不单单由一个维度来判断，而是基于两个方面：当前格子距离起始点的实际距离，和当前格子距离目标点的预估距离。在A* 算法的论文中，这个因素表示为：

$$f(n) = g(n) + h(n)$$

其中， $g(n)$ 表示当前格子到起始点的实际距离，我们简单表示为 g ， $h(n)$ 表示当前格子到目标点的预估距离，我们简单表示为 h。

当 h 取 0 的时候，整个 f 就退化成了只考虑当前各自距离起始点的实际距离，这与Dijkstra算法是一致的，可以认为此时 A* 就是Dijkstra算法。

当 h 取非常大，远大于 g 的时候，整个f可以看作只考虑距离目标点的预估距离，这时 A* 就是BFS算法。

或者更准确的，可以看下面这个表格：

情况	函数	结果
h(n) == 0	A*演变成Dijkstra算法	保证能找到最短路径
h(n) <= 实际代价	h(n)越小，A*扩展的结点越多，运行就得越慢	保证能找到一条最短路径，但运算更快了
h(n) == 实际代价	仅仅寻找最佳路径而不扩展别的任何结点	保证能找到一条最短路径，并且运算非常快
h(n) > 实际代价	寻找最佳路径且扩展别的任何结点	不能保证找到一条最短路径，但运算更快了
h(n) >> g(n)	A*演变成BFS算法	不能保证找到一条最短路径，但运算非常快

对于 A* 来说，由于既考虑了当前实际距离，又考虑了距离目标点的预估距离，因此只要设置合适的启发因子 h ，就可以保证既能找到一条最短路径，又能有非常快的运算速度。

启发因子的常见计算

A*运行的效果，最主要取决于启发函数如何设计。一般来说，启发函数主要有曼哈顿距离、考虑对角线的曼哈顿距离、欧几里得距离几种。在很多 demo 中，曼哈顿距离因为实现简单而且高效，经常被使用。欧几里得距离得到的效果最好，但是因为会包含根号计算会有性能损失。

在UE4中， $g(n)$ 和 $h(n)$ 都是使用欧几里得距离。

A* 算法描述

我们定义两个集合，OPEN集和CLOSE集。其中OPEN集包含将要检查的节点，CLOSE集则包含已经检查过的节点。另外，每个节点会记录当前已经耗费的cost，并记录其父节点。

整个算法其实很简单。基于上述定义，流程图如下：

UE4 的寻路算法实现

废话少说，我们直接来看UE4源码实现。

寻路节点结构

UE4使用了Detour模块来进行寻路。Detour模块会使用Recast构造出来的数据，再次构造出自己寻路需要的数据结构（所有以dt为前缀的结构体）。

之前的文章介绍过，Recast是以Tile为单位来组织的。同样，Detour也是以Tile为单位来使用的。其tile类叫做dtMeshTile。这个类会包含一个tile上的所有导航所需要的信息，例如坐标、宽度、所有顶点、所有三角形，每个节点的邻居节点，等等。

/// Defines a navigation mesh tile.
/// @ingroup detour
struct dtMeshTile
{
	unsigned int salt;					///< 标记当前tile有变更的标记

	unsigned int linksFreeList;			///< Index to the next free link.
	dtMeshHeader* header;				///< The tile header. 会记录一些size、坐标等信息。注意这个header将会放在头部
	dtPoly* polys;						///< 所有的多边形. [Size: dtMeshHeader::polyCount]
	float* verts;						///< 所有的顶点. [Size: dtMeshHeader::vertCount]
	dtLink* links;						///< 所有的连接信息. [Size: dtMeshHeader::maxLinkCount]
	dtPolyDetail* detailMeshes;			///< 当前tile拥有的所有detail mesh数据. [Size: dtMeshHeader::detailMeshCount]
	
	/// 所有的detail顶点. [(x, y, z) * dtMeshHeader::detailVertCount]
	float* detailVerts;	

	/// 所有的detail三角形. [(vertA, vertB, vertC) * dtMeshHeader::detailTriCount]
	unsigned char* detailTris;	

	/// 将所有节点组织成一颗bvtree. [Size: dtMeshHeader::bvNodeCount]
	dtBVNode* bvTree;

	dtOffMeshConnection* offMeshCons;		///< 记录所有navlink信息. [Size: dtMeshHeader::offMeshConCount]

	unsigned char* data;					///< tile原始数据。会转成可序列化的数据，供序列化函数使用。
	int dataSize;							///< tile原始数据长度
	int flags;								///< Tile flags. (See: #dtTileFlags)
	dtMeshTile* next;						///< The next free tile, or the next tile in the spatial grid.

	//@UE4 BEGIN
#if WITH_NAVMESH_SEGMENT_LINKS
	dtOffMeshSegmentConnection* offMeshSeg;		///< The tile off-mesh connections. [Size: dtMeshHeader::offMeshSegConCount]
#endif // WITH_NAVMESH_SEGMENT_LINKS

#if WITH_NAVMESH_CLUSTER_LINKS
	dtCluster* clusters;						///< Cluster data
	unsigned short* polyClusters;				///< Cluster Id for each ground type polygon [Size: dtMeshHeader::polyCount]

	dtChunkArray<dtClusterLink> dynamicLinksC;	///< Dynamic links array (indices starting from DT_CLINK_FIRST)
	unsigned int dynamicFreeListC;				///< Index of the next free dynamic link
#endif // WITH_NAVMESH_CLUSTER_LINKS

	dtChunkArray<dtLink> dynamicLinksO;			///< Dynamic links array (indices starting from dtMeshHeader::maxLinkCount)
	unsigned int dynamicFreeListO;				///< Index of the next free dynamic link
	//@UE4 END
};

每个tile会存储所有的link信息，而每个节点，都会记录其所拥有的link信息，每个link可以让这个节点记录一个邻居节点信息。

这是每个节点的定义。

struct dtNode
{
	float pos[3];				///< 当前坐标
	float cost;					///< 当前经过的所有路径的花费
	float total;				///< 预估的总花费，包括当前花费+预估剩余花费
	unsigned int pidx : 30;		///< 父节点ID
	unsigned int flags : 2;		///< 当前节点状态： 0/open/closed.
	dtPolyRef id;				///< 当前节点ID。更确切的说是当前节点对应的多边形ID
};

可以注意到，flags字段，使用位域，来记录节点的三种状态：未访问、open、closed。由于在实际游戏中，并不会存在负权值的路径，因此CLOSE集合是可以不存在的。改为使用CLOSE状态，来节省内存并提高访问速度（缓存更友好）

那这个节点的邻居节点怎么获取呢？注意到有一个字段类型是dtPolyRef：

/// A handle to a polygon within a navigation mesh tile.
/// @ingroup detour
typedef UE4Type_uint64 dtPolyRef;

这其实就是一个uint64的ID，指向一个唯一的dtPoly。而在结构dtPoly中，就会存储其相邻的dtPoly节点。

一个dtPoly就可以认为是图上的一个节点。每个dtPoly有多少个邻居，就有多少个dtLink，每个dtLink会存储其对应的邻居节点。可以注意到，即使A和B互为邻居，共用同一条边，但是也会存在两个dtLink结构。

我们这里先看一下dtPoly和dtLink的定义，等会会一起大概描述一下其结构：

/// Defines a polyogn within a dtMeshTile object.
/// @ingroup detour
struct dtPoly
{
    // 指向该节点的第一个链接
	/// Index to first link in linked list. (Or #DT_NULL_LINK if there is no link.)
	unsigned int firstLink;

	// 当前的顶点索引
	unsigned short verts[DT_VERTS_PER_POLYGON];

	/// Packed data representing neighbor polygons references and flags for each edge.
	// 每条边对应的邻居
	unsigned short neis[DT_VERTS_PER_POLYGON];

	/// The user defined polygon flags.
	unsigned short flags;

	// 当前多边形的顶点数量
	unsigned char vertCount;

	// 8个bit，高两位用来表示type，低6位用来表示area
	unsigned char areaAndtype;
};

/// Defines a link between polygons.
/// @note This structure is rarely if ever used by the end user.
/// @see dtMeshTile
struct dtLink
{
	dtPolyRef ref;					///< Neighbour reference. (The neighbor that is linked to.)
	unsigned int next;				///< Index of the next link.
	unsigned char edge;				///< Index of the polygon edge that owns this link.
	unsigned char side;				///< If a boundary link, defines on which side the link is.
	unsigned char bmin;				///< If a boundary link, defines the minimum sub-edge area.
	unsigned char bmax;				///< If a boundary link, defines the maximum sub-edge area.
};

寻路算法实现

detour的寻路过程，在dtNavMeshQuery::findPath()函数中实现。基本完全复刻了A*算法，并加了一些自身的优化。

Open列表数据结构

detour使用了一个二叉堆，来实现OPEN集。因为在OPEN列表中，我们只需要关心 $f(n)$ 最小的元素。这也是A*的常见优化方案。

// dtNodeQueue具体实现就是一个标准的最小堆，这里不再贴代码
class dtNodeQueue* m_openList;		///< Pointer to open list queue.

代价函数

从上面节点定义可以看出，每个节点都会存储一个cost，total就是父节点的total加上父节点到自身的cost。

其代价函数计算方式为：

inline float getInlineCost(const float* pa, const float* pb,
		const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly,
		const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly,
		const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const
	{
#if WITH_FIXED_AREA_ENTERING_COST
		// areaChangeCost，可以支持在切换area类型的时候有更高的开销，从而让生成的路径尽量不要频繁切换area类型
		const float areaChangeCost = nextPoly != 0 && nextPoly->getArea() != curPoly->getArea()
			? data.m_areaFixedCost[nextPoly->getArea()] : 0.f;

		return dtVdist(pa, pb) * data.m_areaCost[curPoly->getArea()] + areaChangeCost;
#else
		return dtVdist(pa, pb) * data.m_areaCost[curPoly->getArea()];
#endif // #if WITH_FIXED_AREA_ENTERING_COST
	}

总结一下，就是当前cost = BestNode's cost + BestNode距离当前Node的欧几里得距离 * 当前area系数 + BestNode距离当前Node的切换area类型开销。

area系数，是为了给不同的地形添加权重，例如沼泽、雪地和草地，从寻路规划来说他们的权重肯定不一样。而areaChangeCost，则是为了不让玩家经常切换area类型。例如沼泽地和雪地可能权重一样，但是我穿了雪地靴，就不会想去沼泽地中。这也是一个常见的优化A*路径合理性的方案。

启发函数

而启发函数计算则简单一些：

// 计算启发因子系数
inline float getModifiedHeuristicScale() const { return data.heuristicScale * ((data.lowestAreaCost > 0) ? data.lowestAreaCost : 1.0f); }

// ...

// 实际计算启发因子
const float H_SCALE = filter->getModifiedHeuristicScale();
heuristic = dtVdist(neiPos, endPos)*H_SCALE;

dtVdist就是其欧几里得距离。

这里需要注意，不能使用欧几里得距离的平方！这是因为我们观察的结构，表现为：

$$f(n) = g(n) + h(n)$$

正常情况下， $g(n)$ 和 $h(n)$ 相对于距离来说都是线性关系，如果 $h(n)$ 使用距离的平方，那么会更频繁的出现远大于 $h(n)$ 或者远小于 $g(n)$ 的情况，会使得 A* 算法很容易就偏向于Dijkstra或者BFS算法，出现寻路路径过慢或者不是最佳的情况。

具体流程

detour的寻路过程，在dtNavMeshQuery::findPath()函数中实现。基本完全复刻了A*算法，并加了一些自身的优化。

下列代码会把与A*相关的关键代码列出来，一些无关的部分会用注释代替，减少贴出来的代码长度。

dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
								  const float* startPos, const float* endPos,
								  const float costLimit, const dtQueryFilter* filter, //@UE4
								  dtQueryResult& result, float* totalCost) const
{
	// 使用A*算法寻路
	
	// ... 初始化即校验
	
	//@UE4 BEGIN
	// 添加H的scale来影响寻路
	const float H_SCALE = filter->getModifiedHeuristicScale();
	
	// 理论上，如果一个路径图不可能有负权值，那么一个closed的节点不会存在被reopen的情况
	const bool shouldIgnoreClosedNodes = filter->getShouldIgnoreClosedNodes();
	//@UE4 END
	
	// ... 将第一个点放入openlist，openList是一个二叉堆

	while (!m_openList->empty())
	{
		// 取出open列表中的最佳点，标记为close
		dtNode* bestNode = m_openList->pop();

		// 使用open和close的标记来代替传统的close_set
		bestNode->flags &= ~DT_NODE_OPEN;
		bestNode->flags |= DT_NODE_CLOSED;
		
		// 如果已经找到终点，那么结束查询
		if (bestNode->id == endRef)
		{
			lastBestNode = bestNode;
			break;
		}
		
		// ... 校验。并获取当前节点以及父节点的tile和poly数据

		// 遍历当前节点（Poly）的所有邻居
		unsigned int i = bestPoly->firstLink;
		while (i != DT_NULL_LINK)
		{
			// 整个循环会单独处理每一个邻居
			
			// 一个link指向当前的一个邻居
			const dtLink& link = m_nav->getLink(bestTile, i);
			i = link.next;

			dtPolyRef neighbourRef = link.ref;
			
			// Skip invalid ids and do not expand back to where we came from.
			if (!neighbourRef || neighbourRef == parentRef
//@UE4 BEGIN
				|| !filter->isValidLinkSide(link.side))
//@UE4 END
				continue;
			
			// 获取邻居节点的tile和poly数据
			const dtMeshTile* neighbourTile = 0;
			const dtPoly* neighbourPoly = 0;
			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);			

            // 可以通过自定义的filter，来过滤掉哪些不希望被查询的节点
			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly) || !passLinkFilterByRef(neighbourTile, neighbourRef))
				continue;

			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
			if (!neighbourNode)
			{
				status |= DT_OUT_OF_NODES;
				continue;
			}
//@UE4 BEGIN
			// 如果不存在负权值，过滤掉对于状态为close的节点
			else if (shouldIgnoreClosedNodes && (neighbourNode->flags & DT_NODE_CLOSED) != 0)
			{
				continue;
			}
//@UE4 END

			// Try to update node position for current edge to make paths more precise
			// Unless heuristic is not admissible (overestimates), in which case
			// we have to use constant values for given node to avoid creating cycles
			float neiPos[3] = { 0.0f, 0.0f, 0.0f };
			// h系数大于1表示距离目标点的距离估算占比更大，更偏向于BFS算法，一般情况运算速度更快。
			// h系数小于1表示当前已经历的距离占比更大，更偏向于Dijkstra算法，一般情况更精确。
			// 一般来说，h系数越大，表明希望运算速度越快，而精度会越低
			if (H_SCALE <= 1.0f || neighbourNode->flags == 0)
			{
				// 当h系数小于1时，或者邻居节点并没有被检查过时，表明希望使用更精确的路径，会取到相邻的两个多边形的邻边的中点来代替邻居节点的坐标
				// 算法见dtNavMeshQuery::getPortalPoints()
				getEdgeMidPoint(bestRef, bestPoly, bestTile,
					neighbourRef, neighbourPoly, neighbourTile,
					neiPos);
			}
			else
			{
				dtVcopy(neiPos, neighbourNode->pos);
			}

			// Calculate cost and heuristic.
			float cost = 0;
			float heuristic = 0;
			float curCost = 0;

			// Special case for last node.
			if (neighbourRef != endRef)
			{
				// 计算从当前的bestNode到当前Node的cost。当前cost = BestNode's cost + BestNode距离当前Node的欧几里得距离 * 当前area系数 + BestNode距离当前Node的切换area类型开销
				curCost = filter->getCost(bestNode->pos, neiPos, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly);
				cost = bestNode->cost + curCost;
				// h函数非常简单，就是计算距离终点的欧几里得距离
				heuristic = dtVdist(neiPos, endPos)*H_SCALE;
			}
			else
			{
				const float endCost = filter->getCost(neiPos, endPos, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly, 0, 0, 0);
				curCost = filter->getCost(bestNode->pos, neiPos, parentRef, parentTile, parentPoly, bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly);
				cost = bestNode->cost + curCost + endCost;
				heuristic = 0;
			}

			// total就是G+H，预估的总花费。需要找出总花费更少的邻居节点。
			const float total = cost + heuristic;

            // ... 一系列的检查，当当前路径比之前路径更差时，需要忽略当前路径

			// 邻居节点是新节点，或者邻居节点从当前路径的开销更小，那么将邻居节点加入open_set
			// 设置邻居节点的新属性
			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
			neighbourNode->id = neighbourRef;
			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
			neighbourNode->cost = cost;
			neighbourNode->total = total;
			dtVcopy(neighbourNode->pos, neiPos);
			
			if (neighbourNode->flags & DT_NODE_OPEN)
			{
				// Already in open, update node location.
				m_openList->modify(neighbourNode);
			}
			else
			{
				// Put the node in open list.
				neighbourNode->flags |= DT_NODE_OPEN;
				m_openList->push(neighbourNode);
				m_queryNodes++;
			}
			
			// Update nearest node to target so far.
			// 因为循环的目的是找到预估与终点最近的邻居，因此只需要记录h值即可
			if (heuristic < lastBestNodeCost)
			{
				lastBestNodeCost = heuristic;
				lastBestNode = neighbourNode;
			}
		}
	}

	// 理论上这时应该找到了终点
	
	if (lastBestNode->id != endRef)
		status |= DT_PARTIAL_RESULT;
	
	// 通过父节点，回溯出整个路径
	dtNode* prev = 0;
	dtNode* node = lastBestNode;
	int n = 1;
	do
	{
		dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
		node->pidx = m_nodePool->getNodeIdx(prev);
		prev = node;
		node = next;
	}
	while (node && ++n < loopLimit);
	
	if (n >= loopLimit)
	{
		return DT_FAILURE | DT_INVALID_CYCLE_PATH;
	}

    // 将终点->起点的路径，逆转为起点->终点的路径
	result.reserve(n);

	// ... 一些收尾和赋值工作

	return status;
}

Detour对A*算法的优化

通过以上UE4代码的分析，我们可以总结一下Detour和UE4所作的一些优化：

1. 我们可以去掉CLOSE集合，而只是在当前节点记录一个状态表示当前是否已经CLOSE。

2. 对于没有负权值的路径图，只要在CLOSE状态的节点，我们可以不用再去计算

3. 对于OPEN集，我们只需要 $f(n)$ 最小的元素。因此使用一个最小堆来作为OPEN集。

4. 对于实际的启发因子，可以乘上一个系数，来给不同的路径刷上权重，可以支持沼泽、雪地等多种地形综合寻路。

5. 对于不同的area，实际游戏中不希望路径经常切换area，因此可以加上一个areaChangeCost来减少这种情况。

另外，UE4也加上了各种循环限制来避免出现卡死的情况。算是让程序更加健壮。

小结

这一篇讲述了Detour是如何实现A算法，并对A 算法进行优化的。总体来看，基本就是一个标准的A* 算法优化实现。利用欧几里得距离是三维的这一点，可以直接实现三维坐标下的 A* 寻路。