diff --git a/src/DotRecast.Detour/DtNodePool.cs b/src/DotRecast.Detour/DtNodePool.cs index 08ecf8f..c94b460 100644 --- a/src/DotRecast.Detour/DtNodePool.cs +++ b/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> m_map; - private int m_nodeCount; - private readonly List m_nodes; + private int m_usedNodesCount; + private List m_buckets; + private readonly int m_initialBufferCapacityBase; - public DtNodePool() + public DtNodePool(int initialBufferCapacityBase = 6) // initial size 64 { m_map = new Dictionary>(); - m_nodes = new List(); + m_buckets = new List(); + 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 nodes) @@ -93,15 +133,10 @@ namespace DotRecast.Detour private DtNode Create(long id, int state, List nodes) { - if (m_nodes.Count <= m_nodeCount) - { - var newNode = new DtNode(m_nodeCount); - m_nodes.Add(newNode); - } - - int i = m_nodeCount; - m_nodeCount++; - var node = m_nodes[i]; + int i = m_usedNodesCount++; + int bucketIndex = GetBucketIndexByElementIndex(i); + int bucketStartIndex = GetBucketStartIndex(bucketIndex); + var node = EnsureBucket(bucketIndex)[i - bucketStartIndex]; node.pidx = 0; node.cost = 0; node.total = 0; @@ -123,9 +158,16 @@ namespace DotRecast.Detour public DtNode GetNodeAtIdx(int idx) { - return idx != 0 - ? m_nodes[idx - 1] - : null; + if (idx == 0) + return 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 AsEnumerable() { - return m_nodes.Take(m_nodeCount); + return m_buckets.SelectMany(x => x); } } } \ No newline at end of file