游戏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. 剔除不必要的结点(关键)
仅是不断分裂生成小方块,那最终不还是和我们开头的笨方法一样吗(会有一堆密密麻麻的点)?我们可以注意到,其实有一些方块没必要继续分裂下去。分裂行为其实是有目的的:检测出哪里有障碍物。一个大方块不断分裂变小,就是更进一步定位内部障碍物位置的过程,如果它一开始就没碰到什么障碍物,那也没必要分裂了。
我们需要对先前几个类中的内容稍加修改:
- 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) 条件能剔除所有障碍结点?
是可以做的,我们来看看下面几种情况:- 有子节点的障碍方块。以下图绿色十字星的 \(4\times4\) 方形为例,它会被剔除
- 无子节点的障碍方块。仍是以绿色十字星标记的方形为例,它也会被剔除
- 有子节点的障碍方块。以下图绿色十字星的 \(4\times4\) 方形为例,它会被剔除
大改一下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多条边):
- (node.Children == null && node.Type != NodeType.Obstacles) 条件能剔除所有障碍结点?
立体网格寻路
网格已经构建完成,离寻路还差最后一步了。或许有的同学只知道在平面地图寻路的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);
}
}
}
}
}
}
在尝试用渐进式的方式写文章,基本就是顺着思路走下来的 (如果有某几步跳得比较大,那就是我写烦了。所以文中的代码修改会比较频繁,但我觉得比起先将思路再贴出完整代码的方式,这样会更容易让人理解(当然,只是个人觉得。