diff options
Diffstat (limited to 'WorldlineKeepers/Assets/Scripts/Physics/Quadtree.cs')
| -rw-r--r-- | WorldlineKeepers/Assets/Scripts/Physics/Quadtree.cs | 292 |
1 files changed, 292 insertions, 0 deletions
diff --git a/WorldlineKeepers/Assets/Scripts/Physics/Quadtree.cs b/WorldlineKeepers/Assets/Scripts/Physics/Quadtree.cs new file mode 100644 index 0000000..a012d26 --- /dev/null +++ b/WorldlineKeepers/Assets/Scripts/Physics/Quadtree.cs @@ -0,0 +1,292 @@ +using JetBrains.Annotations; +using System; +using System.Collections; +using System.Collections.Generic; +using System.Drawing; +using System.Linq; +using System.Runtime.InteropServices.WindowsRuntime; +using Unity.VisualScripting; +using UnityEngine; + +// https://gamedev.stackexchange.com/questions/6345/quad-tree-vs-grid-based-collision-detection + +namespace mh +{ + + public interface IQuadTreeObject + { + public Vector4 bound { get; } + } + + /// <summary> + /// 四叉树空间分割 + /// </summary> + public class Quadtree + { + public const int kMaxObjectsPerBlock = 4; + public const int kMaxLevel = 5; + + private int m_Level; + private Vector4 m_Bounds; // x,y,z,w => posx,posy,width,height + private Quadtree[] m_SubTrees; // 从右上角开始逆时针索引 + private List<IQuadTreeObject> m_Objects; // 非叶节点的为0 + private bool m_IsRoot; + + public Vector4 bound { get { return m_Bounds; } } + public float x { get { return m_Bounds.x; } } + public float y { get { return m_Bounds.y; } } + public float w { get { return m_Bounds.z; } } + public float h { get { return m_Bounds.w; } } + public float halfW { get { return w / 2; } } + public float halfH { get { return h / 2; } } + public float left { get { return x - halfW; } } + public float right { get { return x + halfW; } } + public float top { get { return y + halfH; } } + public float bottom { get { return y - halfH; } } + + public bool isRoot { get { return m_IsRoot; } } + + private static Queue<List<IQuadTreeObject>> m_QuadtreeObjPool = new Queue<List<IQuadTreeObject>>(); + private static Queue<Quadtree> m_QuadtreePool = new Queue<Quadtree>(); + + private Quadtree QueryQuadtree(int level, Vector4 bounds) + { + if(m_QuadtreePool.Count == 0) + { + return new Quadtree(level, bounds); + } + Quadtree tree = m_QuadtreePool.Dequeue(); + tree.m_Level = level; + tree.m_Bounds = bounds; + if (tree.m_Objects == null) + tree.m_Objects = QueryQuadtreeObjList(); + return tree; + } + + private void RecycleQuadtree(ref Quadtree tree) + { + tree.Clear(); + m_QuadtreePool.Enqueue(tree); + tree = null; + } + + private List<IQuadTreeObject> QueryQuadtreeObjList() + { + if(m_QuadtreeObjPool.Count == 0) + { + return new List<IQuadTreeObject>(); + } + List<IQuadTreeObject> list = m_QuadtreeObjPool.Dequeue(); + return list; + } + + private void RecycleQuadtreeObjList(ref List<IQuadTreeObject> list) + { + list.Clear(); + m_QuadtreeObjPool.Enqueue(list); + list = null; + } + + public Quadtree(int level, Vector4 bounds, bool isRoot = false) + { + m_Level = level; + m_Bounds = bounds; + m_SubTrees = new Quadtree[4]; + m_Objects = QueryQuadtreeObjList(); + m_IsRoot = isRoot; + } + + /// <summary> + /// 更改边界，只适用于最外层的tree + /// </summary> + /// <param name="bounds"></param> + public void Rebound(Vector4 bounds) + { + if(!m_IsRoot) + { + Debug.LogError("Quadtree.Rebound()只能运用于最外层"); + return; + } + m_Bounds = bounds; + } + + public void Clear(bool clearObjectList = true) + { + if (clearObjectList) + RecycleQuadtreeObjList(ref m_Objects); + else + m_Objects.Clear(); + for (int i = 0; i < m_SubTrees.Length; i++) + { + if (m_SubTrees[i] != null) + { + //m_SubTrees[i].Clear(); + //m_SubTrees[i] = null; + RecycleQuadtree(ref m_SubTrees[i]); + } + } + } + + public void Split() + { + float subWidth = (m_Bounds.z / 2); + float subHeight = (m_Bounds.w / 2); + float x = m_Bounds.x; + float y = m_Bounds.y; + m_SubTrees[0] = QueryQuadtree(m_Level + 1, new Vector4(x + subWidth / 2, y + subHeight / 2, subWidth, subHeight)); + m_SubTrees[1] = QueryQuadtree(m_Level + 1, new Vector4(x - subWidth / 2, y + subHeight / 2, subWidth, subHeight)); + m_SubTrees[2] = QueryQuadtree(m_Level + 1, new Vector4(x - subWidth / 2, y - subHeight / 2, subWidth, subHeight)); + m_SubTrees[3] = QueryQuadtree(m_Level + 1, new Vector4(x + subWidth / 2, y - subHeight / 2, subWidth, subHeight)); + } + + /// <summary> + /// 0表示没法完全放在一个subTree: subtree交界或者整个越界 + /// </summary> + /// <param name="bound"></param> + /// <returns></returns> + public int GetSubtreeIndices(Vector4 bound) + { + int indices = 0; + float halfw = bound.z / 2; + float halfh = bound.w / 2; + float lowerx = bound.x - halfw; + float higherx = bound.x + halfw; + float lowery = bound.y - halfh; + float highery = bound.y + halfh; + bool startIsNorth = highery > y; + bool startIsWest = lowerx < x; + bool endIsEast = higherx > x; + bool endIsSouth = lowery < y; + //top-right quad + if (startIsNorth && endIsEast) + { + indices |= 1; + } + + //top-left quad + if (startIsWest && startIsNorth) + { + indices |= 1 << 1; + } + + //bottom-left quad + if (startIsWest && endIsSouth) + { + indices |= 1 << 2; + } + + //bottom-right quad + if (endIsEast && endIsSouth) + { + indices |= 1 << 3; + } + + return indices; + } + + public void Insert(IQuadTreeObject obj) + { + if (m_SubTrees[0] != null) + { + int indices = GetSubtreeIndices(obj.bound); + for(int i = 0; i < 4; i++) + { + if((indices & (1 << i)) != 0) + { + m_SubTrees[i].Insert(obj); + } + } + return; + } + + m_Objects.Add(obj); + + if (m_Objects.Count > kMaxObjectsPerBlock && m_Level < kMaxLevel) // 本层满了之后重新排布层内对象 + { + if (m_SubTrees[0] == null) Split(); + + for (int i = m_Objects.Count - 1; i >= 0; i--) + { + int indices = GetSubtreeIndices(m_Objects[i].bound); + for (int j = 0; j < 4; j++) + { + if ((indices & (1 << j)) != 0) + { + m_SubTrees[j].Insert(m_Objects[i]); + } + } + } + + m_Objects.Clear(); + } + } + + /// <summary> + /// 获得可能和obj碰撞的对象（不包括自己） + /// </summary> + /// <param name="returnObjs"></param> + /// <param name="obj"></param> + /// <returns></returns> + public bool Retrieve(ref List<IQuadTreeObject> returnObjs, IQuadTreeObject obj) + { + for(int i = 0; i < m_Objects.Count; ++i) + { + if (!returnObjs.Contains(m_Objects[i]) && obj != m_Objects[i]) + { + returnObjs.Add(m_Objects[i]); + } + } + if (m_SubTrees[0] != null) + { + int indices = GetSubtreeIndices(obj.bound); + for (int i = 0; i < 4; i++) + { + if ((indices & (1 << i)) != 0) + { + m_SubTrees[i].Retrieve(ref returnObjs, obj); + } + } + } + return returnObjs.Count > 0; + } + + /// <summary> + /// 获得可能和obj碰撞的对象（不包括自己） + /// </summary> + /// <param name="returnObjs"></param> + /// <param name="obj"></param> + /// <returns></returns> + public bool Retrieve(ref List<IQuadTreeObject> returnObjs, Vector4 bound) + { + for (int i = 0; i < m_Objects.Count; ++i) // 根节点count==0 + { + if (!returnObjs.Contains(m_Objects[i])) + { + returnObjs.Add(m_Objects[i]); + } + } + if (m_SubTrees[0] != null) + { + int indices = GetSubtreeIndices(bound); + for (int i = 0; i < 4; i++) + { + if ((indices & (1 << i)) != 0) + { + m_SubTrees[i].Retrieve(ref returnObjs, bound); + } + } + } + return returnObjs.Count > 0; + } + + public void Iterate(System.Action<Quadtree> action) + { + action?.Invoke(this); + m_SubTrees[0]?.Iterate(action); + m_SubTrees[1]?.Iterate(action); + m_SubTrees[2]?.Iterate(action); + m_SubTrees[3]?.Iterate(action); + } + + } +}
\ No newline at end of file |