A*寻路实例项目实践笔记

• 前言
• • A*的算法原理
  • 实现网格系统
  • A*寻路算法实现
  • 使用堆优化节点查找
  • 路径运动单元
  • 运用权重
  • 使权重更平滑（Smooth Weights）
  • 路径平滑

好久不见！今天使用Unity照着Sebastian Lague大佬的视频做一个A寻路算法的实例项目，包括A寻路的介绍，网格系统的创建，以及一些实际使用案例。

简单来说，A是一种寻路算法，通过A可以找到一系列网格中从A到B的最短路线。假如说，我们想得到下图所示A到B的最短路线，第一步我们需要得到关于A节点的所有相邻节点和这些相邻节点的的一些参数。第一个参数是相邻节点到A的距离，叫做G cost。第二个参数是该相邻节点到B节点的距离，叫做H cost，基本上可以说H cost是G cost的反面。最后一个参数为G cost和H cost的和，叫做F cost。

将这些相邻节点存入一个集合，然后算法看一圈，并拿到F cost最低的节点，对拿到的点重复上诉过程。

直到在相邻节点中找到了B点，算法就完成工作了。在没有障碍加入的情况下，路径向着终点就去了。

当然，如果你的游戏里面没有障碍物，还需要什么寻路算法，接下来我们看看加入障碍物以后的情况。如下图所示，在执行了一步操作以后，出现了三个F cost相同的相邻节点，这时候我们选择H cost，也就是离B点最近的节点

然后，依旧是选择F cost最小的两个中的一个，这一次两个节点从参数上看完全一样，所以随便选一个，反正如果选错的话得到的相邻节点的F cost最后都不可能大于另一个选择。

现在我们可以看到，鼠标指向的54是下一个选择，但是这里有个小细节，他左边相邻节点的G cost为38，而不是最短距离30，这是因为我们第一次将该节点考虑进来是通过鼠标指针上面的48和其相邻。所以说G cost得到的是根据最近一次路径运算得到的最小值，而如果我们马上考虑鼠标指向的54，我们就会发现到达这个左边相邻节点的G cost出现更小值，所以我们进行更新。这个节点现在G cost为更小值30，F cost为60。总结一下：G cost当前的值不一定是最小值，而是当前所以算过的路径下的最小值。其实说到这里，我们就知道节点还需要存储一个父节点，表明目前这个最小的G cost是从与哪个节点相邻得来的。

说句题外话，既然G cost不一定是最短， H cost呢？H cost一定是最短，H cost通过某节点先直线到B节点的同一横向或纵向，再直线到B得来的，所以一定是最短。

我们继续，现在更新后的60是最短，我们选60。

就这样一直选下去，最后我们就能得到这条路径，对了，别忘了前面说的每个节点需要记录自己是通过哪个父节点走到的，这样才能得到路径。

保存父节点，就像这样

来看一下A* 算法的伪代码。

可以将OPEN理解为上图中的绿色节点，意味着候选的路径节点，在算法起始时起点为OPEN中的唯一选择，CLOSED理解为红色的点过的节点，意味着算法已经算出了到这个点的最短距离了，以后也不需要再看他了。每次循环开始时从OPEN里面找到F cost最小的作为当前节点，对于当前节点的相邻节点，如果这个节点不可移动，或者已经找到最短路径了，就跳过，否则查看这个相邻节点是否不在OPEN里面或者能得到一个更短的G cost，一个更短的G cost意味着找到了新的到这个相邻节点的更短路径，需要更新这个节点的G cost F cost以及设置当前节点为该相邻节点新的父节点，而不在OPEN意味着该相邻节点从未被考虑，现在要被考虑进来。如此循环往复，当某次循环发现当前节点就是目标节点时，寻路结束。
接下来要做的就是找到目标节点的父节点，再找到这个父节点的父节点，直到找回起始点，得到路径。

