游戏AI寻路——八叉树+A*寻路

利用八叉树的空中寻路

你有思考过在空中如何进行寻“路”吗？来想象一个的场景：飞机从空中基地出发，要避开许多空中建筑，最终到达目的地。这种情况下的寻路是没有路面的，寻路物体的移动方向也比较自由，这该怎么寻呢？

如果我们只是在一个平面进行寻路，我们可以直接用A*寻路，铺好一个地面网格，这样就可以在网格点上设置目标点来寻路了。假设我们要在一个 \(500\times500\) 大小的网格寻路，就算一个单位设置一个网格点，那就要 \(500 \times 500 = 25,0000\) 这么多个点，不过倒也是不能接受。

现在我们算上“领空”，就算取100得到的数值 \(500 \times 500 \times 100\) 也是挺大的……有办法减少结点又保证网格连接合理吗？如果解决这两个问题，也不是不能继续使用A*寻路。

欸，这就可以通过八叉树来实现！

注意：文中代码部分有些地方会用省略号，表示「对应部分内容与之前一样，不需要修改」，是为了突出重点内容。如需要完整代码，文末会给出。

寻路中八叉树的作用

利用八叉树的寻路，并不是说要用八叉树做一个像A*那样的寻路算法，而是利用它来生成寻路区域。可以认为它是另一种寻路网格，八叉树最终生成的会比之前我们想的那种笨方法的结点更少，在八叉树生成的网格里我们依然可以使用原本的寻路算法。

PS：八叉树还有其它的正经工作，比如碰撞检测，对引擎开发感兴趣的同学也可以去了解一下。

生成寻路网格

1. 八叉树结点

现在就要看看如何用八叉树来生成寻路结点了。先说说八叉树吧，八叉树本身并不复杂，它说的是这么一个结构：

所谓“一尺之棰，日取其半，万世不竭”，难道要一直分下去吗？我们可以给它设置一个最小尺寸来限制，只有当前方块尺寸比最小尺寸大时才分裂，至此，我们可以初步构建八叉树结点：

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

public class MyOctreeNode
{
    private const float MIN_CUBE_SIZE = 1f; // 最小方格尺寸
    public MyOctreeNode Parent{ get; set; } //父结点
    public MyOctreeNode[] Children; //子结点
    public Bounds NodeCube; //用包围盒作为结点方块，方便后续检测

    public MyOctreeNode(Bounds nodeCube, MyOctreeNode parent)
    {
        Parent = parent;
        NodeCube = nodeCube;
    }
    public void Divide()
    {
        //因为是正方体，所以用一条边来判断尺寸即可
        if(NodeCube.size.x >= MIN_CUBE_SIZE)
        {
            // 子方块的半尺寸， 用半尺寸是因为构建Bounds需要
            float childHalfSize = NodeCube.size.x / 4;
            if (Children == null)
                Children = new MyOctreeNode[8];
            Vector3 offset; //子结点偏移
            for(int i = 0; i < 8; ++i)
            {
                //待补充
                var childBounds = new Bounds();
                //

                if(Children[i] == null)
                    Children[i] = new MyOctreeNode(childBounds, this);
                Children[i].Divide(); // 每个子结点继续分裂
            }
        }
    }
}

子结点的方块该怎么布置呢？简单分析下位置关系就可以看出来：

每个子方块对于原本方块中心的各轴的偏移量都是原本边长的 \(\frac{1}{4}\)，无非是 \(+\frac{1}{4}、-\frac{1}{4}\) 的差别。但好在，我们不关心子结点的顺序（也就是说在数组中这八个方块谁先谁后都无所谓），那么这8种正负号的组合方案可以通过对0~7的数取二进制的3个位来得到（下图0 ~ 7是乱序的，只是为了对照）：

当然，如果你觉得不够直观，也可以用数组记录这8个情况再遍历赋值，这里就只是图省个数组而已。那就用上述方法完善一下Divide方法：

public void Divide()
{
    //因为是正方体，所以用一条边来判断尺寸即可
    if(NodeCube.size.x >= MIN_CUBE_SIZE)
    {
        // 子方块的半尺寸， 用半尺寸是因为构建Bounds需要
        float childHalfSize = NodeCube.size.x / 4;
        if (Children == null)
            Children = new MyOctreeNode[8];
        Vector3 offset; //子节点偏移
        for(int i = 0; i < 8; ++i)
        {
            //0~7的二进制位结构恰好满足我们所需要的组合形式
            offset.x = (1 & i) != 0 ? childHalfSize : -childHalfSize; //取二进制第0位
            offset.y = (2 & i) != 0 ? childHalfSize : -childHalfSize; //取二进制第1位
            offset.z = (4 & i) != 0 ? childHalfSize : -childHalfSize; //取二进制第2位
            var childBounds = new Bounds(NodeCube.center + offset, 2 * childHalfSize * Vector3.one);

            if(Children[i] == null)
                Children[i] = new MyOctreeNode(childBounds, this);
            Children[i].Divide(); // 每个子节点继续分裂
        }
    }
}

为了方便观察结果，再在类中添个用于绘制方块的函数，当它在OnDrawGizmos中调用时就可以看到方块了：

//isSeeOne为true，则只查看分裂后的一个，否则查看所有分裂后的方块
public void Draw(bool isSeeOne)
{
    Gizmos.color = Color.green;
    Gizmos.DrawWireCube(NodeCube.center, NodeCube.size);
    if (Children == null)
        return;
    foreach(var c in Children)
    {
        c.Draw(isSeeOne);
        if(isSeeOne)
        {
            break;
        }
    }
}

为了方便在Unity中使用，我们创建一个继承了MonoBehaviour的类MyOctreeBuilder，并将它挂在一个边长为8的Cube上：

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

