A*寻路算法(三)：Unity中三维动态A*网格寻路的实现

经过前两篇文章的探讨，我们已经掌握了A*寻路算法的核心思想和实现流程。在图中进行寻路的前提，是必须获取每个节点的关键信息，包括其坐标和障碍物状态等。

本文将聚焦于在Unity引擎中实现基于网格的A*寻路算法，使其适用于三维空间，并能够动态避开移动障碍物，最终找到最短路径。

1. 核心架构设计

系统采用分层架构实现职责分离：

graph TD A[寻路系统PathFindingSystem] --> B[动态障碍物管理] A --> C[节点地图管理] A --> D[路径规划算法] B --> F[动态障碍物检测] C --> G[节点状态缓存] D --> H[A*算法核心]

• 路径平滑处理： 对A*生成的原始网格路径进行平滑处理（如使用漏斗算法或贝塞尔曲线），使其更自然。
• 动态障碍物检测： 具体执行物理检测，判断网格点是否被动态障碍物占据。
• 节点状态缓存： 存储节点状态（尤其是障碍物状态），避免重复进行昂贵的物理检测。
• A*算法核心： 实现A*算法的具体步骤（开放列表、关闭列表、代价计算等）。

**2. 节点构造与寻路过程 **

对于大型世界地图，预先存储所有可能的节点信息在性能上是不可行的。因此，我们的策略是让A*算法在寻路过程中按需创建和查询节点。当算法访问到某个网格坐标时，首先检查该位置对应的节点是否已存在（并缓存了状态信息）；如果存在，则直接使用缓存结果；如果不存在，则创建新节点并执行必要的状态检测（如障碍物检测）。这种基于网格的坐标查询方式非常高效且直观。

Node GetOrCreateNode(Vector3Int position)
    {
        if (!nodeMap.TryGetNode(GetId(position), out Node node))
        {
            node.SetPosition(position);
            // 默认障碍物检测（可选）
            node.UpdateObstacleStatus(IsObstacle(position));
        }
        return node;
    }

为此我们需要的节点数据结构：

class Node
{
    public Vector3Int Position { get; set; }
    public AStarCell Parent { get; set; }
    public int F { get; set; }  // F = G + H
    public int G { get; set; }
    public int H { get; set; }
    public bool IsDynamicObstacle { get; set; } // 是否为障碍，需要在查询过程中确认
}

结合结论，最终的寻路函数大抵如下：

List<Vector3Int> FindPath(Vector3Int startPos, Vector3Int endPos)
{
        ResetSearch();//每次寻路清理开发集合和关闭集合
        
        //GetOrCreateNode 返回目标网格节点Node，若该网格没查询过，则新构建一个加入状态表
        var start = GetOrCreateNode(startPos);
        var end = GetOrCreateNode(endPos);

        // 验证起点和终点是否存在障碍
        if (start.IsDynamicObstacle || end.IsDynamicObstacle)
        {
            Debug.LogWarning("起点或终点被障碍物阻挡");
            return new List<Vector3Int>();
        }

        AddToOpenList(start);
        start.CalculateValues(end, start);

        int nodesProcessed = 0;//查询计数器
        while (openList.Count > 0 && nodesProcessed < maxSearchNodes)
        {
            nodesProcessed++;

            //从优先队列取出节点
            var currentNode = RemoveFromOpenList();

            // 检查是否到达终点
            if (currentNode.Equals(end))
            {
                //RetracePath 回溯节点，返回路径列表
                return RetracePath(currentNode);
            }
               
            //加入关闭集合
            AddToClosedList(currentNode);

            // 处理邻居节点
            foreach (var neighborPos in GetNeighbors(currentNode))
            {
                // 检查节点是否在关闭列表中
                if (closedDic.ContainsKey(GetId(neighborPos)))
                    continue;

                var neighbor = GetOrCreateNode(neighborPos);

                // 跳过障碍物
                if (neighbor.IsDynamicObstacle)
                    continue;

                // 计算新路径代价
                int newMovementCost = currentNode.G + neighbor.CalculateMovementCost(currentNode);

                // 检查是否在开放列表中
                if (openDic.ContainsKey(neighbor.Id))
                {
                    // 如果新路径更好，更新节点
                    if (newMovementCost < neighbor.G)
                    {
                        neighbor.Parent = currentNode;
                        neighbor.CalculateValues(end, start);
                        openList.RebuildHeap();//整理开放集合，确保队列顶部节点F值最小
                    }
                }
                else
                {
                    // 新节点加入开放列表
                    neighbor.Parent = currentNode;
                    neighbor.CalculateValues(end, start);
                    AddToOpenList(neighbor);
                }
            }
        }

        Debug.LogWarning($"寻路失败，未找到路径 {nodesProcessed}");
        return new List<Vector3Int>();
    }

