Added `DtCollectPolysQuery` and `FindCollectPolyTest`

CHANGELOG.md

@@ -7,13 +7,14 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 ## [Unreleased] - yyyy-mm-dd
 
 ### Added
-- Nothing
+- Added `DtCollectPolysQuery` and `FindCollectPolyTest`
 
 ### Fixed
 - Nothing
 
 ### Changed
 - Changed `IDtPolyQuery` interface to make `Process()` more versatile
+- Changed `PolyQueryInvoker` to `DtActionPolyQuery`
 
 ### Removed
 - Nothing

src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs

@@ -55,7 +55,7 @@ namespace DotRecast.Detour.Extras.Jumplink
             RcAtomicBoolean found = new RcAtomicBoolean();
             RcAtomicFloat minHeight = new RcAtomicFloat(pt.Y);
 
-            navMeshQuery.QueryPolygons(pt, halfExtents, DtQueryNoOpFilter.Shared, new PolyQueryInvoker((tile, poly, refs, count) =>
+            navMeshQuery.QueryPolygons(pt, halfExtents, DtQueryNoOpFilter.Shared, new DtCallbackPolyQuery((tile, poly, refs, count) =>
             {
                 for (int i = 0; i < count; ++i)
                 {

src/DotRecast.Detour/DtCallbackPolyQuery.cs

@@ -1,12 +1,12 @@
 using System;
 
-namespace DotRecast.Detour.Extras.Jumplink
+namespace DotRecast.Detour
 {
-    public class PolyQueryInvoker : IDtPolyQuery
+    public class DtCallbackPolyQuery : IDtPolyQuery
     {
         private readonly Action<DtMeshTile, DtPoly[], long[], int> _callback;
 
-        public PolyQueryInvoker(Action<DtMeshTile, DtPoly[], long[], int> callback)
+        public DtCallbackPolyQuery(Action<DtMeshTile, DtPoly[], long[], int> callback)
         {
             _callback = callback;
         }

src/DotRecast.Detour/DtCollectPolysQuery.cs

@@ -0,0 +1,42 @@
+using DotRecast.Core;
+
+namespace DotRecast.Detour
+{
+    public class DtCollectPolysQuery : IDtPolyQuery
+    {
+        private long[] m_polys;
+        private int m_maxPolys;
+        private int m_numCollected;
+        private bool m_overflow;
+
+        public DtCollectPolysQuery(long[] polys, int maxPolys)
+        {
+            m_polys = polys;
+            m_maxPolys = maxPolys;
+        }
+
+        public int NumCollected()
+        {
+            return m_numCollected;
+        }
+
+        public bool Overflowed()
+        {
+            return m_overflow;
+        }
+
+        public void Process(DtMeshTile tile, DtPoly[] poly, long[] refs, int count)
+        {
+            int numLeft = m_maxPolys - m_numCollected;
+            int toCopy = count;
+            if (toCopy > numLeft)
+            {
+                m_overflow = true;
+                toCopy = numLeft;
+            }
+
+            RcSpans.Copy<long>(refs, 0, m_polys, m_numCollected, toCopy);
+            m_numCollected += toCopy;
+        }
+    }
+}

src/DotRecast.Detour/DtNavMeshQuery.cs

@@ -591,7 +591,7 @@ namespace DotRecast.Detour
                 var tbmin = tile.data.header.bmin;
                 var tbmax = tile.data.header.bmax;
                 float qfac = tile.data.header.bvQuantFactor;
-                
+
                 // Calculate quantized box
                 Span<int> bmin = stackalloc int[3];
                 Span<int> bmax = stackalloc int[3];
@@ -625,7 +625,7 @@ namespace DotRecast.Detour
                         {
                             polyRefs[n] = refs;
                             polys[n] = tile.data.polys[node.i];
-                            
+
                             if (n == batchSize - 1)
                             {
                                 query.Process(tile, polys, polyRefs, batchSize);
@@ -706,19 +706,54 @@ namespace DotRecast.Detour
             }
         }
 
-        /**
-     * Finds polygons that overlap the search box.
-     *
-     * If no polygons are found, the function will return with a polyCount of zero.
-     *
-     * @param center
-     *            The center of the search box. [(x, y, z)]
-     * @param halfExtents
-     *            The search distance along each axis. [(x, y, z)]
-     * @param filter
-     *            The polygon filter to apply to the query.
-     * @return The reference ids of the polygons that overlap the query box.
-     */
+        /// @par 
+        ///
+        /// If no polygons are found, the function will return #DT_SUCCESS with a
+        /// @p polyCount of zero.
+        ///
+        /// If @p polys is too small to hold the entire result set, then the array will 
+        /// be filled to capacity. The method of choosing which polygons from the 
+        /// full set are included in the partial result set is undefined.
+        ///
+        /// Finds polygons that overlap the search box.
+        ///  @param[in]		center		The center of the search box. [(x, y, z)]
+        ///  @param[in]		halfExtents		The search distance along each axis. [(x, y, z)]
+        ///  @param[in]		filter		The polygon filter to apply to the query.
+        ///  @param[out]	polys		The reference ids of the polygons that overlap the query box.
+        ///  @param[out]	polyCount	The number of polygons in the search result.
+        ///  @param[in]		maxPolys	The maximum number of polygons the search result can hold.
+        /// @returns The status flags for the query.
+        public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents,
+            IDtQueryFilter filter,
+            long[] polys, out int polyCount, int maxPolys)
+        {
+            polyCount = 0;
+            if (null == polys || maxPolys < 0)
+                return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
+
+            DtCollectPolysQuery collector = new DtCollectPolysQuery(polys, maxPolys);
+            DtStatus status = QueryPolygons(center, halfExtents, filter, collector);
+            if (status.Failed())
+                return status;
+
+            polyCount = collector.NumCollected();
+            return collector.Overflowed()
+                ? DtStatus.DT_SUCCESS | DtStatus.DT_BUFFER_TOO_SMALL
+                : DtStatus.DT_SUCCESS;
+        }
+
+        /// @par 
+        ///
+        /// The query will be invoked with batches of polygons. Polygons passed
+        /// to the query have bounding boxes that overlap with the center and halfExtents
+        /// passed to this function. The dtPolyQuery::process function is invoked multiple
+        /// times until all overlapping polygons have been processed.
+        ///
+        /// Finds polygons that overlap the search box.
+        ///  @param[in]		center		The center of the search box. [(x, y, z)]
+        ///  @param[in]		halfExtents		The search distance along each axis. [(x, y, z)]
+        ///  @param[in]		filter		The polygon filter to apply to the query.
+        ///  @param[in]		query		The query. Polygons found will be batched together and passed to this query.
         public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, IDtQueryFilter filter, IDtPolyQuery query)
         {
             if (!center.IsFinite() || !halfExtents.IsFinite() || null == filter)

test/DotRecast.Detour.Test/FindCollectPolyTest.cs

@@ -0,0 +1,68 @@
+using System;
+using System.Linq;
+using DotRecast.Core.Numerics;
+using NUnit.Framework;
+
+namespace DotRecast.Detour.Test;
+
+public class FindCollectPolyTest : AbstractDetourTest
+{
+    private static readonly long[][] POLY_REFS =
+    {
+        new long[]
+        {
+            281474976710697L,
+            281474976710695L,
+            281474976710696L,
+            281474976710691L,
+        },
+        new long[]
+        {
+            281474976710769L,
+            281474976710773L,
+        },
+        new long[]
+        {
+            281474976710676L,
+            281474976710678L,
+            281474976710679L,
+            281474976710674L,
+            281474976710677L,
+            281474976710683L,
+            281474976710680L,
+            281474976710684L,
+        },
+
+        new long[]
+        {
+            281474976710748L,
+            281474976710753L,
+            281474976710752L,
+            281474976710750L,
+        },
+
+        new long[]
+        {
+            281474976710736L,
+            281474976710733L,
+            281474976710735L,
+        }
+    };
+
+    [Test]
+    public void TestFindNearestPoly()
+    {
+        IDtQueryFilter filter = new DtQueryDefaultFilter();
+        RcVec3f extents = new RcVec3f(2, 4, 2);
+        var polys = new long[32];
+        for (int i = 0; i < startRefs.Length; i++)
+        {
+            Array.Fill(polys, 0);
+            RcVec3f startPos = startPoss[i];
+            var status = query.QueryPolygons(startPos, extents, filter, polys, out var polyCount, 32);
+            Assert.That(status.Succeeded(), Is.True, $"index({i})");
+            Assert.That(polyCount, Is.EqualTo(POLY_REFS[i].Length), $"index({i})");
+            Assert.That(polys.AsSpan(0, polyCount).ToArray(), Is.EqualTo(POLY_REFS[i]), $"index({i})");
+        }
+    }
+}

test/DotRecast.Detour.Test/FindNearestPolyTest.cs

@@ -21,10 +21,12 @@ using NUnit.Framework;
 
 namespace DotRecast.Detour.Test;
 
-
 public class FindNearestPolyTest : AbstractDetourTest
 {
-    private static readonly long[] POLY_REFS = { 281474976710696L, 281474976710773L, 281474976710680L, 281474976710753L, 281474976710733L };
+    private static readonly long[] POLY_REFS =
+    {
+        281474976710696L, 281474976710773L, 281474976710680L, 281474976710753L, 281474976710733L
+    };
 
     private static readonly RcVec3f[] POLY_POS =
     {