public class MyOctreeBuilder : MonoBehaviour
{
    public bool isSeeOne = true; //只看分裂后的一个
    private MyOctreeNode node;
    private void Awake()
    {
        //用Cube本身的包围盒做为起始尺寸进行划分
        node = new MyOctreeNode(GetComponent<Renderer>().bounds, node);
        node.Divide();
    }

    private void OnDrawGizmos()
    {
        if (Application.isPlaying)
        {
            node.Draw(isSeeOne);
        }
    }
}

我们设置的最小尺寸为1，从8减半到1，一共要3次，划分出的方块数符合预期。

2. 根结点

那要如何设置包围盒才能让它刚好能包围我的场景呢，总不能拿Cube去自己试吧？欸，好在Unity的Bounds类有个可以帮助我们的方法：

Encapsulate方法可以让包围盒自行扩大以容纳下传进来的包围盒。所以我们让一个包围盒把场景中的所有物体都容纳进去，这样就能得到足够大的包围盒了。我们新建一个MyOctree类：

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

public class MyOctree
{
    public MyOctreeNode RootNode;
    public MyOctree(GameObject[] allObjects)
    {
        var baseCube = new Bounds();
        foreach(var o in allObjects)
        {
            baseCube.Encapsulate(o.GetComponent<Collider>().bounds);
        }
        //选取最长的一条边来作为正方体的边长，并将包围盒改成正方体
        //这里为了更好设置包围盒，同样记录半尺寸
        var cubeHalfSize = 0.5f * Mathf.Max(baseCube.size.x, baseCube.size.y, baseCube.size.z) * Vector3.one;
        baseCube.SetMinMax(baseCube.center - cubeHalfSize, baseCube.center + cubeHalfSize);

        RootNode = new MyOctreeNode(baseCube, null);
        RootNode.Divide();
    }
}

顺便也改改MyOctreeBuilder脚本，让它画出八叉树，而不是单一节点：

public class MyOctreeBuilder : MonoBehaviour
{
    public GameObject[] objects; //需要包含的物体
    public bool isSeeOne = true; //只看分裂后的一个
    private MyOctree myOctree; //八叉树

    private void Awake()
    {
        myOctree = new MyOctree(objects);
    }

    private void OnDrawGizmos()
    {
        if (Application.isPlaying)
        {
            myOctree.RootNode.Draw(isSeeOne);
        }
    }
}

随便在场景里摆了几个立方体，最终生成的最大包围盒能将它们都裹住：

至此，准备工作完成。

3. 剔除不必要的结点（关键）

仅是不断分裂生成小方块，那最终不还是和我们开头的笨方法一样吗（会有一堆密密麻麻的点）？我们可以注意到，其实有一些方块没必要继续分裂下去。分裂行为其实是有目的的：检测出哪里有障碍物。一个大方块不断分裂变小，就是更进一步定位内部障碍物位置的过程，如果它一开始就没碰到什么障碍物，那也没必要分裂了。

我们需要对先前几个类中的内容稍加修改：

1. MyOctreeNode类的Divide方法中分裂前要进行一些条件判断：

   public void Divide(Collider collider)
   {
       //因为是正方体，所以用一条边来判断尺寸即可
       if(NodeCube.size.x >= MIN_CUBE_SIZE)
       {
           // 子方块的半尺寸， 用半尺寸是因为构建Bounds需要
           float childHalfSize = NodeCube.size.x / 4;
           if (Children == null)
               Children = new MyOctreeNode[8];
           Vector3 offset; //子节点偏移
           for(int i = 0; i < 8; ++i)
           {
               //0~7的二进制位结构恰好满足我们所需要的组合形式
               offset.x = (1 & i) != 0 ? childHalfSize : -childHalfSize;
               offset.y = (2 & i) != 0 ? childHalfSize : -childHalfSize;
               offset.z = (4 & i) != 0 ? childHalfSize : -childHalfSize;
               var childBounds = new Bounds(NodeCube.center + offset, 2 * childHalfSize * Vector3.one);
               if(Children[i] == null)
                   Children[i] = new MyOctreeNode(childBounds, this);
               /*
               进一步分裂前，先判断一下有没有遇到障碍物，没有就不要继续分裂了;
               也可以再附带添加些其它检测条件，比如obj.layer等
               */
               if(childBounds.Intersects(collider.bounds))
               {
                   Children[i].Divide(collider); // 每个子节点继续分裂
               }
           }
       }
   }
   

2. MyOctree类初始化时具体分裂：

   public MyOctree(GameObject[] allObjects)
   {
       var baseCube = new Bounds();
       foreach(var o in allObjects)
       {
           baseCube.Encapsulate(o.GetComponent<Collider>().bounds);
       }
       //选取最长的一条边来作为正方体的边长，并将包围盒改成正方体
       //这里为了更好设置包围盒，同样记录半尺寸
       var cubeHalfSize = 0.5f * Mathf.Max(baseCube.size.x, baseCube.size.y, baseCube.size.z) * Vector3.one;
       baseCube.SetMinMax(baseCube.center - cubeHalfSize, baseCube.center + cubeHalfSize);
   
       RootNode = new MyOctreeNode(baseCube, null);
       foreach(var o in allObjects) //具体分裂
       {
           //有碰撞体的物体才有检测的必要
           if(o.TryGetComponent(out Collider collider))
           {
               RootNode.Divide(collider);
           }
       }
   }
   

我们是在进一步分裂前制止分裂的，算是一种前剪枝策略；相对的，如果是在所有结点都生成好后，再删掉不必要的结点，那就是后剪枝策略。考虑到场景可能会很大，前剪枝的策略明显会更好些。

让我们看看修改后的效果：

八叉树替代网格的关键就在于此：存在障碍物的地方才会有密集的结点，空旷的地方倒没什么结点。

