A星寻路算法(四):二叉树与Heap

我们在寻路逻辑中对开放集合使用了for循环遍历，这种方式的时间复杂度为 O(n)，实现的逻辑代码如下：

for (int i = 1; i < openSet.Count; i ++) {
    if (openSet[i].fCost < node.fCost || (openSet[i].fCost == node.fCost && openSet[i].hCost < node.hCost)) {
        node = openSet[i];
    }
}

这节主要讲解如何更快的进行遍历，以及遍历所使用的方法，我们会使用Heap (堆)的概念进行逻辑处理。

我们可以将Heap想象成为一种类似二叉树的数据结构。每个节点都拥有自己的两个子节点。各自的子节点又有对应的两个子节点，这样以此类推。

需要注意的是在实际代码中，将使用数组表示。元素的索引按从左到右的顺序排列。但是在A星寻路算法中我们针对的是FCost进行的数据存储与查询。如下图：

针对该FCost的存储形式，父节点的FCost需要比子节点的FCost小才能满足该存储规则。假设我们要在节点FCost为10的节点下新增了FCost为5的节点。这时候就需要重新排序，因为它比父节点的FCost还小，这是不被允许的：

所以我们把5跟10的位置交换：

发现5还是比7小，所以还是得选择继续交换。

如果我们要取出FCost最小的那个节点，这时候该种存储数据方式的优势就显现出来了。我们只需要取出第一个节点(FCost为4)即可，因为该节点位于Heap的顶部，取出后的节点用最后一个节点补足：

接着我们还是要与它的子节点的FCost进行比较，确保父节点的FCost小于子节点。如果对比的结果比子节点小就需要与子节点交换。交换的结果如下：

这是一种快速的排序方式，我们只需要在排序过程中完成对其中一个节点侧的排序，对另一侧已完成的节点序可以不需要做任何修改。

上面是把FCost直接代入对应索引所对应的节点，以方便说明该种方式的优越性。现在切换回索引节点图，如果我们的FCost已经全部排序就绪，如何根据索引求取当前节点的父节点或两个子节点的索引，毕竟数组的存储是从左到右的顺序存储的：

假设我们要求取索引11的父节点：

只需要把 (11-1)/2，推导出一般性公式则为：

$$
Node_p=floor((n-1)/2)
$$

• 要求取索引11的父节点，只需要让其减1的结果再除以2，等于5。
• 要求取索引12的父节点也一样，只需要让其减1的结果再除以2，等于5.5，需要向下取整等于5。

如果已知节点的索引是5，要求取左子节点与右子节点呢？

求取左子节点的公式为：

$$
Node_{childL}=2*n+1
$$

求取右子节点的公式为:

$$
Node_{childR}=2*n+2
$$

实现Heap：

using UnityEngine;
using System.Collections;
using System;

/// <summary>
/// 泛型最小堆类，用于高效维护优先队列
/// 泛型约束：T 必须实现 IHeapItem<T> 接口（提供 HeapIndex 和 CompareTo 方法）
/// </summary>
public class Heap<T> where T : IHeapItem<T> {

	// 存储堆元素的数组
	private T[] items;
	// 当前堆中元素数量
	private int currentItemCount;

	/// <summary>
	/// 构造函数：初始化堆的最大容量
	/// </summary>
	/// <param name="maxHeapSize">堆的最大容量</param>
	public Heap(int maxHeapSize) {
		items = new T[maxHeapSize];
	}

	/// <summary>
	/// 向堆中添加新元素
	/// 将元素放在数组末尾，然后通过向上排序（SortUp）维护堆结构
	/// </summary>
	/// <param name="item">要添加的元素</param>
	public void Add(T item) {
		item.HeapIndex = currentItemCount; // 设置元素在堆中的索引
		items[currentItemCount] = item;    // 将元素放入数组末尾
		SortUp(item);                      // 向上排序以维护堆结构
		currentItemCount++;                // 元素计数加1
	}

	/// <summary>
	/// 移除并返回堆顶元素（最小元素）
	/// 将数组最后一个元素移动到堆顶，然后通过向下排序（SortDown）维护堆结构
	/// </summary>
	/// <returns>堆顶元素</returns>
	public T RemoveFirst() {
		// 保存堆顶元素
		T firstItem = items[0];
		// 元素计数减1
		currentItemCount--;
		// 将最后一个元素移动到堆顶
		items[0] = items[currentItemCount];
		// 更新堆顶元素的索引
		items[0].HeapIndex = 0;
		// 向下排序以维护堆结构
		SortDown(items[0]);
		// 返回原堆顶元素
		return firstItem;
	}

