Node pool change

src/DotRecast.Detour/DtNodePool.cs

@@ -18,6 +18,7 @@ freely, subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
 using System.Collections.Generic;
 using System.Linq;
 
@@ -27,24 +28,63 @@ namespace DotRecast.Detour
     {
         private readonly Dictionary<long, List<DtNode>> m_map;
 
-        private int m_nodeCount;
+        private int m_usedNodesCount;
-        private readonly List<DtNode> m_nodes;
+        private List<DtNode[]> m_buckets;
+        private readonly int m_initialBufferCapacityBase;
 
-        public DtNodePool()
+        public DtNodePool(int initialBufferCapacityBase = 6) // initial size 64
         {
             m_map = new Dictionary<long, List<DtNode>>();
-            m_nodes = new List<DtNode>();
+            m_buckets = new List<DtNode[]>();
+            m_initialBufferCapacityBase = initialBufferCapacityBase;
+        }
+
+        private void AddNewBucket()
+        {
+            var bucketIndex = m_buckets.Count;
+            var bucket = new DtNode[1 << (bucketIndex + m_initialBufferCapacityBase)];
+            m_buckets.Add(bucket);
+            FillBucket(bucketIndex);
+        }
+
+        private void FillBucket(int bucketIndex)
+        {
+            var bucket = m_buckets[bucketIndex];
+            var startIndex = GetBucketStartIndex(bucketIndex);
+            for (int i = 0; i < bucket.Length; i++)
+            {
+                bucket[i] = new DtNode(startIndex + i);
+            }
+        }
+
+        private int GetBucketStartIndex(int bucketIndex)
+        {
+            return ((1 << (bucketIndex + m_initialBufferCapacityBase)) - 1) ^ ((1 << m_initialBufferCapacityBase) - 1);
+        }
+
+        private DtNode[] EnsureBucket(int bucketIndex)
+        {
+            if (m_buckets.Count == bucketIndex)
+                AddNewBucket();
+            else if (m_buckets.Count < bucketIndex)
+                throw new Exception();
+            return m_buckets[bucketIndex];
+        }
+
+        private int GetBucketIndexByElementIndex(int elementIndex)
+        {
+            return DtUtils.Ilog2((elementIndex >> m_initialBufferCapacityBase) + 1);
         }
 
         public void Clear()
         {
             m_map.Clear();
-            m_nodeCount = 0;
+            m_usedNodesCount = 0;
         }
 
         public int GetNodeCount()
         {
-            return m_nodeCount;
+            return m_usedNodesCount;
         }
 
         public int FindNodes(long id, out List<DtNode> nodes)
@@ -93,15 +133,10 @@ namespace DotRecast.Detour
 
         private DtNode Create(long id, int state, List<DtNode> nodes)
         {
-            if (m_nodes.Count <= m_nodeCount)
+            int i = m_usedNodesCount++;
-            {
+            int bucketIndex = GetBucketIndexByElementIndex(i);
-                var newNode = new DtNode(m_nodeCount);
+            int bucketStartIndex = GetBucketStartIndex(bucketIndex);
-                m_nodes.Add(newNode);
+            var node = EnsureBucket(bucketIndex)[i - bucketStartIndex];
-            }
-
-            int i = m_nodeCount;
-            m_nodeCount++;
-            var node = m_nodes[i];
             node.pidx = 0;
             node.cost = 0;
             node.total = 0;
@@ -123,9 +158,16 @@ namespace DotRecast.Detour
 
         public DtNode GetNodeAtIdx(int idx)
         {
-            return idx != 0
+            if (idx == 0)
-                ? m_nodes[idx - 1]
+                return null;
-                : null;
+            
+            int bucketIndex = GetBucketIndexByElementIndex(idx - 1);
+            if (m_buckets.Count <= bucketIndex)
+                throw new ArgumentOutOfRangeException();
+
+            int bucketStartIndex = GetBucketStartIndex(bucketIndex);
+            var node = EnsureBucket(bucketIndex)[idx - bucketStartIndex - 1];
+            return node;
         }
 
         public DtNode GetNode(long refs)
@@ -135,7 +177,7 @@ namespace DotRecast.Detour
 
         public IEnumerable<DtNode> AsEnumerable()
         {
-            return m_nodes.Take(m_nodeCount);
+            return m_buckets.SelectMany(x => x);
         }
     }
 }