Recast&Detour中A*算法的应用

A*算法介绍

    先看看这个绝对能有个清晰的认识 https://blog.csdn.net/hitwhylz/article/details/23089415

    （influence Mapping、knownWalkability 数组）

     以及这篇英文以及他的翻译（为什么叫A*算法？）

自己总结一下：

    问题描述：从一个节点到另一个节点的最优路径。最优的定义使得f值最小。f = c + h 

    已知条件或者需要准备好的条件：

            1：每个节点的邻节点有哪些。

            2：cost函数 和 heri函数的计算函数选取。

    步骤：

        1. 初始化：把起点节点放入openlist表中，计算c值，h值，以及f值，以及父索引设置为空。closelist表初始化为空。

        2. 当openlist不为空时：

                1）从openlist中取出f值最小的节点，放入closelist中。

                2）如果该节点就是要找的终点，那么进入到4.

                3）遍历该节点的邻节点

                            如果邻节点已经在closelist中，请忽略他。

                           如果邻节点已经在openlist中，重新计算其c值，如果比原来小，更新他，更新的内容包括父索引和c值以及f值，否则忽略他。

                            除了上述两种情况，该节点就是第一次访问到的，计算c值，h值，以及f值，以及设置其父索引节点，并把他放入到openlist中。

        3. 返回2.直到openlist为空或者找到终点

        4. 根据找到的node以及其父索引node就是一条反向的路径，从终点到起点。       

源码

使用二叉堆来维护openlist列表（保证堆顶元素始终是具有最小f值的，插入openlist和从openlist中取出堆顶元素的时候都要进行重新维护堆）

使用flag值来标记是否在closelist OR openlist

如果找到的邻节点在closelist中没有直接忽略，而是对比其f值。如果更小，会再一次放入openlist（会出现这种情况吗？）

// 如果不能找到 endRef 节点，那就把离他最近的一个poly节点当作是 path 的最后一个节点
// 如果找到的 path 中节点太多了(大于 maxPath)，就只取前面 maxPath 个 
dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
								  const float* startPos, const float* endPos,
								  const dtQueryFilter* filter,
								  dtPolyRef* path, int* pathCount, const int maxPath) const
{
	dtAssert(m_nav);
	dtAssert(m_nodePool);
	dtAssert(m_openList);
	if (pathCount)
		*pathCount = 0;
	
	// Validate input
	if (!m_nav->isValidPolyRef(startRef) || !m_nav->isValidPolyRef(endRef) ||
		!startPos || !endPos || !filter || maxPath <= 0 || !path || !pathCount)
		return DT_FAILURE | DT_INVALID_PARAM;
	// 如果起始节点 和 终止节点是同一个
	if (startRef == endRef)
	{
		path[0] = startRef;
		*pathCount = 1;
		return DT_SUCCESS;
	}
	// 1.初始化工作 把起始节点存入到 openlist 中
	m_nodePool->clear();// 初始化数组链表 m_first , 以及 m_nodeCount. 让 getNode 能正常工作。
	m_openList->clear();
	// 创建Node, getNode 中没有初始化pos, 外面没有初始化 state
	dtNode* startNode = m_nodePool->getNode(startRef);
	dtVcopy(startNode->pos, startPos);
	startNode->pidx = 0;// parent idx 标识父节点为空
	startNode->cost = 0;// c = 0
	startNode->total = dtVdist(startPos, endPos) * H_SCALE;// g = c + h = h
	startNode->id = startRef;
	startNode->flags = DT_NODE_OPEN;// DT_NODE_OPEN = 0x01,
	m_openList->push(startNode);// 存入 m_openList->m_heap[i] 中 即存入二叉堆结构中
	
	// 初始化path中的最后一个poly节点以及g值
	dtNode* lastBestNode = startNode;
	float lastBestNodeCost = startNode->total;
	
	bool outOfNodes = false;
	
	// 2.遍历openlist
	while (!m_openList->empty())
	{
		// 1)Remove node from open list and put it in closed list.
		dtNode* bestNode = m_openList->pop();
		bestNode->flags &= ~DT_NODE_OPEN;	// 移出openlist
		bestNode->flags |= DT_NODE_CLOSED;	// 移入closelist
		
		// 2)Reached the goal, stop searching.
		if (bestNode->id == endRef)
		{
			lastBestNode = bestNode;
			break;
		}
		
		// Get current poly and tile.
		// The API input has been checked already, skip checking internal data.
		const dtPolyRef bestRef = bestNode->id;
		const dtMeshTile* bestTile = 0;
		const dtPoly* bestPoly = 0;
		m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
		
		// Get parent poly and tile.如果没有parent 均为0
		dtPolyRef parentRef = 0;
		const dtMeshTile* parentTile = 0;
		const dtPoly* parentPoly = 0;
		if (bestNode->pidx)
			parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
		if (parentRef)
			m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
		// 3) 遍历该节点的邻节点
		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
		{
			dtPolyRef neighbourRef = bestTile->links[i].ref;
			
			// Skip invalid ids and do not expand back to where we came from.
			if (!neighbourRef || neighbourRef == parentRef)
				continue;
			
			// Get neighbour poly and tile.
			// The API input has been cheked already, skip checking internal data.
			const dtMeshTile* neighbourTile = 0;
			const dtPoly* neighbourPoly = 0;
			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);			
			
			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
				continue;

			// deal explicitly with crossing tile boundaries 
			// 不等于 0xff，说明是不同的tile之间的连接 solomesh 不存在的。
			// side == 0 2 4 6 >> 1， 变成 0 1 2 3
			unsigned char crossSide = 0;
			if (bestTile->links[i].side != 0xff)
				crossSide = bestTile->links[i].side >> 1;

			// get the node 如果在 getNode中第一次创建的，那么flags = 0
			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide);
			if (!neighbourNode)// 标记寻路过程中nodepool 空间不足，会可能出现的不是最优解
			{
				outOfNodes = true;
				continue;
			}
			
			// If the node is visited the first time, calculate node position.
			// 赋值 neighbourNode->pos ，后面用来就算c 和 h 值
			if (neighbourNode->flags == 0)
			{
				getEdgeMidPoint(bestRef, bestPoly, bestTile,
								neighbourRef, neighbourPoly, neighbourTile,
								neighbourNode->pos);
			}

			// Calculate cost and heuristic.
			float cost = 0;
			float heuristic = 0;
			
			// Special case for last node.
			if (neighbourRef == endRef)
			{
				// Cost
				const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
													  parentRef, parentTile, parentPoly,
													  bestRef, bestTile, bestPoly,
													  neighbourRef, neighbourTile, neighbourPoly);
				const float endCost = filter->getCost(neighbourNode->pos, endPos,
													  bestRef, bestTile, bestPoly,
													  neighbourRef, neighbourTile, neighbourPoly,
													  0, 0, 0);
				// 计算真实的cost值。
				// h值为零，不需要估值了，因为已经到达终点了。
				cost = bestNode->cost + curCost + endCost;
				heuristic = 0;
			}
			else
			{
				// Cost
				const float curCost = filter->getCost(bestNode->pos, neighbourNode->pos,
													  parentRef, parentTile, parentPoly,
													  bestRef, bestTile, bestPoly,
													  neighbourRef, neighbourTile, neighbourPoly);
				cost = bestNode->cost + curCost;
				heuristic = dtVdist(neighbourNode->pos, endPos)*H_SCALE;
			}

			const float total = cost + heuristic;
			
			// The node is already in open list and the new result is worse, skip.
			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
				continue;
			// The node is already visited and process, and the new result is worse, skip.
			if ((neighbourNode->flags & DT_NODE_CLOSED) && total >= neighbourNode->total)
				continue;
			
			// Add or update the node.
			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
			neighbourNode->id = neighbourRef;
			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
			neighbourNode->cost = cost;
			neighbourNode->total = total;
			
			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);
			}
			
			// Update nearest node to target so far.
			if (heuristic < lastBestNodeCost)
			{
				lastBestNodeCost = heuristic;
				lastBestNode = neighbourNode;
			}
		}
	}
       
	dtStatus status = getPathToNode(lastBestNode, path, pathCount, maxPath);

	if (lastBestNode->id != endRef)
		status |= DT_PARTIAL_RESULT;

	if (outOfNodes)
		status |= DT_OUT_OF_NODES;
	
	return status;
}