	/// <summary>
	/// 更新堆中的元素位置（当元素的优先级变化时调用）
	/// 通过向上排序（SortUp）调整元素位置
	/// </summary>
	/// <param name="item">需要更新的元素</param>
	public void UpdateItem(T item) {
		SortUp(item);
	}

	/// <summary>
	/// 获取当前堆中元素数量
	/// </summary>
	public int Count {
		get {
			return currentItemCount;
		}
	}

	/// <summary>
	/// 检查堆是否包含指定元素
	/// 通过比较元素的 HeapIndex 和实际值进行验证
	/// </summary>
	/// <param name="item">要查找的元素</param>
	/// <returns>是否包含该元素</returns>
	public bool Contains(T item) {
		return Equals(items[item.HeapIndex], item);
	}

	/// <summary>
	/// 向下排序（从父节点到子节点）
	/// 维护堆的最小堆性质（父节点 <= 子节点）
	/// </summary>
	/// <param name="item">当前需要排序的节点</param>
	void SortDown(T item) {
		while (true) {
			// 计算左右子节点的索引
			int childIndexLeft = item.HeapIndex * 2 + 1;
			int childIndexRight = item.HeapIndex * 2 + 2;
			int swapIndex = 0;

			// 如果左子节点在堆范围内
			if (childIndexLeft < currentItemCount) {
				// 默认选择左子节点
				swapIndex = childIndexLeft;

				// 如果右子节点也在范围内
				if (childIndexRight < currentItemCount) {
					// 选择较小的子节点
					if (items[childIndexLeft].CompareTo(items[childIndexRight]) < 0) {
						swapIndex = childIndexRight;
					}
				}

				// 如果当前节点大于选中的子节点，则交换
				if (item.CompareTo(items[swapIndex]) < 0) {
					Swap(item, items[swapIndex]);
				}
				else {
					// 否则排序完成
					return;
				}

			}
			else {
				// 没有子节点，排序完成
				return;
			}

		}
	}

	/// <summary>
	/// 向上排序（从子节点到父节点）
	/// 维护堆的最小堆性质（父节点 <= 子节点）
	/// </summary>
	/// <param name="item">当前需要排序的节点</param>
	void SortUp(T item) {
		// 计算父节点的索引
		int parentIndex = (item.HeapIndex-1)/2;
	
		// 不断比较并交换，直到满足堆性质或到达根节点
		while (true) {
			// 获取父节点
			T parentItem = items[parentIndex];
			// 如果当前节点大于父节点，则交换
			if (item.CompareTo(parentItem) > 0) {
				Swap(item, parentItem);
			}
			else {
				// 否则排序完成
				break;
			}

			// 更新父节点索引
			parentIndex = (item.HeapIndex-1)/2;
		}
	}

	/// <summary>
	/// 交换两个元素的位置，并更新它们的 HeapIndex
	/// </summary>
	/// <param name="itemA">元素A</param>
	/// <param name="itemB">元素B</param>
	void Swap(T itemA, T itemB) {
		// 交换数组中的位置
		items[itemA.HeapIndex] = itemB;
		items[itemB.HeapIndex] = itemA;
		// 更新元素的 HeapIndex
		int itemAIndex = itemA.HeapIndex;
		itemA.HeapIndex = itemB.HeapIndex;
		itemB.HeapIndex = itemAIndex;
	}
}

/// <summary>
/// 堆元素接口，定义堆操作所需的基本功能
/// </summary>
public interface IHeapItem<T> : IComparable<T> {
	/// <summary>
	/// 获取或设置该元素在堆中的索引位置
	/// </summary>
	int HeapIndex {
		get;
		set;
	}
}

我们还需要修改下PathFinding的逻辑，把开放集修改为Heap存储：