要在项目中使用A*我们首先需要网格，这里大佬直接教我们自制一套网格系统。我们的网格包含一个节点类，一个网格类。节点负责定义网格中的一个位置以及该位置的信息（目前只有unwalkable）。节点将由网格负责创建。可以自定网格整体大小，单个节点大小，可以显示在Scene面板(OnDrawGizmos)，可以通过给定（网格中的）某个位置获得网格中的单个节点，由此可以获得玩家所在的网格节点。

节点类

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

public class Node
{
    // node能走吗？会根据是否与障碍物重合判断
    public bool walkable;
    // node中心的世界坐标位置
    public Vector3 worldPosition;
    // 在Grid二维坐标中的位置
    public int gridX;
    public int gridY;
    // 节点到起点的距离
    public int gCost;
    // 节点到终点的距离
    public int hCost;
    // 父节点 也就是路径中该节点的上一个节点
    public Node parent;

    // 构造函数
    public Node(bool _walkable, Vector3 _worldPos, int _gridX, int _gridY)
    {
        walkable = _walkable;
        worldPosition = _worldPos;
        gridX = _gridX;
        gridY = _gridY;
    }

    // fCost为gCost + hCost 所以写个属性就行
    public int fCost
    {
        get { return gCost + hCost; }
    }
}

网格类

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

// Grid的GameObject X、Y轴要在场景正中
public class Grid : MonoBehaviour
{
    // 用来存储unwalkable的layermask
    public LayerMask unwalkableMask;
    // grid的大小
    public Vector2 gridWorldSize; // Vector2的y对应世界坐标中的z轴
    // grid中的node的半径（node立方体边长的一半）
    public float nodeRadius;
    // 玩家的位置
    public Transform player;

    // grid是二位的node数组
    Node[,] grid;
    // grid中的node的直径
    float nodeDiameter;
    // Grid中的node数量
    int gridSizeX, gridSizeY;

    // 路径
    public List path;

    private void Start()
    {
        // 根据grid的尺寸和node的尺寸计算node的数量并填入二维数组
        nodeDiameter = nodeRadius * 2;
        gridSizeX = Mathf.RoundToInt(gridWorldSize.x / nodeDiameter);
        gridSizeY = Mathf.RoundToInt(gridWorldSize.y / nodeDiameter);
        CreateGrid();
    }

    // 创建grid实例
    private void CreateGrid()
    {
        grid = new Node[gridSizeX, gridSizeY];
        // 计算得到grid（从上往下看）左下角的世界坐标位置
        Vector3 worldButtomLeft = transform.position - Vector3.right * gridWorldSize.x / 2 - Vector3.forward * gridWorldSize.y / 2; //forward没错，y对应node的z坐标

        for (int x = 0; x < gridSizeX; x++)
        {
            for (int y = 0; y < gridSizeY; y++)
            {
                // 计算每一个node的世界坐标位置
                Vector3 worldPoint = worldButtomLeft +
                    Vector3.right * (x * nodeDiameter + nodeRadius) +
                    Vector3.forward * (y * nodeDiameter + nodeRadius);

                // 判断是否有obstacles，如果有就将node设置为unwalkable
                bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask));
                // 创建每个node实例并给成员赋值
                grid[x, y] = new Node(walkable, worldPoint, x, y);
            }
        }
    }

    // 获取节点的相邻节点
    public List GetNeighbours(Node node)
    {
        List neighbours = new List();

        for (int x = -1; x <= 1; x++)
        {
            for (int y = -1; y <= 1; y++)
            {
                if (x == 0 && y == 0)
                {                  
                    continue;// 这就是节点自己
                }

                int checkX = node.gridX + x;
                int checkY = node.gridY + y;

                // 结果不能超出Grid的范围
                if(checkX >= 0 && checkY < gridSizeY && checkX < gridSizeX && checkY >= 0)
                {
                    neighbours.Add(grid[checkX, checkY]);
                }
            }
        }

        return neighbours;
    }

    // 通过世界坐标获得Node
    public Node GetNodeFromWorldPoint(Vector3 worldPosition)
    {
        // 通过将坐标换算为Grid中的百分比位置来获取Node
        float percentX = (worldPosition.x + gridWorldSize.x / 2) / gridWorldSize.x;
        float percentY = (worldPosition.z + gridWorldSize.y / 2) / gridWorldSize.y; //grid的y长对应世界坐标系的z

        percentX = Mathf.Clamp01(percentX);
        percentY = Mathf.Clamp01(percentY);

        int x = Mathf.RoundToInt((gridSizeX - 1) * percentX); //减一是因为gridSize是1开始，我们需要index
        int y = Mathf.RoundToInt((gridSizeY - 1) * percentY);
        return grid[x, y];
    }

    // 在Scene面板中显示grid
    private void OnDrawGizmos()
    {
        Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));

        if (grid != null)
        {
            Node playerNode = GetNodeFromWorldPoint(player.position);
            foreach(Node node in grid)
            {
                Gizmos.color = node.walkable ? Color.white : Color.red;
                if(playerNode == node)
                {
                    Gizmos.color = Color.cyan;
                }
                if(path != null)
                {
                    if (path.Contains(node))
                        Gizmos.color = Color.green;
                }
                Gizmos.DrawCube(node.worldPosition, Vector3.one * (nodeDiameter - 0.1f));
            }
        }
    }
}

