Changed `IDtPolyQuery` interface to make `Process()` more versatile

CHANGELOG.md

@@ -13,7 +13,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Nothing
 
 ### Changed
-- Nothing
+- Changed `IDtPolyQuery` interface to make `Process()` more versatile
 
 ### Removed
 - Nothing

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

@@ -55,15 +55,19 @@ 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) =>
+            navMeshQuery.QueryPolygons(pt, halfExtents, DtQueryNoOpFilter.Shared, new PolyQueryInvoker((tile, poly, refs, count) =>
             {
-                var status = navMeshQuery.GetPolyHeight(refs, pt, out var h);
+                for (int i = 0; i < count; ++i)
-                if (status.Succeeded())
                 {
-                    if (h > minHeight.Get() && h < maxHeight)
+                    var status = navMeshQuery.GetPolyHeight(refs[i], pt, out var h);
+                    if (status.Succeeded())
                     {
-                        minHeight.Exchange(h);
+                        if (h > minHeight.Get() && h < maxHeight)
-                        found.Set(true);
+                        {
+                            minHeight.Exchange(h);
+                            found.Set(true);
+                            return;
+                        }
                     }
                 }
             }));

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

@@ -4,16 +4,16 @@ namespace DotRecast.Detour.Extras.Jumplink
 {
     public class PolyQueryInvoker : IDtPolyQuery
     {
-        public readonly Action<DtMeshTile, DtPoly, long> _callback;
+        private readonly Action<DtMeshTile, DtPoly[], long[], int> _callback;
 
-        public PolyQueryInvoker(Action<DtMeshTile, DtPoly, long> callback)
+        public PolyQueryInvoker(Action<DtMeshTile, DtPoly[], long[], int> callback)
         {
             _callback = callback;
         }
 
-        public void Process(DtMeshTile tile, DtPoly poly, long refs)
+        public void Process(DtMeshTile tile, DtPoly[] poly, long[] refs, int count)
         {
-            _callback?.Invoke(tile, poly, refs);
+            _callback?.Invoke(tile, poly, refs, count);
         }
     }
 }

src/DotRecast.Detour/DtFindNearestPolyQuery.cs

@@ -7,44 +7,49 @@ namespace DotRecast.Detour
     {
         private readonly DtNavMeshQuery _query;
         private readonly RcVec3f _center;
-        private long _nearestRef;
-        private RcVec3f _nearestPt;
-        private bool _overPoly;
         private float _nearestDistanceSqr;
+        private long _nearestRef;
+        private RcVec3f _nearestPoint;
+        private bool _overPoly;
 
         public DtFindNearestPolyQuery(DtNavMeshQuery query, RcVec3f center)
         {
-            this._query = query;
+            _query = query;
-            this._center = center;
+            _center = center;
             _nearestDistanceSqr = float.MaxValue;
-            _nearestPt = center;
+            _nearestPoint = center;
         }
 
-        public void Process(DtMeshTile tile, DtPoly poly, long refs)
+        public void Process(DtMeshTile tile, DtPoly[] poly, long[] refs, int count)
         {
-            // Find nearest polygon amongst the nearby polygons.
+            for (int i = 0; i < count; ++i)
-            _query.ClosestPointOnPoly(refs, _center, out var closestPtPoly, out var posOverPoly);
+            {
+                long polyRef = refs[i];
+                float d;
+                
+                // Find nearest polygon amongst the nearby polygons.
+                _query.ClosestPointOnPoly(polyRef, _center, out var closestPtPoly, out var posOverPoly);
 
-            // If a point is directly over a polygon and closer than
+                // If a point is directly over a polygon and closer than
-            // climb height, favor that instead of straight line nearest point.
+                // climb height, favor that instead of straight line nearest point.
-            float d = 0;
+                RcVec3f diff = RcVec3f.Subtract(_center, closestPtPoly);
-            RcVec3f diff = RcVec3f.Subtract(_center, closestPtPoly);
+                if (posOverPoly)
-            if (posOverPoly)
+                {
-            {
+                    d = MathF.Abs(diff.Y) - tile.data.header.walkableClimb;
-                d = MathF.Abs(diff.Y) - tile.data.header.walkableClimb;
+                    d = d > 0 ? d * d : 0;
-                d = d > 0 ? d * d : 0;
+                }
-            }
+                else
-            else
+                {
-            {
+                    d = diff.LengthSquared();
-                d = diff.LengthSquared();
+                }
-            }
 
-            if (d < _nearestDistanceSqr)
+                if (d < _nearestDistanceSqr)
-            {
+                {
-                _nearestPt = closestPtPoly;
+                    _nearestPoint = closestPtPoly;
-                _nearestDistanceSqr = d;
+                    _nearestDistanceSqr = d;
-                _nearestRef = refs;
+                    _nearestRef = polyRef;
-                _overPoly = posOverPoly;
+                    _overPoly = posOverPoly;
+                }
             }
         }
 
@@ -55,7 +60,7 @@ namespace DotRecast.Detour
 
         public RcVec3f NearestPt()
         {
-            return _nearestPt;
+            return _nearestPoint;
         }
 
         public bool OverPoly()

src/DotRecast.Detour/DtNavMeshQuery.cs

@@ -20,6 +20,7 @@ freely, subject to the following restrictions:
 
 using System;
 using System.Collections.Generic;
+using System.Reflection;
 using DotRecast.Core;
 using DotRecast.Core.Collections;
 using DotRecast.Core.Numerics;
@@ -575,15 +576,22 @@ namespace DotRecast.Detour
             return DtStatus.DT_SUCCESS;
         }
 
-        // FIXME: (PP) duplicate?
+        /// Queries polygons within a tile.
         protected void QueryPolygonsInTile(DtMeshTile tile, RcVec3f qmin, RcVec3f qmax, IDtQueryFilter filter, IDtPolyQuery query)
         {
+            const int batchSize = 32;
+            long[] polyRefs = new long[batchSize];
+            DtPoly[] polys = new DtPoly[batchSize];
+            int n = 0;
+
             if (tile.data.bvTree != null)
             {
                 int nodeIndex = 0;
+                int end = tile.data.header.bvNodeCount;
                 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];
@@ -604,7 +612,6 @@ namespace DotRecast.Detour
 
                 // Traverse tree
                 long @base = m_nav.GetPolyRefBase(tile);
-                int end = tile.data.header.bvNodeCount;
                 while (nodeIndex < end)
                 {
                     DtBVNode node = tile.data.bvTree[nodeIndex];
@@ -616,7 +623,18 @@ namespace DotRecast.Detour
                         long refs = @base | (long)node.i;
                         if (filter.PassFilter(refs, tile, tile.data.polys[node.i]))
                         {
-                            query.Process(tile, tile.data.polys[node.i], refs);
+                            polyRefs[n] = refs;
+                            polys[n] = tile.data.polys[node.i];
+                            
+                            if (n == batchSize - 1)
+                            {
+                                query.Process(tile, polys, polyRefs, batchSize);
+                                n = 0;
+                            }
+                            else
+                            {
+                                n++;
+                            }
                         }
                     }
 
@@ -645,6 +663,7 @@ namespace DotRecast.Detour
                         continue;
                     }
 
+                    // Must pass filter
                     long refs = @base | (long)i;
                     if (!filter.PassFilter(refs, tile, p))
                     {
@@ -664,10 +683,27 @@ namespace DotRecast.Detour
 
                     if (DtUtils.OverlapBounds(qmin, qmax, bmin, bmax))
                     {
-                        query.Process(tile, p, refs);
+                        polyRefs[n] = refs;
+                        polys[n] = p;
+
+                        if (n == batchSize - 1)
+                        {
+                            query.Process(tile, polys, polyRefs, batchSize);
+                            n = 0;
+                        }
+                        else
+                        {
+                            n++;
+                        }
                     }
                 }
             }