/// <summary>
/// A* 寻路算法主函数
/// </summary>
/// <param name="startPos">起点世界坐标</param>
/// <param name="targetPos">终点世界坐标</param>
void FindPath(Vector3 startPos, Vector3 targetPos) {
    // 将世界坐标转换为对应的网格节点
    Node startNode = grid.NodeFromWorldPoint(startPos);
    Node targetNode = grid.NodeFromWorldPoint(targetPos);

    // 初始化开放集（使用最小堆实现）和关闭集
    Heap<Node> openSet = new Heap<Node>(grid.MaxSize); // 堆容量设为网格最大节点数
    HashSet<Node> closedSet = new HashSet<Node>();
    openSet.Add(startNode); // 将起点加入开放集

    // 主循环：持续搜索直到开放集为空
    while (openSet.Count > 0) {
        // 从堆中取出 fCost 最小的节点（A* 算法核心）
        Node currentNode = openSet.RemoveFirst();
        closedSet.Add(currentNode); // 标记当前节点为已访问

        // 如果找到目标节点，回溯路径并结束搜索
        if (currentNode == targetNode) {
            RetracePath(startNode, targetNode);
            return;
        }

        // 遍历当前节点的所有邻居节点
        foreach (Node neighbour in grid.GetNeighbours(currentNode)) {
            // 跳过不可行走的节点或已访问的节点
            if (!neighbour.walkable || closedSet.Contains(neighbour)) {
                continue;
            }

            // 计算从起点到邻居的新路径成本
            int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
          
            // 如果新路径成本更低，或邻居尚未加入开放集
            if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) {
                // 更新邻居节点的成本和父节点
                neighbour.gCost = newMovementCostToNeighbour;
                neighbour.hCost = GetDistance(neighbour, targetNode);
                neighbour.parent = currentNode;

                // 将邻居加入开放集（如果尚未在其中）
                if (!openSet.Contains(neighbour))
                    openSet.Add(neighbour);
                else {
                    // 以下代码被注释：当节点已存在于堆中时，应调用 UpdateItem 通知堆更新该节点的位置
                    // openSet.UpdateItem(neighbour);
                }
            }
        }
    }
}

还需要让Node实现IHeapItem接口，让其具备CompareTo与HeapIndex能力：

using UnityEngine;
using System.Collections;

/// <summary>
/// 表示网格中的一个节点，用于A*寻路算法
/// 实现 IHeapItem<Node> 接口以支持堆操作
/// </summary>
public class Node : IHeapItem<Node> {

	/// <summary>
	/// 节点是否可行走（障碍物不可行走）
	/// </summary>
	public bool walkable;

	/// <summary>
	/// 节点在世界坐标系中的位置
	/// </summary>
	public Vector3 worldPosition;

	/// <summary>
	/// 节点在网格中的X轴索引
	/// </summary>
	public int gridX;

	/// <summary>
	/// 节点在网格中的Y轴索引
	/// </summary>
	public int gridY;

	/// <summary>
	/// 从起点到当前节点的实际移动成本（g值）
	/// </summary>
	public int gCost;

	/// <summary>
	/// 从当前节点到目标节点的启发式估计成本（h值）
	/// </summary>
	public int hCost;

	/// <summary>
	/// 当前节点的父节点，用于路径回溯
	/// </summary>
	public Node parent;

	/// <summary>
	/// 节点在堆中的索引位置（由堆维护）
	/// </summary>
	int heapIndex;

	/// <summary>
	/// 节点构造函数
	/// </summary>
	/// <param name="_walkable">是否可行走</param>
	/// <param name="_worldPos">世界坐标位置</param>
	/// <param name="_gridX">网格X索引</param>
	/// <param name="_gridY">网格Y索引</param>
	public Node(bool _walkable, Vector3 _worldPos, int _gridX, int _gridY) {
		walkable = _walkable;
		worldPosition = _worldPos;
		gridX = _gridX;
		gridY = _gridY;
	}

	/// <summary>
	/// 获取节点的总评估成本 f = g + h
	/// </summary>
	public int fCost {
		get {
			return gCost + hCost;
		}
	}

	/// <summary>
	/// 获取或设置节点在堆中的索引位置
	/// </summary>
	public int HeapIndex {
		get {
			return heapIndex;
		}
		set {
			heapIndex = value;
		}
	}

	/// <summary>
	/// 比较当前节点与另一个节点的优先级（用于堆排序）
	/// 返回值规则：
	/// - 负值：当前节点应排在参数节点之前
	/// - 正值：当前节点应排在参数节点之后
	/// - 0：优先级相同
	/// 
	/// 注意：返回值取反（-compare）是为了配合最小堆实现
	/// 实际比较逻辑为：优先级由 fCost 升序，若 fCost 相同则按 hCost 升序
	/// </summary>
	/// <param name="nodeToCompare">要比较的节点</param>
	/// <returns>比较结果</returns>
	public int CompareTo(Node nodeToCompare) {
		int compare = fCost.CompareTo(nodeToCompare.fCost);
		if (compare == 0) {
			compare = hCost.CompareTo(nodeToCompare.hCost);
		}
		return -compare; // 取反以适配最小堆排序（fCost 小的节点优先）
	}
}

通过Heap的方式优化后，我们的寻路逻辑比之前更加快速。由之前的平均25ms的计算时间，缩短至5ms。并且随着地图的增大，优化的效率会越明显。