diff --git a/src/DotRecast.Core/NodeQueue.cs b/src/DotRecast.Core/NodeQueue.cs
index eea103d..4fffb7f 100644
--- a/src/DotRecast.Core/NodeQueue.cs
+++ b/src/DotRecast.Core/NodeQueue.cs
@@ -24,13 +24,13 @@ namespace DotRecast.Core
 {
     using System.Collections.Generic;
 
-    public class OrderedQueue<T>
+    public class SortedQueue<T>
     {
         private bool _dirty;
         private readonly List<T> _items;
         private readonly Comparison<T> _comparison;
 
-        public OrderedQueue(Comparison<T> comparison)
+        public SortedQueue(Comparison<T> comparison)
         {
             _items = new List<T>();
             _comparison = (x, y) => comparison.Invoke(x, y) * -1; // reverse
diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs
index 755de6d..e749b27 100644
--- a/src/DotRecast.Detour.Crowd/Crowd.cs
+++ b/src/DotRecast.Detour.Crowd/Crowd.cs
@@ -563,7 +563,7 @@ namespace DotRecast.Detour.Crowd
         {
             _telemetry.start("updateMoveRequest");
 
-            OrderedQueue<CrowdAgent> queue = new OrderedQueue<CrowdAgent>((a1, a2) => a2.targetReplanTime.CompareTo(a1.targetReplanTime));
+            SortedQueue<CrowdAgent> queue = new SortedQueue<CrowdAgent>((a1, a2) => a2.targetReplanTime.CompareTo(a1.targetReplanTime));
 
             // Fire off new requests.
             foreach (CrowdAgent ag in agents)
@@ -823,7 +823,7 @@ namespace DotRecast.Detour.Crowd
         {
             _telemetry.start("updateTopologyOptimization");
 
-            OrderedQueue<CrowdAgent> queue = new OrderedQueue<CrowdAgent>((a1, a2) => a2.topologyOptTime.CompareTo(a1.topologyOptTime));
+            SortedQueue<CrowdAgent> queue = new SortedQueue<CrowdAgent>((a1, a2) => a2.topologyOptTime.CompareTo(a1.topologyOptTime));
 
             foreach (CrowdAgent ag in agents)
             {
diff --git a/src/DotRecast.Detour.Crowd/PathQueue.cs b/src/DotRecast.Detour.Crowd/PathQueue.cs
index 381b4b3..61fc474 100644
--- a/src/DotRecast.Detour.Crowd/PathQueue.cs
+++ b/src/DotRecast.Detour.Crowd/PathQueue.cs
@@ -47,6 +47,7 @@ namespace DotRecast.Detour.Crowd
                 {
                     break;
                 }
+                queue.RemoveFirst();
 
                 // Handle query start.
                 if (q.result.status == null)
diff --git a/src/DotRecast.Detour/NodeQueue.cs b/src/DotRecast.Detour/NodeQueue.cs
index 50004a0..8be600d 100644
--- a/src/DotRecast.Detour/NodeQueue.cs
+++ b/src/DotRecast.Detour/NodeQueue.cs
@@ -26,7 +26,7 @@ namespace DotRecast.Detour
 
     public class NodeQueue
     {
-        private readonly OrderedQueue<Node> m_heap = new OrderedQueue<Node>((n1, n2) => n1.total.CompareTo(n2.total));
+        private readonly SortedQueue<Node> m_heap = new SortedQueue<Node>((n1, n2) => n1.total.CompareTo(n2.total));
 
         public int count()
         {
diff --git a/test/DotRecast.Detour.Crowd.Test/Crowd4Test.cs b/test/DotRecast.Detour.Crowd.Test/Crowd4Test.cs
index 8508bba..8ea7b65 100644
--- a/test/DotRecast.Detour.Crowd.Test/Crowd4Test.cs
+++ b/test/DotRecast.Detour.Crowd.Test/Crowd4Test.cs
@@ -356,26 +356,14 @@ public class Crowd4Test : AbstractCrowdTest
         setMoveTarget(endPoss[0], false);
         for (int i = 0; i < EXPECTED_A1Q2T.Length; i++)
         {
-            // if (i == 37)
-            // {
-            //     int a = 3;
-            // }
-
             crowd.update(1 / 5f, null);
             CrowdAgent ag = agents[2];
             Assert.That(ag.npos[0], Is.EqualTo(EXPECTED_A1Q2T[i][0]).Within(0.00001f), $"{i} - {ag.npos[0]} {EXPECTED_A1Q2T[i][0]}");
-            Console.WriteLine($"{i} - {ag.npos[0]} {EXPECTED_A1Q2T[i][0]}");
             Assert.That(ag.npos[1], Is.EqualTo(EXPECTED_A1Q2T[i][1]).Within(0.00001f), $"{i} - {ag.npos[1]} {EXPECTED_A1Q2T[i][1]}");
-            Console.WriteLine($"{i} - {ag.npos[1]} {EXPECTED_A1Q2T[i][1]}");
             Assert.That(ag.npos[2], Is.EqualTo(EXPECTED_A1Q2T[i][2]).Within(0.00001f), $"{i} - {ag.npos[2]} {EXPECTED_A1Q2T[i][2]}");
-            Console.WriteLine($"{i} - {ag.npos[2]} {EXPECTED_A1Q2T[i][2]}");
             Assert.That(ag.nvel[0], Is.EqualTo(EXPECTED_A1Q2T[i][3]).Within(0.00001f), $"{i} - {ag.nvel[0]} {EXPECTED_A1Q2T[i][3]}");
-            Console.WriteLine($"{i} - {ag.nvel[0]} {EXPECTED_A1Q2T[i][3]}");
             Assert.That(ag.nvel[1], Is.EqualTo(EXPECTED_A1Q2T[i][4]).Within(0.00001f), $"{i} - {ag.nvel[1]} {EXPECTED_A1Q2T[i][4]}");
-            Console.WriteLine($"{i} - {ag.nvel[1]} {EXPECTED_A1Q2T[i][4]}");
             Assert.That(ag.nvel[2], Is.EqualTo(EXPECTED_A1Q2T[i][5]).Within(0.00001f), $"{i} - {ag.nvel[2]} {EXPECTED_A1Q2T[i][5]}");
-            Console.WriteLine($"{i} - {ag.nvel[2]} {EXPECTED_A1Q2T[i][5]}");
-            Thread.Sleep(1);
         }
     }
 }
\ No newline at end of file