遊戲AI尋路——八叉樹+A*尋路

狐王驾虎發表於2024-06-02

利用八叉樹的空中尋路

你有思考過在空中如何進行尋“路”嗎？來想象一個的場景：飛機從空中基地出發，要避開許多空中建築，最終到達目的地。這種情況下的尋路是沒有路面的，尋路物體的移動方向也比較自由，這該怎麼尋呢？

如果我們只是在一個平面進行尋路，我們可以直接用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. 剔除不必要的結點（關鍵）

僅是不斷分裂生成小方塊，那最終不還是和我們開頭的笨方法一樣嗎（會有一堆密密麻麻的點）？我們可以注意到，其實有一些方塊沒必要繼續分裂下去。分裂行為其實是有目的的：檢測出哪裡有障礙物。一個大方塊不斷分裂變小，就是更進一步定位內部障礙物位置的過程，如果它一開始就沒碰到什麼障礙物，那也沒必要分裂了。

我們需要對先前幾個類中的內容稍加修改：

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); // 每個子節點繼續分裂
            }
        }
    }
}

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. 連線成網

結點已經全部劃分出來了，那如何連線成網格呢？我們可以自然而然想到兩種做法（當然，可以有其它做法）：

父子相連：每個結點都與它的父結點和子結點相連（如果有的話）
全連線：每個結點和其它結點依次相連

這兩種樸素的做法其實已經反映出了連線需要考慮的問題：

首先，這兩種做法都可以算是對的。它們都能保證整個網路是連通的，也就說，在這兩種方法構建的網格下，我們總可以找到路徑從一個結點到另外一個結點。

對於第一種做法，它連線成網所構建的邊的數量明顯比第二種來得少。但很顯然，由於邊的數量過少，實際的路徑選擇也會很少，即使是去不同的地方，走出的路徑也是大同小異，並且還會出現繞遠路的情況。

而第二種就是另一個極端了，它所構建的網格連通性極高，兩點之間通常都含有著豐富的路徑選擇，但需要儲存的邊實在是太多了。

哪種方案更好？這是根據實際情況調整的。

這裡我們採用一種第二種策略，但有一點要注意：我們應該把全是障礙物的結點排除掉，因為它們所在的位置已經沒有行走的餘地了。

現在就來實現一下：

我們準備一個列舉，來區分結點的型別（也方便後續擴充），暫時就分兩類結點：通常、障礙（針對最小障礙）。並在分裂過程中判別哪些是障礙。根據我們的分裂邏輯，可以清楚地想到：只要仍需分裂的最小結點才是最小障礙：

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;
        //……
    }
}

可以清楚的看到排除的結點：

顯然，我們需要用到圖結構。由於本文的重點是在八叉樹上，所以就不贅述圖的實現了，作為一種基礎的資料結構，我希望你能夠自己實現。當然，實在沒有的話，這裡也提供一份作為參考吧(⊙ˍ⊙)：

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);
                    }
                }
            }
        }
    }
}

在嘗試用漸進式的方式寫文章，基本就是順著思路走下來的 ~~（如果有某幾步跳得比較大，那就是我寫煩了~~。所以文中的程式碼修改會比較頻繁，但我覺得比起先將思路再貼出完整程式碼的方式，這樣會更容易讓人理解（當然，只是個人覺得。

二叉搜尋樹
2024-11-21
A*尋路
2024-03-30
遊戲尋路中 A* 演算法的改進
2020-08-21
遊戲演算法
從二分搜尋到二叉搜尋樹
2023-04-03
Day20 | 654.最大二叉樹、 617.合併二叉樹、 700.二叉搜尋樹中的搜尋 98.驗證二叉搜尋樹
2024-06-15
二叉樹
程式碼隨想錄day18 || 530 二叉搜尋樹最小差，501 二叉搜尋樹眾數，236 二叉搜尋樹最近公共祖先
2024-08-03
二叉搜尋樹的操作集
2018-08-04
二叉搜尋樹的結構
2024-10-20
javascript實現二叉搜尋樹
2021-09-09
JavaScript
有序表和搜尋二叉樹
2021-10-14
二叉樹
js實現完全排序二叉樹、二叉搜尋樹
2019-07-08
JS排序二叉樹
二叉搜尋樹和二叉樹的最近公共祖先
2021-05-29
二叉樹
Leetcode 700. 二叉搜尋樹中的搜尋（DAY 2）
2020-12-20
LeetCode
資料結構中的樹(二叉樹、二叉搜尋樹、AVL樹)
2020-08-04
資料結構二叉樹
尋寶路線
2018-04-12
JavaScript 二叉搜尋樹以及實現翻轉二叉樹
2018-06-10
JavaScript二叉樹
使用GML的八方向自動尋路
2020-04-21
二叉搜尋樹的python實現
2019-02-16
Python
演算法篇 - 二叉搜尋樹
2019-01-21
演算法
96. 不同的二叉搜尋樹
2024-11-11
【資料結構】二叉搜尋樹！！！
2018-03-08
資料結構
手寫AVL平衡二叉搜尋樹
2021-09-09
資料結構-二叉搜尋樹
2021-01-05
資料結構
leetcode 700. 二叉搜尋樹中的搜尋思考分析
2020-10-24
LeetCode
滿二叉樹、完全二叉樹、平衡二叉樹、二叉搜尋樹（二叉查詢樹）和最優二叉樹
2020-11-02
二叉樹
尋寶路漫漫，從《金銀島》聊聊”尋寶“主題遊戲的代入感進階之路
2019-04-25
遊戲
python 二叉樹深度優先搜尋和廣度優先搜尋
2019-02-16
Python二叉樹
unity 自動尋路
2018-10-28
Unity
不走尋常路
2020-10-13
資料結構之「二叉搜尋樹」
2019-03-29
資料結構
LeetCode 95 | 構造出所有二叉搜尋樹
2020-09-01
LeetCode
[Python手撕]不同的二叉搜尋樹
2024-10-06
Python
[Python手撕]判斷二叉搜尋樹
2024-09-26
Python
LeetCode98. 驗證二叉搜尋樹
2024-07-29
LeetCode
LeetCode-098-驗證二叉搜尋樹
2021-11-17
LeetCode
LeetCode-099-恢復二叉搜尋樹
2021-11-18
LeetCode
二叉搜尋樹如何刪除節點
2020-11-24
【LeetCode】98. 驗證二叉搜尋樹
2021-08-02
LeetCode