效果

再开始之前说一句，VS2022的人工智能代码提示真好用，快进到人工智能写代码淘汰我这种废物程序员。

继续咱们的A*算法，按照先前的算法思路，代码实现如下

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

public class Pathfinding : MonoBehaviour
{
    public Transform seeker, target;

    public Grid grid;

    private void Awake()
    {
        grid = this.GetComponent();
    }

    private void Update()
    {
        FindPath(seeker.position, target.position);
    }

    void FindPath(Vector3 startPos, Vector3 targetPos)
    {
        // 获取起点终点
        Node startNode = grid.GetNodeFromWorldPoint(startPos);
        Node targetNode = grid.GetNodeFromWorldPoint(targetPos);
        
        // Open列表 存放所有预选的节点
        List openSet = new List();
        HashSet closeSet = new HashSet();
        openSet.Add(startNode);

        while (openSet.Count > 0)
        {
            Node currentNode = openSet[0];
            for (int i = 1; i < openSet.Count; i++)
            {
                // 寻找一个比当前节点更优的节点 晚点再来做优化
                if(openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].gCost < currentNode.gCost)
                {
                    currentNode = openSet[i];
                }
            }

            openSet.Remove(currentNode);
            closeSet.Add(currentNode);
            
            // 碰到终点了
            if (currentNode == targetNode)
            {
                // 回溯节点以获取路径
                RetracePath(startNode, targetNode);
                return;
            }

            // 查看每个相邻节点
            foreach (Node neighbourNode in grid.GetNeighbours(currentNode))
            {
                // 如果相邻节点unwalkable或者已经在closeSet里面了 啥也不干
                if(!neighbourNode.walkable || closeSet.Contains(neighbourNode))
                {
                    continue;
                }
                // 计算从当前节点来看的neighbourNode的gCost
                int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbourNode);
                // 如果新的gCost更小 或者这是第一次考虑此neighbourNode
                if(newMovementCostToNeighbour < neighbourNode.gCost || !openSet.Contains(neighbourNode))
                {
                    // 更新此neighbourNode的Cost
                    neighbourNode.gCost = newMovementCostToNeighbour;
                    neighbourNode.hCost = GetDistance(neighbourNode, targetNode);
                    neighbourNode.parent = currentNode;

                    if(!openSet.Contains(neighbourNode))
                    {
                        openSet.Add(neighbourNode);
                    }
                }
            }
        }
    }

    void RetracePath(Node startNode, Node endNode)
    {
        List path = new List();
        Node currentNode = endNode;

        while(currentNode != startNode)
        {
            path.Add(currentNode);
            currentNode = currentNode.parent;
        }
        path.Reverse();

        grid.path = path;
    }

    int GetDistance(Node nodeA, Node nodeB)
    {
        int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
        int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);

        if(dstX > dstY)
        {
            return 14 * dstY + 10 * (dstX - dstY);
        }
        else
        {
            return 14 * dstX + 10 * (dstY - dstX);
        }
    }
}