这其实很符合人的自然智慧：首先我们要明白结点多意味着什么，这其实意味着能更精细的寻路。在有障碍物的地方，我们就得小心避障、“步步为营”，所以需要更多结点细化落脚点；而空旷的地方就不用这样绕来绕去，直接“两点一线”就够了。

4. 连接成网

结点已经全部划分出来了，那如何连接成网格呢？我们可以自然而然想到两种做法（当然，可以有其它做法）：

1. 父子相连：每个结点都与它的父结点和子结点相连（如果有的话）
2. 全连接：每个结点和其它结点依次相连

这两种朴素的做法其实已经反映出了连接需要考虑的问题：

首先，这两种做法都可以算是对的。它们都能保证整个网络是连通的，也就说，在这两种方法构建的网格下，我们总可以找到路径从一个结点到另外一个结点。

对于第一种做法，它连接成网所构建的边的数量明显比第二种来得少。但很显然，由于边的数量过少，实际的路径选择也会很少，即使是去不同的地方，走出的路径也是大同小异，并且还会出现绕远路的情况。

而第二种就是另一个极端了，它所构建的网格连通性极高，两点之间通常都含有着丰富的路径选择，但需要存储的边实在是太多了。

哪种方案更好？这是根据实际情况调整的。

这里我们采用一种第二种策略，但有一点要注意：我们应该把全是障碍物的结点排除掉，因为它们所在的位置已经没有行走的余地了。

现在就来实现一下：

1. 我们准备一个枚举，来区分结点的类型（也方便后续拓展），暂时就分两类结点：通常、障碍（针对最小障碍）。并在分裂过程中判别哪些是障碍。根据我们的分裂逻辑，可以清楚地想到：只要仍需分裂的最小结点才是最小障碍：

   public enum NodeType
   {
       Normal, Obstacles,
   }
   public class MyOctreeNode
   {
       //……
       public Bounds NodeCube; //用包围盒作为结点方块，方便后续检测
       public NodeType Type = NodeType.Normal;
       //……
   
       public void Divide(Collider collider)
       {
           //因为是正方体，所以用一条边来判断尺寸即可
           if(NodeCube.size.x >= MIN_CUBE_SIZE)
           {
               //……
           }
           else
           {
               Type = NodeType.Obstacles;
           }
       }
       //isSeeOne为true，则只查看分裂后的一个，否则查看所有分裂后的方块
       public void Draw(bool isSeeOne)
       {
           var drawColor = Color.green;
           if(Type == NodeType.Obstacles)
               drawColor = Color.red;
           Gizmos.color = drawColor;
           //……
       }
   }
   

   可以清楚的看到排除的结点：

   