+
+            // Process the last polygons that didn't make a full batch.
+            if (n > 0)
+            {
+                query.Process(tile, polys, polyRefs, n);
+            }
         }
 
         /**

src/DotRecast.Detour/IDtPolyQuery.cs

@@ -1,7 +1,12 @@
 namespace DotRecast.Detour
 {
+    /// Provides custom polygon query behavior.
+    /// Used by dtNavMeshQuery::queryPolygons.
+    /// @ingroup detour
     public interface IDtPolyQuery
     {
-        void Process(DtMeshTile tile, DtPoly poly, long refs);
+        /// Called for each batch of unique polygons touched by the search area in dtNavMeshQuery::queryPolygons.
+        /// This can be called multiple times for a single query.
+        void Process(DtMeshTile tile, DtPoly[] poly, long[] refs, int count);
     }
 }

test/DotRecast.Detour.Test/FindNearestPolyTest.cs

@@ -44,11 +44,11 @@ public class FindNearestPolyTest : AbstractDetourTest
         {
             RcVec3f startPos = startPoss[i];
             var status = query.FindNearestPoly(startPos, extents, filter, out var nearestRef, out var nearestPt, out var _);
-            Assert.That(status.Succeeded(), Is.True);
+            Assert.That(status.Succeeded(), Is.True, $"index({i})");
-            Assert.That(nearestRef, Is.EqualTo(POLY_REFS[i]));
+            Assert.That(nearestRef, Is.EqualTo(POLY_REFS[i]), $"index({i})");
-            Assert.That(nearestPt.X, Is.EqualTo(POLY_POS[i].X).Within(0.001f));
+            Assert.That(nearestPt.X, Is.EqualTo(POLY_POS[i].X).Within(0.001f), $"index({i})");
-            Assert.That(nearestPt.Y, Is.EqualTo(POLY_POS[i].Y).Within(0.001f));
+            Assert.That(nearestPt.Y, Is.EqualTo(POLY_POS[i].Y).Within(0.001f), $"index({i})");
-            Assert.That(nearestPt.Z, Is.EqualTo(POLY_POS[i].Z).Within(0.001f));
+            Assert.That(nearestPt.Z, Is.EqualTo(POLY_POS[i].Z).Within(0.001f), $"index({i})");
         }
     }