效果：

在先前的代码中，我们采用遍历大法来寻找fcost更低的节点，现在我们实现一个效率更好的办法。

        while (openSet.Count > 0)
        {
            Node currentNode = openSet[0];
            for (int i = 1; i < openSet.Count; i++)
            {
                // 寻找一个比当前节点更优的节点 晚点再来做优化
                if(openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].gCost < currentNode.gCost)
                {
                    currentNode = openSet[i];
                }
            }

            openSet.Remove(currentNode);
// ... ...

我好像一直都没了解过堆（heap）这种数据结构，在Sebastian大佬的描述中，堆就是 二叉树（）。上网查了一下，堆是一种特殊的完全二叉树：二叉树和堆（理论）

现在，我们有一个节点fcost组成的二叉树，每个父节点都一定要比它的子节点小。

假如我们向其中插入一个新节点，比如图中右下角的5，显然现在5的位置不符合要求，如果出现这种情况，我们就将5和它的父节点10交换。

如果交换以后发现新的父节点依然大于它，则再次交换，直到到达所有父节点小于子节点的要求。这样一来，fcost最小的节点一定是堆顶部的节点。而在图示情况下我们只进行了三次比较而不是与全部其它14个节点做比较。

接下来我们把最小的那个节点拿走，取堆末尾的10放到最前。还是继续做交换，如果现在的父节点比两个子节点都小，就直接与最小的子节点交换。

继续换下去。直到再次符合要求。

观察上图的堆，可以发现，给定任意一个子节点索引为n，他的父节点索引为：(n-1)/2的计算机整数除法结果。比如节点12，减一再通过计算机的整数除法除以2会得到5，恰巧就是父节点。反过来，父节点n的两个子节点分别是n2+1和n2+2。
通过上述获得节点的办法，接下来我们写一个堆。

using System;
using System.Collections;
using UnityEngine;

public class Heap where T : IHeapItem
{
    T[] items;
    int currentItemCount;

    public Heap(int maxHeapSize) //考虑到我们用的数组不好调整大小 所以指定堆的大小
    {
        items = new T[maxHeapSize];
    }

    // 添加元素
    public void Add(T item)
    {
        item.HeapIndex = currentItemCount;
        items[currentItemCount] = item;
        SortUp(item);
        currentItemCount++;
    }

    // 移除并获取堆的第一个元素 第一个元素优先级总是最小的
    public T RemoveFirst()
    {
        T firstItem = items[0];
        currentItemCount--;
        items[0] = items[currentItemCount]; //取堆末尾的元素填到最前
        items[0].HeapIndex = 0;
        SortDown(items[0]);
        return firstItem;
    }

    // 更新元素在堆中的位置
    public void UpdateItem(T item)
    {
        // 这两个最多有一个有用 不存在冲突
        SortUp(item);
        SortDown(item);
    }

    public int Count
    {
        get { return currentItemCount; }
    }

    // 堆中是否包含item
    public bool Contains(T item)
    {
        return Equals(items[item.HeapIndex], item);
    }

    // 将父节点与其子节点做比较并交换
    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 (items[swapIndex].CompareTo(item) > 0)
                {
                    Swap(items[swapIndex], item); //子节点需要更靠前
                }
                else
                {
                    return; //item在当前位置符合堆的要求
                }
            }
            else
            {
                return; // 没有子节点
            }

        }
    }

    // 将子节点与其父节点做比较并交换
    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; //item在当前位置符合堆的要求
            }
            // 计算新的parent
            parentIndex = (item.HeapIndex - 1) / 2;
        }
    }

    void Swap(T itemA, T itemB)
    {
        items[itemA.HeapIndex] = itemB;
        items[itemB.HeapIndex] = itemA;

        int itemAIndex = itemA.HeapIndex;
        itemA.HeapIndex = itemB.HeapIndex; 
        itemB.HeapIndex = itemAIndex;
    }
}