2. 显然，我们需要用到图结构。由于本文的重点是在八叉树上，所以就不赘述图的实现了，作为一种基础的数据结构，我希望你能够自己实现。当然，实在没有的话，这里也提供一份作为参考吧(⊙ˍ⊙)：

   using System.Collections.Generic;
   
   public class MyGraph<TNode, TEdge>
   {
   	public readonly HashSet<TNode> NodeSet;//结点列表
   	public readonly Dictionary<TNode, List<TNode>> NeighborList;//邻居列表
   	public readonly Dictionary<(TNode, TNode), List<TEdge>> EdgeList;//边列表
   
       public MyGraph()
   	{
   		NodeSet = new HashSet<TNode>();
   		NeighborList = new Dictionary<TNode, List<TNode>>();
   		EdgeList = new Dictionary<(TNode, TNode), List<TEdge>>();
       }
   
   	/// <summary>
   	/// 寻找指定结点
   	/// </summary>
   	/// <returns>找到的结点，没找到时返回null</returns>
   	public TNode FindNode(TNode node)
   	{
   		NodeSet.TryGetValue(node, out TNode res);
   		return res;
   	}
   
   	/// <summary>
   	/// 寻找指点起、终点之间直接连接的所有边
   	/// </summary>
   	/// <param name="source">起点</param>
   	/// <param name="target">终点</param>
   	/// <returns>找到的边，没找到时返回null</returns>
   	public List<TEdge> FindEdge(TNode source, TNode target)
   	{
   		var s = FindNode(source);
   		var t = FindNode(target);
   		if (s != null && t != null)
   		{
   			var nodePairs = (s, t);
   			if (!EdgeList.ContainsKey(nodePairs))
   			{
   				return EdgeList[nodePairs];
   			}
   		}
   		return null;
   	}
   
   	/// <summary>
   	/// 添加结点，用HashSet，包含重复检测
   	/// </summary>
   	public bool AddNode(TNode node)
   	{
   		return NodeSet.Add(node);
   	}
   
   	/// <summary>
   	/// 添加指定边，含空结点判断、重复添加判断
   	/// </summary>
   	/// <param name="source">边起点</param>
   	/// <param name="target">边终点</param>
   	/// <param name="edge">指定边</param>
   	/// <returns>添加成功与否</returns>
   	public bool AddEdge(TNode source, TNode target, TEdge edge)
   	{
   		var s = FindNode(source);
   		var t = FindNode(target);
   		if (s == null || t == null)
   			return false;
   		var nodePairs = (s, t);
   		if(!EdgeList.ContainsKey(nodePairs))
   		{
   			EdgeList.Add(nodePairs, new List<TEdge>());
   		}
   		var allEdges = EdgeList[nodePairs];
   		if(!allEdges.Contains(edge))
   		{
   			allEdges.Add(edge);
   			if(!NeighborList.ContainsKey(source))
   			{
   				NeighborList.Add(source, new List<TNode>());
   			}
   			NeighborList[source].Add(target);
   			return true;
   		}
   		return false;
       }
   
   	/// <summary>
   	/// 移除指定结点
   	/// </summary>
   	/// <returns>移除成功与否</returns>
   	public bool RemoveNode(TNode node)
   	{
   		return NodeSet.Remove(node);
   	}
   
   	/// <summary>
   	/// 移除指定起、终点的指定边
   	/// </summary>
   	/// <param name="source">边起点</param>
   	/// <param name="target">边终点</param>
   	/// <param name="edge">指定边</param>
   	/// <returns>移除成功与否</returns>
   	public bool RemoveEdge(TNode source, TNode target, TEdge edge)
   	{
   		var allEdges = FindEdge(source, target);
   		return allEdges != null && allEdges.Remove(edge);
   	}
   
   	/// <summary>
   	/// 移除指定起、终点的所有边
   	/// </summary>
   	/// <param name="source">边起点</param>
   	/// <param name="target">边终点</param>
   	/// <returns>移除成功与否</returns>
   	public bool RemoveEdgeList(TNode source, TNode target)
   	{
   		return EdgeList.Remove((source, target));
   	}
   
   	/// <summary>
   	/// 获取指定结点可抵达的所有邻居结点
   	/// </summary>
   	public List<TNode> GetNeighbor(TNode node)
   	{
   		return NeighborList[node];
   	}
   
   	/// <summary>
   	/// 获取指定结点所延伸出的所有边
   	/// </summary>
   	public List<TEdge> GetConnectedEdge(TNode node)
   	{
   		var resEdge = new List<TEdge>();
   		var neighbor = GetNeighbor(node);
   		for(int i = 0; i < neighbor.Count; ++i)
   		{
   			var curEdgeList = EdgeList[(node, neighbor[i])];
   			for(int j = 0; j < curEdgeList.Count; ++j)
   			{
   				resEdge.Add(curEdgeList[j]);
   			}
   		}
   		return resEdge;
   	}
   }
   

   接下来就是让结点入图，我们在MyOctree类中声明一个图，并将树中所有正常结点都传入图中，这里也修改下构造函数，让图从外部传入（因为最终我们想要只操作MyOctreeBuilder脚本就能实现八叉树构建，所以把这些工作留给MyOctreeBuilder）：

   public class MyOctree
   {
   	public MyOctreeNode RootNode;
   	public MyGraph<MyOctreeNode, int> NavGraph; //寻路网格
   	public MyOctree(GameObject[] allObjects)
   	{
   		var baseCube = new Bounds();
   		NavGraph = new MyGraph<MyOctreeNode, int>();
   
           //……
   
   		NodeToGraph(RootNode);
   	}
   
   	//将树中的所有有效结点入图
   	private void NodeToGraph(MyOctreeNode node)
   	{
   		if (node == null) return;
   		// 没有子节点且为非障碍的结点才能入图
   		if(node.Children == null && node.Type != NodeType.Obstacles)
   		{
   			NavGraph.AddNode(node);
   		}
   		if(node.Children != null)
   		{
   			foreach(var c in node.Children)
   			{
   				NodeToGraph(c);
   			}
   		}
   	}
   }
   

   • (node.Children == null && node.Type != NodeType.Obstacles) 条件能剔除所有障碍结点？
     
     是可以做的，我们来看看下面几种情况：
     1. 有子节点的障碍方块。以下图绿色十字星的 \(4\times4\) 方形为例，它会被剔除
        
     2. 无子节点的障碍方块。仍是以绿色十字星标记的方形为例，它也会被剔除
        

   大改一下MyOctreeBuilder的内容，让它能绘制图也能绘制树，并根据功能开关绘制的内容：

   public class MyOctreeBuilder : MonoBehaviour
   {
   	public GameObject[] Objects; //场景包含的全部对象
   	public MyOctree Octree; // 八叉树
   	[SerializeField] private bool isSeeOne = false; //是否只观察一个分裂后的节点
   	[SerializeField] private bool isDrawOctreeCube = true; //是否绘制二叉树
   	[SerializeField] private bool isDrawNode = true; // 是否要绘制图的节点
   	[SerializeField] private bool isDrawEdge = true; // 是否要绘制图的边
   
   	private void Awake()
   	{
   		Octree = new MyOctree(Objects, new MyGraph<MyOctreeNode, int>());
   	}
   
   	private void OnDrawGizmos()
   	{
   		if(Application.isPlaying)
   		{
   			if(isDrawOctreeCube)
   			{
   				Octree.RootNode.Draw(isSeeOne);
   			}
   			DrawGraph();
   		}
   	}
   
       private void DrawGraph()
   	{
   		if(isDrawEdge)
   		{
   			foreach(var edge in Octree.NavGraph.EdgeList)
   			{
   				Gizmos.color = Color.red;
   				Gizmos.DrawLine(edge.Key.Item1.NodeCube.center,
   				edge.Key.Item2.NodeCube.center);
   			}
   		}
   		if(isDrawNode)
   		{
   			foreach(var node in Octree.NavGraph.NodeSet)
   			{
   				Gizmos.color = new Color(1, 1, 0);
   				Gizmos.DrawWireSphere(node.NodeCube.center, 0.25f);
   			}
   		}
   	}
   }
   

   可以看到，障碍物内部是没有结点的，障碍结点都被剔除了：

   

   最后，就将这些结点连接起来吧：

   public class MyOctree
   {
   	public MyOctreeNode RootNode;
   	public MyGraph<MyOctreeNode, int> NavGraph; //寻路网格图
   	public MyOctree(GameObject[] allObjects)
   	{
           //……
   
   		NodeToGraph(RootNode);
   		GenerateEdges();
   	}
   
       //……
   
   	//生成边
   	private void GenerateEdges()
   	{
   		foreach(var f in NavGraph.NodeSet)
   		{
   			foreach(var t in NavGraph.NodeSet)
   			{
   				if (f == t)
   					continue;
   				var ray = new Ray(f.NodeCube.center, t.NodeCube.center - f.NodeCube.center);
   				// 限制全连接范围
   				var maxDistance = f.NodeCube.size.y * 0.7f;
   				if(t.NodeCube.IntersectRay(ray, out float hitDistance))
   				{
   					if (hitDistance > maxDistance)
   						continue;
   					// 添加无向边（双向），路径长度默认为1，如有需求可自行调整
   					NavGraph.AddEdge(f, t, 1);
   					NavGraph.AddEdge(t, f, 1);
   				}
   			}
   		}
   	}
   }
   

   最后的样子（共大概600个结点、4000多条边）：

   