熟悉A*的话上面的内容也不会陌生了，还有一个问题是如何去管理被存储的节点Node，用什么样的集合去管理？最理想的是哈希表，以坐标值作为键(或者设计哈希函数将坐标值转为唯一值，但要注意哈希冲突问题)进行复杂度O(1)的查询。

节点管理与快速查询

• 核心需求： 需要高效地根据坐标查找或创建节点。
• 解决方案：哈希表 (Dictionary)
  • 使用节点的坐标作为键进行存储和检索。
  • 时间复杂度接近 O(1)，效率极高。
• 键的设计 (坐标 -> 唯一ID)： 将三维坐标 Vector3Int 映射为一个唯一的整数ID，作为哈希表的键。

  • 哈希函数示例：

    public static int GetId(Vector3Int pos)
    {
        unchecked // 允许整数溢出（不报错），常用于哈希计算
        {
            // 使用质数乘法、位运算组合坐标分量，减少冲突概率
            return (pos.x * 31) ^ (pos.y << 16) ^ (pos.z * 63);
        }
    }

  • 注意： 虽然冲突概率很低，但设计良好的哈希函数至关重要。也可考虑 (x, y, z) 直接作为复合键（如果语言/环境支持）。

3. 节点状态管理架构

缓存与更新

• 节点状态（尤其是 IsDynamicObstacle）需要进行物理检测（Physics.CheckSphere），开销较大。
• 每次寻路都会重置节点对象（清空 Parent, G, H, F），但物理检测结果（障碍状态）不应该每次都被丢弃，否则会造成大量重复的昂贵检测。

三层缓存体系

解决方案：三层缓存体系

graph LR A[PathFindingSystem] -->|请求节点/状态| C[NodeMap] C -->|对象复用| B[节点对象池 NodePool] C -->|状态缓存| D[节点状态字典 NodeStatusCache] C -->|按需检测| E[物理碰撞查询 Physics.CheckSphere]

---

1. 节点对象池 (NodePool):

   • 目的： 避免频繁创建和销毁 Node 对象带来的内存分配/回收开销和内存碎片。

   • 机制： 当节点不再被当前寻路使用（即每次 ResetSearch 后），将其回收到对象池中，而不是销毁。下次需要相同坐标的节点时，优先从池中获取复用对象。

   • 实现简化示例：

     public class NodePool
     {
         private Dictionary<int, Node> _activeNodes = new Dictionary<int, Node>(); // 当前活跃节点 (ID->Node)
         private Stack<Node> _inactivePool = new Stack<Node>(); // 闲置节点池
     
         public Node GetNode(int id)
         {
             if (!_activeNodes.TryGetValue(id, out Node node))
             {
                 // 池中有闲置节点则复用，否则新建
                 node = (_inactivePool.Count > 0) ? _inactivePool.Pop() : new Node();
                 _activeNodes.Add(id, node);
             }
             return node;
         }
     
         public void ReleaseNode(Node node)
         {
             // 从活跃字典移除
             if (_activeNodes.Remove(GetId(node.Position)))
             {
                 // 重置节点寻路相关状态（Parent, G, H, F），**保留障碍状态**
                 node.ResetForReuse();
                 // 放入闲置池
                 _inactivePool.Push(node);
             }
         }
         // ... (ResetPool 等方法)
     }

2. 节点状态缓存 (NodeStatusCache):

   • 目的： 持久化存储物理检测结果（IsDynamicObstacle），避免对同一网格位置进行重复检测。
   • 机制： 一个独立的 Dictionary<int, bool> 或其他结构，以节点ID为键，存储该位置的障碍物状态。Node 对象的 IsDynamicObstacle 属性在获取时从该缓存读取（或在首次检测后写入缓存）。
   • 更新： 由动态障碍物管理模块负责在障碍物移动后，更新其影响范围内网格在缓存中的状态。