public interface IHeapItem : IComparable
{
    int HeapIndex
    {
        get;
        set;
    }
}

更新Node代码以实现IHeapItem接口功能

public class Node : IHeapItem
{
	// ... ...
	
    // IHeapItem接口实现
    int heapIndex;

    public int HeapIndex
    {
        get { return heapIndex; }
        set { heapIndex = value; }
    }

    // HeapItem CompareTo实现
    public int CompareTo(Node nodeToCompare)
    {
        int compare = fCost.CompareTo(nodeToCompare.fCost);
        if (compare == 0) // fCost相等 则比较hCost
        {
            compare = hCost.CompareTo(nodeToCompare.hCost);
        }
        return -compare; //fCost/hCost更小的优先级更大
    }
}

将算法中的openSet改为Heap。这样以后，之前的for循环获取最小fCost的Node就变成了调用Heap的RemoveFirst。

        Heap openSet = new Heap(grid.MaxSize);
       // ... ...
       
   		while (openSet.Count > 0)
        {
            Node currentNode = openSet.RemoveFirst();
            closeSet.Add(currentNode);
           
			// ... ...

值得一提的是，我们使用了接口来规范能够放在堆里的类型。Unity官方提供了接口教学：接口 - Unity Learn

能找到路径，我们就可以实现能沿着路径运动的单元。但如果我们同时有大量单元需要寻路，会导致程序出现明显卡顿，为此我们需要将大量单元的寻路从一帧全算完变成一系列的请求，将原来一帧的计算分散到多帧。我们将使用任务队列和协程来完成上述功能。

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

public class PathRequestManager : MonoBehaviour
{
    Queue pathRequestQueue = new Queue();
    PathRequest currentPathRequest;

    static PathRequestManager instance;
    Pathfinding pathfinding;

    bool isProcessingPath;

    private void Awake()
    {
        instance = this;
        pathfinding = GetComponent();
    }

    public static void RequestPath(Vector3 pathStart, Vector3 pathEnd, Action callback)
    {
        PathRequest newRequest = new PathRequest(pathStart, pathEnd, callback);
        instance.pathRequestQueue.Enqueue(newRequest);
        instance.TryProcessNext();
    }

    void TryProcessNext()
    {
        if(!isProcessingPath && pathRequestQueue.Count > 0)
        {
            currentPathRequest = pathRequestQueue.Dequeue();
            isProcessingPath = true;
            pathfinding.StartFindPath(currentPathRequest.pathStart, currentPathRequest.pathEnd);
        }
    }

    public void FinishedProcessingPath(Vector3[] path, bool success)
    {
        currentPathRequest.callback(path, success);
        isProcessingPath = false;
        TryProcessNext();
    }

    struct PathRequest
    {
        public Vector3 pathStart;
        public Vector3 pathEnd;
        public Action callback;

        public PathRequest(Vector3 _start, Vector3 _end, Action _callback)
        {
            pathStart = _start;
            pathEnd = _end;
            callback = _callback;
        }
    }
}

上述是路径请求管理器，主要任务是管理pathRequestQueue，每个pathRequestQueue 调用TryProcessNext来给一个个PathRequest调用A*算法。

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

public class Unit : MonoBehaviour
{
    public Transform target;
    public float speed = 1;
    Vector3[] path;
    int targetIndex;

    void Start()
    {
        PathRequestManager.RequestPath(transform.position, target.position, OnPathFound);
    }

    public void OnPathFound(Vector3[] newPath, bool pathSuccessful)
    {
        if(pathSuccessful)
        {
            path = newPath;
            StopCoroutine("FollowPath");
            StartCoroutine("FollowPath");
        }
    }

    IEnumerator FollowPath()
    {
        Vector3 currentWaypoint = path[0];
        while (true)
        {
            if(transform.position == currentWaypoint)
            {
                targetIndex++;
                if(targetIndex >= path.Length)
                {
                    yield break;
                }

                currentWaypoint = path[targetIndex];
            }
            
            transform.position = Vector3.MoveTowards(transform.position, currentWaypoint, speed * Time.deltaTime);
            yield return null;
        }
    }

    private void OnDrawGizmos()
    {
        if(path != null)
        {
            for (int i = targetIndex; i < path.Length; i++)
            {
                Gizmos.color = Color.black;
                Gizmos.DrawCube(path[i], Vector3.one);

                if(i == targetIndex)
                {
                    Gizmos.DrawLine(transform.position, path[i]);
                }
                else
                {
                    Gizmos.DrawLine(path[i-1], path[i]);
                }
            }
        }
    }
}

RequestPath由运动单元调用，现在每个运动单元会在开始时发出寻路请求，通过Manager的FinishedProcessingPath通知其路径已处理，开始沿路径运动。
为此，Pathfinding也需要做出一些修改。

// Pathfinding.cs ...

    public void StartFindPath(Vector3 pathStart, Vector3 targetPos)
    {
        StartCoroutine(FindPath(pathStart, targetPos));
    }

    IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
    {
        // 接收寻路的
        Vector3[] waypoints = new Vector3[0];
        // 寻路是否成功
        bool pathSuccess = false;

        // 获取起点终点
        Node startNode = grid.GetNodeFromWorldPoint(startPos);
        Node targetNode = grid.GetNodeFromWorldPoint(targetPos);
        
        // 起点和目标点都能走时我们才进行计算
        if(startNode.walkable && targetNode.walkable)
        {
            // Open列表 存放所有预选的节点
            Heap openSet = new Heap(grid.MaxSize);
            HashSet closeSet = new HashSet();
            openSet.Add(startNode);

            while (openSet.Count > 0)
            {
                Node currentNode = openSet.RemoveFirst();
                closeSet.Add(currentNode);

                // 碰到终点了
                if (currentNode == targetNode)
                {
                    pathSuccess = true;
                    break;
                }

                // 查看每个相邻节点
                foreach (Node neighbourNode in grid.GetNeighbours(currentNode))
                {
                    // 如果相邻节点unwalkable或者已经在closeSet里面了 啥也不干
                    if (!neighbourNode.walkable || closeSet.Contains(neighbourNode))
                    {
                        continue;
                    }
                    // 计算从当前节点来看的neighbourNode的gCost
                    int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbourNode);
                    // 如果新的gCost更小 或者这是第一次考虑此neighbourNode
                    if (newMovementCostToNeighbour < neighbourNode.gCost || !openSet.Contains(neighbourNode))
                    {
                        // 更新此neighbourNode的Cost
                        neighbourNode.gCost = newMovementCostToNeighbour;
                        neighbourNode.hCost = GetDistance(neighbourNode, targetNode);
                        neighbourNode.parent = currentNode;

                        if (!openSet.Contains(neighbourNode))
                        {
                            openSet.Add(neighbourNode);
                        }
                    }
                }
            }
        }
       
        yield return null;
        if(pathSuccess)
        {
            // 回溯节点以获取路径
            waypoints = RetracePath(startNode, targetNode);
        }
        requestManager.FinishedProcessingPath(waypoints, pathSuccess);
    }

    Vector3[] RetracePath(Node startNode, Node endNode)
    {
        List path = new List();
        Node currentNode = endNode;

        while(currentNode != startNode)
        {
            path.Add(currentNode);
            currentNode = currentNode.parent;
        }
        Vector3[] waypoints = SimplifyPath(path);
        Array.Reverse(waypoints);
        return waypoints;
    }

    Vector3[] SimplifyPath(List path)
    {
        List waypoints = new List();
        Vector2 directionOld = Vector2.zero;

        for (int i = 1; i < path.Count; i++)
        {
            // 如果一系列路径节点在一个方向上，则取最终的那个节点
            Vector2 directionNew = new Vector2(path[i - 1].gridX - path[i].gridX, path[i - 1].gridY - path[i].gridY);
            if (directionNew != directionOld)
            {
                waypoints.Add(path[i-1].worldPosition);
            }
            directionOld = directionNew;
        }

        return waypoints.ToArray();
    }

