From 1894a5688994d169b531de442485f7bce115ed4f Mon Sep 17 00:00:00 2001
From: ikpil <ikpil0@gmail.com>
Date: Sun, 7 Jul 2024 12:55:51 +0900
Subject: [PATCH] Changed to efficiently stack nearby DtCrowdAgents in
 `DtCrowd.GetNeighbours()`

---
 CHANGELOG.md                                  |  1 +
 src/DotRecast.Detour.Crowd/DtCrowd.cs         | 16 ++++++++++++----
 src/DotRecast.Detour.Crowd/DtProximityGrid.cs |  6 +++---
 3 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 19725c9..537a55a 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -23,6 +23,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Changed vertCount and triCount to byte in `DtPolyDetail`
 - Changed `new float[]` to `stackalloc float[]` in `DtConvexConvexIntersections.Intersect()`
 - Changed agents management from list to dictionary in `DtCrowd`
+- Changed to efficiently stack nearby DtCrowdAgents in `DtCrowd.GetNeighbours()`
 
 ### Removed
 - Removed RcMeshDetails.VdistSq2(float[], float[])
diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs
index 184b718..da29d83 100644
--- a/src/DotRecast.Detour.Crowd/DtCrowd.cs
+++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs
@@ -280,6 +280,11 @@ namespace DotRecast.Detour.Crowd
             return ag;
         }
 
+        public DtCrowdAgent GetAgent(int idx)
+        {
+            return _agents.GetValueOrDefault(idx);
+        }
+
         // Add the agent from the crowd.
         public void AddAgent(DtCrowdAgent agent)
         {
@@ -901,12 +906,15 @@ namespace DotRecast.Detour.Crowd
         {
             result.Clear();
 
-            int MAX_NEIS = 32;
-            var ids = new DtCrowdAgent[MAX_NEIS];
-            int nids = grid.QueryItems(pos.X - range, pos.Z - range, pos.X + range, pos.Z + range, ids, ids.Length);
+            const int MAX_NEIS = 32;
+            Span<int> ids = stackalloc int[MAX_NEIS];
+            int nids = grid.QueryItems(pos.X - range, pos.Z - range,
+                pos.X + range, pos.Z + range,
+                ids, ids.Length);
+            
             for (int i = 0; i < nids; ++i)
             {
-                var ag = ids[i];
+                var ag = GetAgent(ids[i]);
                 if (ag == skip)
                 {
                     continue;
diff --git a/src/DotRecast.Detour.Crowd/DtProximityGrid.cs b/src/DotRecast.Detour.Crowd/DtProximityGrid.cs
index 82a6bb3..3cd182c 100644
--- a/src/DotRecast.Detour.Crowd/DtProximityGrid.cs
+++ b/src/DotRecast.Detour.Crowd/DtProximityGrid.cs
@@ -83,7 +83,7 @@ namespace DotRecast.Detour.Crowd
             }
         }
 
-        public int QueryItems(float minx, float miny, float maxx, float maxy, DtCrowdAgent[] ids, int maxIds)
+        public int QueryItems(float minx, float miny, float maxx, float maxy, Span<int> ids, int maxIds)
         {
             int iminx = (int)MathF.Floor(minx * _invCellSize);
             int iminy = (int)MathF.Floor(miny * _invCellSize);
@@ -110,7 +110,7 @@ namespace DotRecast.Detour.Crowd
                         // Check if the id exists already.
                         int end = n;
                         int i = 0;
-                        while (i != end && ids[i] != item)
+                        while (i != end && ids[i] != item.idx)
                         {
                             ++i;
                         }
@@ -118,7 +118,7 @@ namespace DotRecast.Detour.Crowd
                         // Item not found, add it.
                         if (i == n)
                         {
-                            ids[n++] = item;
+                            ids[n++] = item.idx;
 
                             if (n >= maxIds)
                                 return n;