* quadtree

1 files changed, 209 insertions, 0 deletions

diff --git a/marching/Assets/Scripts/Physics/Quadtree.cs b/marching/Assets/Scripts/Physics/Quadtree.cs
new file mode 100644
index 0000000..c157512
--- /dev/null
+++ b/marching/Assets/Scripts/Physics/Quadtree.cs
@@ -0,0 +1,209 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Runtime.InteropServices.WindowsRuntime;
+using Unity.VisualScripting;
+using UnityEngine;
+
+namespace mh
+{
+
+    public interface IQuadTreeObject
+    {
+        public Vector4 bound { get; }
+    }
+
+    public class Quadtree
+    {
+        public const int kMaxObjectsPerBlock = 5;
+        public const int kMaxLevel = 20;
+
+        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; // 当前层能容纳，但任何一个子树都无法容纳的对象
+
+        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; } }
+
+        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)
+        {
+            m_Level = level;
+            m_Bounds = bounds;
+            m_SubTrees = new Quadtree[4];
+            m_Objects = QueryQuadtreeObjList();
+        }
+
+        public void Rebound(Vector4 bounds)
+        {
+            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));
+        }
+
+        // x, y, z, w
+        /// <summary>
+        /// -1表示没法完全放在一个subTree: subtree交界或者整个越界
+        /// </summary>
+        /// <param name="bound"></param>
+        /// <returns></returns>
+        public int GetSubtreeIndex(Vector4 bound)
+        {
+            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 bFitInLeft = lowerx >= left && higherx <= x;
+            bool bFitInRight = lowerx >= x && higherx <= right;
+            bool bFitInTop = lowery >= y && highery <= top;
+            bool bFitInBottom = lowery >= bottom && highery <= y;
+            if (bFitInRight && bFitInTop) return 0;
+            if (bFitInLeft && bFitInTop) return 1;
+            if (bFitInLeft && bFitInBottom) return 2;
+            if (bFitInRight && bFitInBottom) return 3;
+            return -1;
+        }
+
+        public void Insert(IQuadTreeObject obj)
+        {
+            if (m_SubTrees[0] != null)
+            {
+                int index = GetSubtreeIndex(obj.bound);
+                if (index != -1)
+                {
+                    m_SubTrees[index].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 index = GetSubtreeIndex(m_Objects[i].bound);
+                    if(index != -1) 
+                    {
+                        IQuadTreeObject o = m_Objects[i];
+                        m_Objects.RemoveAt(i);
+                        m_SubTrees[index].Insert(o);
+                    }
+                }
+            }
+        }
+
+        /// <summary>
+        /// 获得可能和obj碰撞的对象（不包括自己）
+        /// </summary>
+        /// <param name="returnObjs"></param>
+        /// <param name="obj"></param>
+        /// <returns></returns>
+        public bool Retrieve(ref List<IQuadTreeObject> returnObjs, IQuadTreeObject obj)
+        {
+            int index = GetSubtreeIndex(obj.bound);
+            returnObjs.AddRange(m_Objects);
+            if (index != -1 && m_SubTrees[0] != null)
+            {
+                m_SubTrees[index].Retrieve(ref returnObjs, obj);
+            }
+            if(returnObjs.Contains(obj))
+            {
+                returnObjs.Remove(obj);
+            }
+            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