比较好玩的是，我们没有直接将A*算法得到的路径交给运动单元，而是用SimplifyPath简化了一下，将一个方向上的节点优化掉了。
展示一下效果。

假设要表现这样一种情况，AI在大马路上运动最快最轻松，在草地上一般，在泥泞中很难移动，但是也不是不能移动。为了在程序中体现这种情况，我们将运用权重来定义网格的通过难易程度。
塞巴大佬使用的方式为，定义节点的移动惩罚(Penalty)，在寻路时加入cost计算中。

public class Node : IHeapItem
{
    // ... ...
    
    // 节点移动权重
    public int movementPenalty;
    
    // ... ...

还要定义一个类来表达不同的地形，定义权重值。

[System.Serializable]
public class TerrainType
{
    public LayerMask terrainMask;
    public int terrainPenalty;
}

public class Grid : MonoBehaviour
{
    // ... ...
    // 地形类别 寻路的移动花销相关
    public TerrainType[] walkableRegionType;

最后，完成一些构建工作。

public class Grid : MonoBehaviour
{
    // ... ...

    // 所有能够行走的layer
    LayerMask walkableMask;
    // 方便节点构造时判断自身类型使用的字典
    Dictionary walkableRegionDictionary = new Dictionary();


    private void Awake()
    {
        // ... ...
        foreach (TerrainType region in walkableRegionType)
        {
            walkableMask.value = walkableMask | region.terrainMask.value;
            walkableRegionDictionary.Add((int)Mathf.Log(region.terrainMask.value, 2), region.terrainPenalty);
        }

        CreateGrid();
    }