立体网格寻路

网格已经构建完成，离寻路还差最后一步了。或许有的同学只知道在平面地图寻路的A*算法实现，怎么将它应用在立体地图中？

咳咳，不是打广告啊=￣ω￣=，也许这篇文章能对你有所帮助，那是我尝试的一个泛用A*搜索的模板，简单实现相关接口就可以在这种地图进行A*寻路了：

PS：个人与2024-6-1优化了上述文章中的优先队列（堆）的实现，所以整体代码有了小变动，如果你是在2024-6-1之后看的，那请忽视这句话。

public class MyOctreeNode:IAStarNode<MyOctreeNode>, IComparable<MyOctreeNode>
{
    private const float MIN_CUBE_SIZE = 1f; // 最小方格尺寸
    public MyOctreeNode Parent{ get; set; } //父节点

    //实现IAStarNode接口属性
    public float SelfCost { get; set; }
    public float GCost { get; set; }
    public float HCost { get; set; }
    public float FCost => GCost + HCost;

    //……

    //实现接口函数
    public float GetDistance(MyOctreeNode otherNode)
    {
        return Vector3.Distance(NodeCube.center, otherNode.NodeCube.center);
    }

    public List<MyOctreeNode> GetSuccessors(object nodeMap)
    {
        var map = (MyGraph<MyOctreeNode, int>)nodeMap;
        return map.GetNeighbor(this);
    }

    public int CompareTo(MyOctreeNode other)
    {
        float res = FCost - other.FCost;
        if(res == 0)
            res = HCost - other.HCost;
        return (int)res;
    }
}

还有一件事，要实现一个将空间点转化为八叉树节点的方法，这也不难，就是可以通过Bounds.Contains方法查询一个点是否在包围盒内部，我们在MyOctree类中添加这样的方法：

/// <summary>
/// 以指定节点开始搜索，寻找到与指定位置最接近的节点
/// </summary>
/// <param name="start">初始点</param>
/// <param name="pos">指定位置</param>
/// <returns>寻找到的节点，若没找到则返回根节点</returns>
public MyOctreeNode GetNodeByPos(MyOctreeNode start, Vector3 pos)
{
    MyOctreeNode findNode = RootNode;
    if (start == null)
        return findNode;
    if (start.Children == null)
    {
        if(start.NodeCube.Contains(pos))
            return start;
    }
    else
    {
        for(int i = 0; i < 8; ++i)
        {
            findNode = GetNodeByPos(start.Children[i], pos);
            if (findNode != RootNode)
                return findNode;
        }
    }
    return findNode;
}

最后，创建一个用来驱动A*搜索器的脚本MyOctreeAStar：

using System.Collections;
using JufGame.AI;
using System.Collections.Generic;
using UnityEngine;

public class MyOctreeAStar : MonoBehaviour
{
    public MyOctreeBuilder octree; //八叉树构建器
    private AStar_Searcher<MyGraph<MyOctreeNode, int>, MyOctreeNode> astar; //A星搜索器
    private Stack<MyOctreeNode> path; //存储路径的栈
    [SerializeField] private Transform start; //寻路起点
    [SerializeField] private Transform end; //寻路终点
    //当该值位false时会进行一次寻路，寻路完成后自动为true
    [SerializeField] private bool isFindPathEnd;
    private void Start()
    {
        astar = new AStar_Searcher<MyGraph<MyOctreeNode, int>, MyOctreeNode>(octree.Octree.NavGraph);
        path = new Stack<MyOctreeNode>();
    }
    private void Update()
    {
        if(!isFindPathEnd)
        {
            //将起点与终点的位置转化为树中节点，然后进行寻路
            var s = octree.Octree.GetNodeByPos(octree.Octree.RootNode, start.position);
            var e = octree.Octree.GetNodeByPos(octree.Octree.RootNode, end.position);
            astar.FindPath(s, e, path);
            isFindPathEnd = true;
        }
    }
    private void OnDrawGizmos()
    {
        if(Application.isPlaying)
        {
            var prevPos = start.position;
            foreach(var n in path)
            {
                Gizmos.color = Color.red;
                Gizmos.DrawLine(prevPos, n.NodeCube.center);
                prevPos = n.NodeCube.center;
            }
            Gizmos.DrawLine(prevPos, end.position);
        }
    }
}

将它挂在场景的一个物体中，设置好起点和终点（要保证起点和终点在八叉树覆盖的范围内，否则寻路会报错），然后就可以尝试寻路了：

结尾（完整代码）

利用八叉树的寻路基本就讲完了，在编写期间因为时不时对代码进行调整，可能导致各文段代码有前后差异（本人努力排查过几次了，但可能难免有疏漏），现贴上最终代码：

八叉树相关的四个类：

using System;
using System.Collections;
using System.Collections.Generic;
using JufGame.AI;
using UnityEngine;

public enum NodeType
{
    Normal, Obstacles,
}
public class MyOctreeNode:IAStarNode<MyOctreeNode>, IComparable<MyOctreeNode>
{
    private const float MIN_CUBE_SIZE = 1f; // 最小方格尺寸
    public MyOctreeNode Parent{ get; set; } //父节点
    public float SelfCost { get; set; }
    public float GCost { get; set; }
    public float HCost { get; set; }
    public float FCost => GCost + HCost;

