From 37b050081d9b504e8b67b831a2ff1cf5cb84342f Mon Sep 17 00:00:00 2001
From: ikpil <ikpil@naver.com>
Date: Thu, 20 Apr 2023 01:02:26 +0900
Subject: [PATCH] upgrade performance

---
 src/DotRecast.Core/NodeQueue.cs     | 29 ++++++++++++++++++++---------
 src/DotRecast.Detour.Crowd/Crowd.cs |  4 ++--
 src/DotRecast.Detour/NodeQueue.cs   | 13 +++++++------
 3 files changed, 29 insertions(+), 17 deletions(-)

diff --git a/src/DotRecast.Core/NodeQueue.cs b/src/DotRecast.Core/NodeQueue.cs
index 2874741..eea103d 100644
--- a/src/DotRecast.Core/NodeQueue.cs
+++ b/src/DotRecast.Core/NodeQueue.cs
@@ -26,33 +26,40 @@ namespace DotRecast.Core
 
     public class OrderedQueue<T>
     {
+        private bool _dirty;
         private readonly List<T> _items;
         private readonly Comparison<T> _comparison;
 
         public OrderedQueue(Comparison<T> comparison)
         {
             _items = new List<T>();
-            _comparison = comparison;
+            _comparison = (x, y) => comparison.Invoke(x, y) * -1; // reverse
         }
 
-        public int count()
+        public int Count()
         {
             return _items.Count;
         }
 
-        public void clear()
+        public void Clear()
         {
             _items.Clear();
         }
 
-        public T top()
+        public T Top()
         {
-            return _items[0];
+            if (_dirty)
+            {
+                _items.Sort(_comparison); // reverse
+                _dirty = false;
+            }
+
+            return _items[_items.Count - 1];
         }
 
         public T Dequeue()
         {
-            var node = top();
+            var node = Top();
             _items.Remove(node);
             return node;
         }
@@ -60,15 +67,19 @@ namespace DotRecast.Core
         public void Enqueue(T item)
         {
             _items.Add(item);
-            _items.Sort(_comparison);
+            _dirty = true;
         }
 
         public void Remove(T item)
         {
-            _items.Remove(item);
+            int idx = _items.FindLastIndex(x => item.Equals(x));
+            if (0 > idx)
+                return;
+            
+            _items.RemoveAt(idx);
         }
 
-        public bool isEmpty()
+        public bool IsEmpty()
         {
             return 0 == _items.Count;
         }
diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs
index eee89ef..755de6d 100644
--- a/src/DotRecast.Detour.Crowd/Crowd.cs
+++ b/src/DotRecast.Detour.Crowd/Crowd.cs
@@ -662,7 +662,7 @@ namespace DotRecast.Detour.Crowd
                 }
             }
 
-            while (!queue.isEmpty())
+            while (!queue.IsEmpty())
             {
                 CrowdAgent ag = queue.Dequeue();
                 ag.targetPathQueryResult = m_pathq.request(ag.corridor.getLastPoly(), ag.targetRef, ag.corridor.getTarget(),
@@ -850,7 +850,7 @@ namespace DotRecast.Detour.Crowd
                 }
             }
 
-            while (!queue.isEmpty())
+            while (!queue.IsEmpty())
             {
                 CrowdAgent ag = queue.Dequeue();
                 ag.corridor.optimizePathTopology(navQuery, m_filters[ag.option.queryFilterType], _config.maxTopologyOptimizationIterations);
diff --git a/src/DotRecast.Detour/NodeQueue.cs b/src/DotRecast.Detour/NodeQueue.cs
index be36268..50004a0 100644
--- a/src/DotRecast.Detour/NodeQueue.cs
+++ b/src/DotRecast.Detour/NodeQueue.cs
@@ -18,17 +18,19 @@ freely, subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+using DotRecast.Core;
+
 namespace DotRecast.Detour
 {
     using System.Collections.Generic;
 
     public class NodeQueue
     {
-        private readonly List<Node> m_heap = new List<Node>();
+        private readonly OrderedQueue<Node> m_heap = new OrderedQueue<Node>((n1, n2) => n1.total.CompareTo(n2.total));
 
         public int count()
         {
-            return m_heap.Count;
+            return m_heap.Count();
         }
 
         public void clear()
@@ -38,7 +40,7 @@ namespace DotRecast.Detour
 
         public Node top()
         {
-            return m_heap[0];
+            return m_heap.Top();
         }
 
         public Node pop()
@@ -50,8 +52,7 @@ namespace DotRecast.Detour
 
         public void push(Node node)
         {
-            m_heap.Add(node);
-            m_heap.Sort((x, y) => x.total.CompareTo(y.total));
+            m_heap.Enqueue(node);
         }
 
         public void modify(Node node)
@@ -62,7 +63,7 @@ namespace DotRecast.Detour
 
         public bool isEmpty()
         {
-            return 0 == m_heap.Count;
+            return 0 == m_heap.Count();
         }
     }
 }
\ No newline at end of file