Merge branch 'risky_optimizations'

src/DotRecast.Detour.Crowd/DtCrowd.cs

@@ -130,7 +130,7 @@ namespace DotRecast.Detour.Crowd
         private readonly DtObstacleAvoidanceParams[] _obstacleQueryParams;
         private readonly DtObstacleAvoidanceQuery _obstacleQuery;
 
-        private DtProximityGrid _grid;
+        private readonly DtProximityGrid _grid;
 
         private int _maxPathResult;
         private readonly RcVec3f _agentPlacementHalfExtents;
@@ -175,6 +175,7 @@ namespace DotRecast.Detour.Crowd
             _agentIdx = new RcAtomicInteger(0);
             _agents = new Dictionary<int, DtCrowdAgent>();
             _activeAgents = new List<DtCrowdAgent>();
+            _grid = new DtProximityGrid(_config.maxAgentRadius * 3);
 
             // The navQuery is mostly used for local searches, no need for large node pool.
             SetNavMesh(nav);
@@ -565,14 +566,18 @@ namespace DotRecast.Detour.Crowd
             }
         }
 
+        private readonly RcSortedQueue<DtCrowdAgent> UpdateMoveRequest_queue = new RcSortedQueue<DtCrowdAgent>((a1, a2) => a2.targetReplanTime.CompareTo(a1.targetReplanTime));
+        private readonly List<long> UpdateMoveRequest_reqPath = new List<long>();
         private void UpdateMoveRequest(IList<DtCrowdAgent> agents, float dt)
         {
             using var timer = _telemetry.ScopedTimer(DtCrowdTimerLabel.UpdateMoveRequest);
 
-            RcSortedQueue<DtCrowdAgent> queue = new RcSortedQueue<DtCrowdAgent>((a1, a2) => a2.targetReplanTime.CompareTo(a1.targetReplanTime));
+            RcSortedQueue<DtCrowdAgent> queue = UpdateMoveRequest_queue;
+            queue.Clear();
 
             // Fire off new requests.
-            List<long> reqPath = new List<long>();
+            List<long> reqPath = UpdateMoveRequest_reqPath;
+            reqPath.Clear();
             for (var i = 0; i < agents.Count; i++)
             {
                 var ag = agents[i];
@@ -865,7 +870,7 @@ namespace DotRecast.Detour.Crowd
         {
             using var timer = _telemetry.ScopedTimer(DtCrowdTimerLabel.BuildProximityGrid);
 
-            _grid = new DtProximityGrid(_config.maxAgentRadius * 3);
+            _grid.Clear();
 
             for (var i = 0; i < agents.Count; i++)
             {

src/DotRecast.Detour.Crowd/DtProximityGrid.cs

@@ -22,6 +22,7 @@ using System;
 using System.Collections.Generic;
 using System.Linq;
 using System.Runtime.CompilerServices;
+using DotRecast.Core.Buffers;
 
 namespace DotRecast.Detour.Crowd
 {
@@ -30,12 +31,14 @@ namespace DotRecast.Detour.Crowd
         private readonly float _cellSize;
         private readonly float _invCellSize;
         private readonly Dictionary<long, List<DtCrowdAgent>> _items;
+        private readonly RcObjectPool<List<DtCrowdAgent>> _listPool;
 
         public DtProximityGrid(float cellSize)
         {
             _cellSize = cellSize;
             _invCellSize = 1.0f / cellSize;
             _items = new Dictionary<long, List<DtCrowdAgent>>();
+            _listPool = new RcObjectPool<List<DtCrowdAgent>>(() => new List<DtCrowdAgent>());
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -57,6 +60,8 @@ namespace DotRecast.Detour.Crowd
 
         public void Clear()
         {
+            foreach (var pair in _items)
+                _listPool.Return(pair.Value);
             _items.Clear();
         }
 
@@ -74,7 +79,7 @@ namespace DotRecast.Detour.Crowd
                     long key = CombineKey(x, y);
                     if (!_items.TryGetValue(key, out var ids))
                     {
-                        ids = new List<DtCrowdAgent>();
+                        ids = _listPool.Get();
                         _items.Add(key, ids);
                     }
 

src/DotRecast.Detour/DtNavMeshQuery.cs

@@ -1842,6 +1842,7 @@ namespace DotRecast.Detour
             return DtStatus.DT_SUCCESS | (straightPathCount >= maxStraightPath ? DtStatus.DT_BUFFER_TOO_SMALL : DtStatus.DT_STATUS_NOTHING);
         }
 
+        private readonly Queue<DtNode> MoveAlongSurface_queue = new Queue<DtNode>();
         /// @par
         ///
         /// This method is optimized for small delta movement and a small number of 
@@ -1897,8 +1898,8 @@ namespace DotRecast.Detour
             startNode.total = 0;
             startNode.id = startRef;
             startNode.flags = DtNodeFlags.DT_NODE_CLOSED;
-            LinkedList<DtNode> stack = new LinkedList<DtNode>();
-            stack.AddLast(startNode);
+            MoveAlongSurface_queue.Clear();
+            MoveAlongSurface_queue.Enqueue(startNode);
 
             RcVec3f bestPos = new RcVec3f();
             float bestDist = float.MaxValue;
@@ -1914,11 +1915,10 @@ namespace DotRecast.Detour
             const int MAX_NEIS = 8;
             Span<long> neis = stackalloc long[MAX_NEIS];
 
-            while (0 < stack.Count)
+            while (0 < MoveAlongSurface_queue.Count)
             {
                 // Pop front.
-                DtNode curNode = stack.First?.Value;
-                stack.RemoveFirst();
+                DtNode curNode = MoveAlongSurface_queue.Dequeue();
 
                 // Get poly and tile.
                 // The API input has been checked already, skip checking internal data.
@@ -2017,7 +2017,7 @@ namespace DotRecast.Detour
                             // Mark as the node as visited and push to queue.
                             neighbourNode.pidx = m_tinyNodePool.GetNodeIdx(curNode);
                             neighbourNode.flags |= DtNodeFlags.DT_NODE_CLOSED;
-                            stack.AddLast(neighbourNode);
+                            MoveAlongSurface_queue.Enqueue(neighbourNode);
                         }
                     }
                 }
@@ -2908,6 +2908,7 @@ namespace DotRecast.Detour
             return DtStatus.DT_SUCCESS;
         }
 
+        private readonly Queue<DtNode> FindLocalNeighbourhood_queue = new Queue<DtNode>();
         /// @par
         ///
         /// This method is optimized for a small search radius and small number of result 
@@ -2959,8 +2960,8 @@ namespace DotRecast.Detour
             startNode.pidx = 0;
             startNode.id = startRef;
             startNode.flags = DtNodeFlags.DT_NODE_CLOSED;
-            LinkedList<DtNode> stack = new LinkedList<DtNode>();
-            stack.AddLast(startNode);
+            FindLocalNeighbourhood_queue.Clear();
+            FindLocalNeighbourhood_queue.Enqueue(startNode);
 
             resultRef.Add(startNode.id);
             resultParent.Add(0L);
@@ -2970,11 +2971,11 @@ namespace DotRecast.Detour
             Span<float> pa = stackalloc float[m_nav.GetMaxVertsPerPoly() * 3];
             Span<float> pb = stackalloc float[m_nav.GetMaxVertsPerPoly() * 3];
 
-            while (0 < stack.Count)
+            while (0 < FindLocalNeighbourhood_queue.Count)
             {
                 // Pop front.
-                DtNode curNode = stack.First?.Value;
-                stack.RemoveFirst();
+                
+                DtNode curNode = FindLocalNeighbourhood_queue.Dequeue();
 
                 // Get poly and tile.
                 // The API input has been checked already, skip checking internal data.
@@ -3087,7 +3088,7 @@ namespace DotRecast.Detour
 
                     resultRef.Add(neighbourRef);
                     resultParent.Add(curRef);
-                    stack.AddLast(neighbourNode);
+                    FindLocalNeighbourhood_queue.Enqueue(neighbourNode);
                 }
             }