    public MyOctreeNode[] Children; //子节点
    public Bounds NodeCube; //用包围盒作为结点方块，方便后续检测
    public NodeType Type = NodeType.Normal;
    public MyOctreeNode(Bounds nodeCube, MyOctreeNode parent)
    {
        Parent = parent;
        NodeCube = nodeCube;
        SelfCost = 1;
    }
    public void Divide(Collider collider)
    {
        //因为是正方体，所以用一条边来判断尺寸即可
        if(NodeCube.size.x >= MIN_CUBE_SIZE)
        {
            // 子方块的半尺寸， 用半尺寸是因为构建Bounds需要
            float childHalfSize = NodeCube.size.x / 4;
            if (Children == null)
                Children = new MyOctreeNode[8];
            Vector3 offset; //子节点偏移
            for(int i = 0; i < 8; ++i)
            {
                //0~7的二进制位结构恰好满足我们所需要的组合形式
                offset.x = (1 & i) != 0 ? childHalfSize : -childHalfSize; //取二进制第0位
                offset.y = (2 & i) != 0 ? childHalfSize : -childHalfSize; //取二进制第1位
                offset.z = (4 & i) != 0 ? childHalfSize : -childHalfSize; //取二进制第2位
                var childBounds = new Bounds(NodeCube.center + offset, 2 * childHalfSize * Vector3.one);
                if(Children[i] == null)
                    Children[i] = new MyOctreeNode(childBounds, this);
                /*
                进一步分裂前，先判断一下有没有遇到障碍物，没有就不要继续分裂了;
                也可以再附带添加些其它检测条件，比如obj.layer等
                */
                if(childBounds.Intersects(collider.bounds))
                {
                    Children[i].Divide(collider); // 每个子节点继续分裂
                }
            }
        }
        else
        {
            Type = NodeType.Obstacles;
        }
    }
    //seeOne为true，则只查看分裂后的一个，否则查看所有分裂后的方块
    public void Draw(bool isSeeOne)
    {
        var drawColor = Color.green;
        if(Type == NodeType.Obstacles)
            drawColor = Color.red;
        Gizmos.color = drawColor;
        Gizmos.DrawWireCube(NodeCube.center, NodeCube.size);
        if (Children == null)
            return;
        foreach(var c in Children)
        {
            c.Draw(isSeeOne);
            if(isSeeOne)
            {
                break;
            }
        }
    }

    public float GetDistance(MyOctreeNode otherNode)
    {
        return Vector3.Distance(NodeCube.center, otherNode.NodeCube.center);
    }

    public List<MyOctreeNode> GetSuccessors(object nodeMap)
    {
        var map = (MyGraph<MyOctreeNode, int>)nodeMap;
        return map.GetNeighbor(this);
    }

    public int CompareTo(MyOctreeNode other)
    {
        float res = FCost - other.FCost;
        if(res == 0)
            res = HCost - other.HCost;
        return (int)res;
    }
}

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

public class MyOctree
{
    public MyOctreeNode RootNode;
    public MyGraph<MyOctreeNode, int> NavGraph; //寻路网格图
    public MyOctree(GameObject[] allObjects, MyGraph<MyOctreeNode, int> navGraph)
    {
        var baseCube = new Bounds();
        NavGraph = navGraph;
        foreach(var o in allObjects)
        {
            baseCube.Encapsulate(o.GetComponent<Collider>().bounds);
        }
        //选取最长的一条边来作为正方体的边长，并将包围盒改成正方体
        //这里为了更好设置包围盒，同样记录半尺寸
        var cubeHalfSize = 0.5f * Mathf.Max(baseCube.size.x, baseCube.size.y, baseCube.size.z) * Vector3.one;
        baseCube.SetMinMax(baseCube.center - cubeHalfSize, baseCube.center + cubeHalfSize);

        RootNode = new MyOctreeNode(baseCube, null);
        foreach(var o in allObjects) //具体分裂
        {
            //有碰撞体的物体才有检测的必要
            if(o.TryGetComponent(out Collider collider))
            {
                RootNode.Divide(collider);
            }
        }
        NodeToGraph(RootNode);
        //Debug.Log(NavGraph.NodeSet.Count); //查看结点数量
        GenerateEdges();
        //Debug.Log(NavGraph.EdgeList.Count); //查看边的数量
    }

    //将树中的所有结点入图
    private void NodeToGraph(MyOctreeNode node)
    {
        if (node == null) return;
        // 没有子节点且为非障碍的结点才能入图
        if(node.Children == null && node.Type != NodeType.Obstacles)
        {
            NavGraph.AddNode(node);
        }
        if(node.Children != null)
        {
            foreach(var c in node.Children)
            {
                NodeToGraph(c);
            }
        }
    }

    //生成边
    private void GenerateEdges()
    {
        foreach(var f in NavGraph.NodeSet)
        {
            foreach(var t in NavGraph.NodeSet)
            {
                if (f == t)
                    continue;
                var ray = new Ray(f.NodeCube.center, t.NodeCube.center - f.NodeCube.center);
                // 限制全连接范围
                var maxDistance = f.NodeCube.size.y * 0.7f;
                if(t.NodeCube.IntersectRay(ray, out float hitDistance))
                {
                    if (hitDistance > maxDistance)
                        continue;
                    // 添加无向边（双向），路径长度默认为1，如有需求可自行调整
                    NavGraph.AddEdge(f, t, 1);
                    NavGraph.AddEdge(t, f, 1);
                }
            }
        }
    } 