    // 创建grid实例
    private void CreateGrid()
    {
        // ... ...

        for (int x = 0; x < gridSizeX; x++)
        {
            for (int y = 0; y < gridSizeY; y++)
            {
                // 计算每一个node的世界坐标位置
                Vector3 worldPoint = worldButtomLeft +
                    Vector3.right * (x * nodeDiameter + nodeRadius) +
                    Vector3.forward * (y * nodeDiameter + nodeRadius);

                // 判断是否有obstacles，如果有就将node设置为unwalkable
                bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask));
                // 设置节点移动权重
                int movementPenalty = 0;
                if (walkable)
                {
                    Ray ray = new Ray(worldPoint + Vector3.up * 50, Vector3.down);
                    RaycastHit hit;
                    if(Physics.Raycast(ray, out hit))
                    {
                        walkableRegionDictionary.TryGetValue(hit.collider.gameObject.layer, out movementPenalty);
                    }
                }

                // 创建每个node实例并给成员赋值
                grid[x, y] = new Node(walkable, worldPoint, x, y, movementPenalty);
            }
        }
    }

效果展示。

上期制作的权重，需要的功能倒是实现了，就是会导致得到的路径总是沿着不同区域的边缘，十分难看。我们将使权重更加平滑来解决这个问题。

要使得权重更平滑，我们将使用一种模糊(blur)算法。假如我们有如下网格，该模糊算法将每个节点的值重新计算为周围值加上自身后的平均值。

要这样做，我们可以将每个节点周围的值加起来，放到新网格中。
对于边缘情况，可以考虑与边缘的值一致（或者说与“旁边”的值一致）。这样在我们的问题中每个节点加上周围的值都只用除以9。省的考虑每个节点周围具体有多少节点。


当然，大佬提到这样直接相加的效率不佳，于是提到了一种新的计算方法。首先水平方向上遍历每个节点，将其和左右两相邻节点相加。

更骚的是，下一个遍历到的节点的和，是上一个的和，减去上一个的左边，加上当前的右边，这样我们甚至省下了看当前节点的值这一操作。。。

计算完水平的，将得到的值保存下来，然后再竖直方向再遍历一次，将节点和上下两个相邻相加。

采用这种水平竖直的计算方式，得到的是和直接将周围节点相加一样的结果。我们可以很简单的看出：
直接相加周围的计算方式我们遍历了：5 * 5 * 9 + 25 = 250次
采用水平竖直方法我们遍历了：5 * 5 * 2 * 2 + 10 = 110次，牛哇！

接下来要在代码中实现上述模糊算法。

// 创建grid实例
    private void CreateGrid()
    {
        // ... ...
		
		// 网格生成完成以后 我们再进行节点权重模糊
        BlurPenaltyMap(2);
    }