---

记录一下看了这个系列文章的重点。

前言：

Pathfinding vs Movement

Pathfinding 是找到一条路径。如果只有pathfind算法，那么movement就根据这条路径进行移动，而不管中途有没有出现障碍。

Movement 是根据某条路径进行移动。如果只有movement算法，那么就是走到哪算哪，没有进行提前规划。

寻路系统需要两者结合。

 

Pathfinding

这篇文章图文并茂循序渐进的介绍了几种图搜索算法，从breadth first search（最简单） 到 Dijkstra's Algorithm 到 Greedy-First Search 最后引出 A* 算法（最优）。

算法比较

	Breadth First Search	Dijkstra’s Algorithm（Uniform Cost Search）
共同点	1.       保证找到最优的路径（cost值最小）。 2.       当需要查找所有方向的路径的时候使用这两种算法。
合适用的场景	Node之间移动Cost值一样	不同类型的node之间cost值不一样
搜索过程	朝所有方向平等的搜索	优先对cost值小的方向移动
算法用途	Regular Pathfinding Flowfield Pathfinding Distance map procedural map generation

 

	Dijkstra’s Algorithm（Uniform Cost Search）	Greedy Best First Search	A*（是Dijkstra 和 greedy的结合。前者只计算起点到当前点的cost，后者只计算当前点到终点的heuristic）
是否保证最短路径	是	不保证，但是速度会比Dijkstra快。	保证，当heuristic值不大于真实的cost值（从当前点到终点）时。
合适用的场景		如果想要查找某一个方向上的路径。
搜索过程	对cost值最小方向移动	朝着终点方向移动，但是途中遇到障碍物，就可能不是最优路径	能找到较快以及找到最优的路径

 

A*算法中的heuristic值

h(n) 函数表示的是 从当前点n到目标点goal的cost的预估值。

如果 h(n) 的值=0，那么A*算法就退化为 Dijkstra算法。

如果 h(n) 能恰好是 n 到 goal 的真实cost， 那么A* 算法能以最快的速度（扩展最少的节点）找到最优路径。

如果 h(n) 的值 远远大于 g(n) (从起点到n的cost值)，那么 A*算法就退化为 Greedy Best First Search。

speed or accuracy

 

scale between g and h

 

exact heruistic

 

 

heruistic for grid map

进行图搜索的前提是要有图，图由node 和 edge 组成（也称为location 和 connection）。 图搜索算法目标需要寻找就是由node和edge组成的一条最优路径，首尾分别是起点node 和 终点node。

 

对于同一张地图，可以有不同的图结构设计，最常用的图就是grids（最简单，也是最多的节点）

 

其实图可以用在很多地方，比如状态机中的状态图，其中node就是state状态，edge就是action可以用来切换状态的输入事件。