    /// <summary>
    /// 以指定节点开始搜索，寻找到与指定位置最接近的节点
    /// </summary>
    /// <param name="start">初始点</param>
    /// <param name="pos">指定位置</param>
    /// <returns>寻找到的节点，若没找到则返回根节点</returns>
    public MyOctreeNode GetNodeByPos(MyOctreeNode start, Vector3 pos)
    {
        MyOctreeNode findNode = RootNode;
        if (start == null)
            return findNode;
        if (start.Children == null)
        {
            if(start.NodeCube.Contains(pos))
                return start;
        }
        else
        {
            for(int i = 0; i < 8; ++i)
            {
                findNode = GetNodeByPos(start.Children[i], pos);
                if (findNode != RootNode)
                    return findNode;
            }
        }
        return findNode;
    }
}

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

public class MyOctreeBuilder : MonoBehaviour
{
    public GameObject[] Objects; //场景包含的全部对象
    public MyOctree Octree; // 八叉树
    [SerializeField] private bool isSeeOne = false; //是否只观察一个分裂后的节点
    [SerializeField] private bool isDrawOctreeCube = true; //是否绘制二叉树
    [SerializeField] private bool isDrawNode = true; // 是否要绘制图的节点
    [SerializeField] private bool isDrawEdge = true; // 是否要绘制图的边
    private void Awake()
    {
        Octree = new MyOctree(Objects, new MyGraph<MyOctreeNode, int>());
    }
    private void OnDrawGizmos()
    {
        if(Application.isPlaying)
        {
            if(isDrawOctreeCube)
            {
                Octree.RootNode.Draw(isSeeOne);
            }
            DrawGraph();
        }
    }
    private void DrawGraph()
    {
        if(isDrawEdge)
        {
            foreach(var edge in Octree.NavGraph.EdgeList)
            {
                Gizmos.color = Color.red;
                Gizmos.DrawLine(edge.Key.Item1.NodeCube.center,
                edge.Key.Item2.NodeCube.center);
            }
        }
        if(isDrawNode)
        {
            foreach(var node in Octree.NavGraph.NodeSet)
            {
                Gizmos.color = new Color(1, 1, 0);
                Gizmos.DrawWireSphere(node.NodeCube.center, 0.25f);
            }
        }
    }
}

using System.Collections;
using JufGame.AI;
using System.Collections.Generic;
using UnityEngine;

public class MyOctreeAStar : MonoBehaviour
{
    public MyOctreeBuilder octree; //八叉树构建器
    private AStar_Searcher<MyGraph<MyOctreeNode, int>, MyOctreeNode> astar; //A星搜索器
    private Stack<MyOctreeNode> path; //存储路径的栈
    [SerializeField] private Transform start; //寻路起点
    [SerializeField] private Transform end; //寻路终点
    //当该值位false时会进行一次寻路，寻路完成后自动为true
    [SerializeField] private bool isFindPathEnd;
    private void Start()
    {
        astar = new AStar_Searcher<MyGraph<MyOctreeNode, int>, MyOctreeNode>(octree.Octree.NavGraph);
        path = new Stack<MyOctreeNode>();
    }
    private void Update()
    {
        if(!isFindPathEnd)
        {
            //将起点与终点的位置转化为树中节点，然后进行寻路
            var s = octree.Octree.GetNodeByPos(octree.Octree.RootNode, start.position);
            var e = octree.Octree.GetNodeByPos(octree.Octree.RootNode, end.position);
            astar.FindPath(s, e, path);
            isFindPathEnd = true;
        }
    }
    private void OnDrawGizmos()
    {
        if(Application.isPlaying)
        {
            var prevPos = start.position;
            foreach(var n in path)
            {
                Gizmos.color = Color.red;
                Gizmos.DrawLine(prevPos, n.NodeCube.center);
                prevPos = n.NodeCube.center;
            }
            Gizmos.DrawLine(prevPos, end.position);
        }
    }
}

与A星相关的代码也贴这里了：

using System;
using System.Collections.Generic;

namespace JufGame.Collections.Generic
{
    public class MyHeap<T> where T : IComparable<T>
    {
        public int NowLength { get; private set; }
        public int MaxLength { get; private set; }
        public T Top => heap[0];
        public bool IsEmpty => NowLength == 0;
        public bool IsFull => NowLength >= MaxLength - 1;
        private readonly Dictionary<T, int> nodeIdxTable; // 记录结点在数组中的位置，方便查找
        private readonly bool isReverse;
        private readonly T[] heap;

        public MyHeap(int maxLength, bool isReverse = false)
        {
            NowLength = 0;
            MaxLength = maxLength;
            heap = new T[MaxLength + 1];
            nodeIdxTable = new Dictionary<T, int>();
            this.isReverse = isReverse;
        }
        public T this[int index]
        {
            get => heap[index];
        }
        public void PushHeap(T value)
        {
            if (NowLength < MaxLength)
            {
                if (nodeIdxTable.ContainsKey(value))
                    nodeIdxTable[value] = NowLength;
                else
                    nodeIdxTable.Add(value, NowLength);
                heap[NowLength] = value;
                Swim(NowLength);
                ++NowLength;
            }
        }
        public void PopHeap()
        {
            if (NowLength > 0)
            {
                nodeIdxTable[heap[0]] = -1;
                heap[0] = heap[--NowLength];
                nodeIdxTable[heap[0]] = 0;
                Sink(0);
            }
        }
        public bool Contains(T value)
        {
            return nodeIdxTable.ContainsKey(value) && nodeIdxTable[value] != -1;
        }
        public T Find(T value)
        {
            if (Contains(value))
                return heap[nodeIdxTable[value]];
            return default;
        }
        public void Clear()
        {
            nodeIdxTable.Clear();
            NowLength = 0;
        }
        private void SwapValue(T a, T b)
        {
            var aIdx = nodeIdxTable[a];
            var bIdx = nodeIdxTable[b];
            heap[aIdx] = b;
            heap[bIdx] = a;
            nodeIdxTable[a] = bIdx;
            nodeIdxTable[b] = aIdx;
        }