    // 使网格的权重图更平滑
    void BlurPenaltyMap(int blurSize) // blurSize定义我们想要一个节点和多大的面积计算模糊，1就意味着和周围一圈，2就是周围两圈
    {
        // kernelSize就是一个节点的模糊计算涉及多大范围 比如3 X 3范围
        int kernelSize = blurSize * 2 + 1;
        // kernelExtents是只一个kernal的中心到边缘之间有几个节点 比如3 X 3的kernelSize有一个
        int kernelExtents = (kernelSize - 1) / 2;

        // 用于保存水平和竖直的计算结果
        int[,] penaltiesHorizontalPass = new int[gridSizeX, gridSizeY];
        int[,] penaltiesVerticalPass = new int[gridSizeX, gridSizeY];

        // 水平方向的遍历
        for (int y = 0; y < gridSizeY; y++)
        {
            for (int x = -kernelExtents; x <= kernelExtents; x++)
            {
                int sampleX = Mathf.Clamp(x, 0, kernelExtents);
                penaltiesHorizontalPass[0 ,y] += grid[sampleX, y].movementPenalty;
            }

            for (int x = 1; x < gridSizeX; x++)
            {
                int removeIndex = Mathf.Clamp(x - kernelExtents - 1, 0, gridSizeX);
                int addIndex = Mathf.Clamp(x + kernelExtents, 0, gridSizeX - 1);

                penaltiesHorizontalPass[x,y] = penaltiesHorizontalPass[x - 1, y] - grid[removeIndex, y].movementPenalty + grid[addIndex, y].movementPenalty;
            }
        }

        // 竖直方向的遍历
        for (int x = 0; x < gridSizeX; x++)
        {
            for (int y = -kernelExtents; y <= kernelExtents; y++)
            {
                int sampleY = Mathf.Clamp(y, 0, kernelExtents);
                penaltiesVerticalPass[x, 0] += penaltiesHorizontalPass[x, sampleY];
            }

            // 将y = 0时得到的blur penalty赋值给grid
            int blurredPenalty = Mathf.RoundToInt((float)penaltiesVerticalPass[x, 0] / (kernelSize * kernelSize));
            grid[x, 0].movementPenalty = blurredPenalty;

            for (int y = 1; y < gridSizeY; y++)
            {
                int removeIndex = Mathf.Clamp(y - kernelExtents - 1, 0, gridSizeY);
                int addIndex = Mathf.Clamp(y + kernelExtents, 0, gridSizeY - 1);

                penaltiesVerticalPass[x, y] = penaltiesVerticalPass[x, y - 1] - penaltiesHorizontalPass[x, removeIndex] + penaltiesHorizontalPass[x, addIndex];
                // 得到的结果 取个整数近似值
                blurredPenalty = Mathf.RoundToInt((float)penaltiesVerticalPass[x, y] / (kernelSize * kernelSize));
                grid[x,y].movementPenalty = blurredPenalty;

                if(blurredPenalty > penaltyMax)
                    penaltyMax = blurredPenalty;
                if(blurredPenalty < penaltyMin)
                    penaltyMin = blurredPenalty;
            }
        }
    }

效果展示。至少不贴边了，看着不错。

权重可视化的效果。