3. 物理碰撞查询 (Physics.CheckSphere):

   • 目的： 实际检测某个网格点是否被障碍物占据。

   • 机制： 仅在以下情况调用：

     • 节点状态缓存中不存在该位置的记录（首次访问）。
     • 动态障碍物管理模块通知该位置的状态可能已改变（障碍物移动后）。

   • 检测方法：

     bool IsObstacle(Vector3 worldPos)
     {
         // 在网格中心位置(worldPos)检测球形碰撞，半径通常设为网格尺寸(gridSize)的一半
         // obstacleLayerMask 指定要检测的障碍物层级
         return Physics.CheckSphere(worldPos, gridSize * 0.5f, obstacleLayerMask);
     }

对象池资源复用

// 节点对象池实现
public class CellPool<V> where V : class {
    private Dictionary<int, V> _activeNodes;
    private Stack<V> _inactivePool;
    
    public V GetNode(int id) {
        if (!_activeNodes.TryGetValue(id, out V node)) {
            node = _inactivePool.Count > 0 ? 
                   _inactivePool.Pop() : CreateNewNode();
            _activeNodes.Add(id, node);
        }
        return node;
    }
}

4. 动态障碍物实时响应机制

核心目标： 当场景中的障碍物（带有Collider）移动（位置、旋转、缩放改变）时，系统能及时感知并更新受其影响的网格节点的障碍状态。

核心工作流程

sequenceDiagram participant Obstacle as 动态障碍物 participant PathSystem as 寻路系统(动态障碍管理) participant NodeMap as 节点地图(NodeMap/状态缓存) Obstacle->>PathSystem: 初始化时注册自身Collider loop 每帧(或定时/变化时触发) Obstacle->>Obstacle: 检测自身Transform(位置/旋转/缩放)是否变化 alt 发生变化 Obstacle->>PathSystem: 报告变化，并提交旧的覆盖网格列表(待清理) PathSystem->>NodeMap: 将旧列表中的网格状态标记为"待重新检测"或清除其障碍缓存 Obstacle->>PathSystem: 计算新的Collider包围盒(AABB) PathSystem->>PathSystem: 将新包围盒覆盖的网格坐标加入检测队列 end end PathSystem->>NodeMap: (异步/分帧)对检测队列中的网格执行物理检测 NodeMap->>NodeMap: 更新节点状态缓存(NodeStatusCache)

---

关键技术点：

1. 障碍物注册与追踪： 动态障碍物需在初始化时向寻路系统注册，系统保存其引用和Collider。

2. 变化检测： 障碍物（或系统）每帧检测其Transform是否发生变化。

3. 包围盒(AABB)计算： 使用 Collider.bounds 获取障碍物的轴对齐包围盒。

4. 网格坐标映射：

   • 将包围盒的 min 和 max 世界坐标对齐到网格坐标 (Vector3Int)。
   • 关键函数： Mathematics.AlignToGrid (或自定义实现，将世界坐标除以 gridSize 并取整/舍入)。

5. 遍历受影响的网格：

   private List<Vector3Int> GetGridsInBounds(Bounds bounds)
   {
       List<Vector3Int> affectedGrids = new List<Vector3Int>();
       Vector3Int minGrid = WorldToGrid(bounds.min);
       Vector3Int maxGrid = WorldToGrid(bounds.max);
   
       // 遍历包围盒覆盖的三维网格范围
       for (int x = minGrid.x; x <= maxGrid.x; x++)
       {
           for (int y = minGrid.y; y <= maxGrid.y; y++)
           {
               for (int z = minGrid.z; z <= maxGrid.z; z++)
               {
                   Vector3Int gridPos = new Vector3Int(x, y, z);
                   affectedGrids.Add(gridPos);
               }
           }
       }
       return affectedGrids;
   }

6. 状态更新：

   • 当障碍物移动时，首先清除 其之前影响范围内网格的缓存障碍状态（将这些网格标记为"需要重新检测"或直接清除缓存条目）。
   • 然后计算 其新的影响范围（包围盒映射的网格列表）。
   • 将新范围内的网格坐标加入一个待检测队列。
   • 系统在后续帧（或异步任务）中，对这个队列中的网格位置执行 IsObstacle 物理检测。
   • 将检测结果更新到节点状态缓存 (NodeStatusCache) 中。

7. 寻路影响： 后续的寻路请求在访问这些更新过的网格节点时，会直接从状态缓存中读取到最新的障碍物状态。