        private void Swim(int index)
        {
            int father;
            while (index > 0)
            {
                father = (index - 1) >> 1;
                if (IsBetter(heap[index], heap[father]))
                {
                    SwapValue(heap[father], heap[index]);
                    index = father;
                }
                else return;
            }
        }

        private void Sink(int index)
        {
            int largest, left = (index << 1) + 1;
            while (left < NowLength)
            {
                largest = left + 1 < NowLength && IsBetter(heap[left + 1], heap[left]) ? left + 1 : left;
                if (IsBetter(heap[index], heap[largest]))
                    largest = index;
                if (largest == index) return;
                SwapValue(heap[largest], heap[index]);
                index = largest;
                left = (index << 1) + 1;
            }
        }
        private bool IsBetter(T v1, T v2)
        {
            return isReverse ? (v2.CompareTo(v1) < 0 ): (v1.CompareTo(v2) < 0);
        }
    }
}

using JufGame.Collections.Generic;
using System;
using System.Collections.Generic;

namespace JufGame.AI
{
	public interface IAStarNode<T> where T : IAStarNode<T>
	{
        public T Parent { get; set; }
        public float SelfCost { get; set; }
        public float GCost { get; set; }//距初始状态的代价
        public float HCost { get; set; }//距目标状态的代价
        public float FCost { get; }
        /// <summary>
        /// 获取与指定节点的预测代价
        /// </summary>
        public float GetDistance(T otherNode);
		/// <summary>
		/// 获取后继（邻居）节点
		/// </summary>
        /// <param name="nodeMap">寻路所在的地图，类型看具体情况转换，
        /// 故用object类型</param>
        /// <returns>后继节点列表</returns>
		public List<T> GetSuccessors(object nodeMap);
        /* 一般比较可用以下函数
        public int CompareTo(AStarNode other)
        {
        	var res = (int)(FCost - other.FCost);
			if(res == 0)
				res = (int)(HCost - other.HCost);
			return res;
        }
        */
	}
    /// <summary>
    /// A星搜索器
    /// </summary>
    /// <typeparam name="T_Map">搜索的图类</typeparam>
    /// <typeparam name="T_Node">搜索的节点类</typeparam>
	public class AStar_Searcher<T_Map, T_Node> where T_Node: IAStarNode<T_Node>, IComparable<T_Node>
	{
        private readonly HashSet<T_Node> closeList;//探索集
        private readonly MyHeap<T_Node> openList;//边缘集
        private readonly T_Map nodeMap;//搜索空间（地图）
        public AStar_Searcher(T_Map map, int maxNodeSize = 200)
        {
            nodeMap = map;
            closeList = new HashSet<T_Node>();
            //maxNodeSize用于限制路径节点的上限，避免陷入无止境搜索的情况
            openList = new MyHeap<T_Node>(maxNodeSize);
        }
        /// <summary>
        /// 搜索（寻路）
        /// </summary>
        /// <param name="start">起点</param>
        /// <param name="target">终点</param>
        /// <param name="pathRes">返回生成的路径</param>
        public void FindPath(T_Node start, T_Node target, Stack<T_Node> pathRes)
        {
            T_Node currentNode;
            pathRes.Clear();//清空路径以备存储新的路径
            closeList.Clear();
            openList.Clear();
            openList.PushHeap(start);
            while (!openList.IsEmpty)
            {
                currentNode = openList.Top;//取出边缘集中最小代价的节点
                openList.PopHeap();
                closeList.Add(currentNode);//拟定移动到该节点，将其放入探索集
                if (currentNode.Equals(target) || openList.IsFull)//如果找到了或图都搜完了也没找到时
                {
                    GenerateFinalPath(start, target, pathRes);//生成路径并保存到pathRes中
                    return;
                }
                UpdateList(currentNode, target);//更新边缘集和探索集
            }
            return;
        }
        private void GenerateFinalPath(T_Node startNode, T_Node endNode, Stack<T_Node> pathStack)
        {
            pathStack.Push(endNode);//因为回溯，所以用栈储存生成的路径
            var tpNode = endNode.Parent;
            while (!tpNode.Equals(startNode))
            {
                pathStack.Push(tpNode);
                tpNode = tpNode.Parent;
            }
            pathStack.Push(startNode);
        }
        private void UpdateList(T_Node curNode, T_Node endNode)
        {
            T_Node sucNode;
            float tpCost;
            bool isNotInOpenList;
            var successors = curNode.GetSuccessors(nodeMap);//找出当前节点的后继节点
            for (int i = 0; i < successors.Count; ++i)
            {
                sucNode = successors[i];
                if (closeList.Contains(sucNode))//后继节点已被探索过就忽略
                    continue;
                tpCost = curNode.GCost + sucNode.SelfCost;
                isNotInOpenList = !openList.Contains(sucNode);
                if (isNotInOpenList || tpCost < sucNode.GCost)
                {
                    sucNode.GCost = tpCost;
                    sucNode.HCost = sucNode.GetDistance(endNode);//计算启发函数估计值
                    sucNode.Parent = curNode;//记录父节点，方便回溯
                    if (isNotInOpenList)
                    {
                        openList.PushHeap(sucNode);
                    }
                }
            }
        }
    }
}

在尝试用渐进式的方式写文章，基本就是顺着思路走下来的 （如果有某几步跳得比较大，那就是我写烦了。所以文中的代码修改会比较频繁，但我觉得比起先将思路再贴出完整代码的方式，这样会更容易让人理解（当然，只是个人觉得。