A星寻路算法(三):实现寻路

我们已在A星寻路算法一:算法中介绍了A星算法的原理，以及如何用A星寻路算法实现网格。这节是A星寻路算法的第三篇。我们将用代码实现寻路逻辑。

目前我们已经实现了网格与障碍物。如图：

我们的寻路算法逻辑与第一节中讲的原理是一样的。我们新增PathFinding脚本。并把该脚本挂载至A*节点中：

以下是PathFinding脚本的完整代码与详细注释：

using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class Pathfinding : MonoBehaviour {

    // 寻路起点和终点的游戏物体
    public Transform seeker, target;
  
    // 网格管理器组件
    Grid grid;

    // 初始化时获取网格组件
    void Awake() {
        grid = GetComponent<Grid>();
    }

    // 每帧更新时寻找路径
    void Update() {
        FindPath(seeker.position, target.position);
    }

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

        // 开放集：待探索的节点列表
        List<Node> openSet = new List<Node>();
        // 关闭集：已探索过的节点集合（使用HashSet提高查找效率）
        HashSet<Node> closedSet = new HashSet<Node>();
  
        // 将起点加入开放集开始搜索
        openSet.Add(startNode);

        // 主循环：直到开放集为空
        while (openSet.Count > 0) {
            // 在开放集中找到fCost最小的节点（启发式函数值最低的节点）
            Node node = openSet[0];
            for (int i = 1; i < openSet.Count; i++) {
                // 按fCost排序，若fCost相同则按hCost排序
                if (openSet[i].fCost < node.fCost || 
                    (openSet[i].fCost == node.fCost && openSet[i].hCost < node.hCost)) {
                    node = openSet[i];
                }
            }

            // 将当前节点移出开放集并加入关闭集
            openSet.Remove(node);
            closedSet.Add(node);

            // 如果找到目标节点，回溯路径并返回
            if (node == targetNode) {
                RetracePath(startNode, targetNode);
                return;
            }

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

                // 计算从当前探索的起点(Parent)到邻居的新路径成本(Gnew = Gparent + Distance)
                int newCostToNeighbour = node.gCost + GetDistance(node, neighbour);
  
                // 如果新路径成本更低，或邻居不在开放集中
                if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) {
                    // 更新邻居节点的成本和父节点
                    neighbour.gCost = newCostToNeighbour;
                    // hCost是从当前节点到目标的启发式估计
                    neighbour.hCost = GetDistance(neighbour, targetNode);
                    neighbour.parent = node;

                    // 将邻居加入开放集（如果尚未在其中）
                    if (!openSet.Contains(neighbour))
                        openSet.Add(neighbour);
                }
            }
        }
    }

    // 回溯路径并保存到网格中
    void RetracePath(Node startNode, Node endNode) {
        List<Node> path = new List<Node>();
        Node currentNode = endNode;

        // 从终点反向追踪到起点
        while (currentNode != startNode) {
            path.Add(currentNode);
            currentNode = currentNode.parent;
        }
        // 反转路径使其从起点到终点
        path.Reverse();

        // 将路径保存到网格组件中
        grid.path = path;
    }

    // 计算两个节点之间的对角线距离（适用于8方向移动）
    int GetDistance(Node nodeA, Node nodeB) {
        int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
        int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);

        // 使用14和10作为斜边和直行的成本系数（基于10单位的格子大小）
        // 先走斜线再直行
        if (dstX > dstY)
            return 14*dstY + 10*(dstX-dstY); 
        else
            return 14*dstX + 10*(dstY-dstX);
    }
}

这里主要讲解下GetDistance函数。主要指两个网格节点如何求取距离。展开讲有两种方式。一种是当两个节点X轴方向的差值大于Y轴方向的差值，如下图：

第二种是当两个节点X轴方向的差值小于Y轴方向的差值，如下图：

在Grid类中我们新增了GetNeighbours函数：

/// <summary>
/// 获取指定节点的所有相邻可行走节点（8个方向）
/// </summary>
/// <param name="node">需要获取邻居的当前节点</param>
/// <returns>返回包含所有合法邻居节点的列表</returns>
public List<Node> GetNeighbours(Node node) {
    // 初始化邻居节点列表
    List<Node> neighbours = new List<Node>();

    // 遍历当前节点周围的8个方向（含对角线）
    for (int x = -1; x <= 1; x++) {
        for (int y = -1; y <= 1; y++) {
            // 跳过自身位置（x=0,y=0）
            if (x == 0 && y == 0)
                continue;

            // 计算目标位置的网格坐标
            int checkX = node.gridX + x;
            int checkY = node.gridY + y;

            // 检查目标坐标是否在网格范围内
            if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY) {
                // 将合法的邻居节点添加到列表中
                // grid[checkX, checkY] 是通过网格索引获取对应位置的节点
                neighbours.Add(grid[checkX, checkY]);
            }
        }
    }

    return neighbours;
}

最后效果：