Queries change

(cherry picked from commit 2397f23fc301f4c32408045ab55b19f88ee08599)

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

@@ -52,25 +52,12 @@ private bool GetNavMeshHeight(DtNavMeshQuery navMeshQuery, RcVec3f pt, float cs,
 
             RcVec3f halfExtents = new RcVec3f { X = cs, Y = heightRange, Z = cs };
             float maxHeight = pt.Y + heightRange;
-            RcAtomicBoolean found = new RcAtomicBoolean();
-            RcAtomicFloat minHeight = new RcAtomicFloat(pt.Y);
+            var query = new DtHeightSamplePolyQuery(navMeshQuery, pt, pt.Y, maxHeight);
+            navMeshQuery.QueryPolygons(pt, halfExtents, DtQueryNoOpFilter.Shared, ref query);
 
-            navMeshQuery.QueryPolygons(pt, halfExtents, DtQueryNoOpFilter.Shared, new DtCallbackPolyQuery((tile, poly, refs) =>
+            if (query.Found)
             {
-                var status = navMeshQuery.GetPolyHeight(refs, pt, out var h);
-                if (status.Succeeded())
-                {
-                    if (h > minHeight.Get() && h < maxHeight)
-                    {
-                        minHeight.Exchange(h);
-                        found.Set(true);
-                    }
-                }
-            }));
-
-            if (found.Get())
-            {
-                height = minHeight.Get();
+                height = query.MinHeight;
                 return true;
             }
 

src/DotRecast.Detour/DtCallbackPolyQuery.cs

@@ -2,7 +2,7 @@
 
 namespace DotRecast.Detour
 {
-    public class DtCallbackPolyQuery : IDtPolyQuery
+    public struct DtCallbackPolyQuery : IDtPolyQuery
     {
         private readonly Action<DtMeshTile, DtPoly, long> _callback;
 

src/DotRecast.Detour/DtFindNearestPolyQuery.cs

@@ -3,7 +3,7 @@
 
 namespace DotRecast.Detour
 {
-    public class DtFindNearestPolyQuery : IDtPolyQuery
+    public struct DtFindNearestPolyQuery : IDtPolyQuery
     {
         private readonly DtNavMeshQuery _query;
         private readonly RcVec3f _center;
@@ -18,6 +18,9 @@ public DtFindNearestPolyQuery(DtNavMeshQuery query, RcVec3f center)
             _center = center;
             _nearestDistanceSqr = float.MaxValue;
             _nearestPoint = center;
+
+            _nearestRef = default;
+            _overPoly = default;
         }
 
         public void Process(DtMeshTile tile, DtPoly[] poly, Span<long> refs, int count)

src/DotRecast.Detour/DtHeightSamplePolyQuery.cs

@@ -0,0 +1,45 @@
+using System;
+using DotRecast.Core;
+using DotRecast.Core.Numerics;
+
+namespace DotRecast.Detour
+{
+    public struct DtHeightSamplePolyQuery : IDtPolyQuery
+    {
+        private readonly DtNavMeshQuery _navMeshQuery;
+        private readonly RcVec3f _pt;
+        private readonly float _maxHeight;
+        public float MinHeight { get; private set; }
+        public bool Found { get; private set; }
+
+        public DtHeightSamplePolyQuery(DtNavMeshQuery navMeshQuery, RcVec3f pt, float minHeight, float maxHeight)
+        {
+            _navMeshQuery = navMeshQuery;
+            _pt = pt;
+            MinHeight = minHeight;
+            _maxHeight = maxHeight;
+            Found = default;
+        }
+
+        public void Process(DtMeshTile tile, Span<DtPoly> poly, Span<long> refs, int count)
+        {
+            for (int i = 0; i < count; i++)
+            {
+                ProcessSingle(refs[i]);
+            }
+        }
+
+        private void ProcessSingle(long refs)
+        {
+            var status = _navMeshQuery.GetPolyHeight(refs, _pt, out var h);
+            if (!status.Succeeded())
+                return;
+
+            if (!(h > MinHeight) || !(h < _maxHeight)) 
+                return;
+
+            MinHeight = h;
+            Found = true;
+        }
+    }
+}

src/DotRecast.Detour/DtNavMeshQuery.cs

@@ -589,7 +589,7 @@ public DtStatus FindNearestPoly(RcVec3f center, RcVec3f halfExtents, IDtQueryFil
 
             // Get nearby polygons from proximity grid.
             DtFindNearestPolyQuery query = new DtFindNearestPolyQuery(this, center);
-            DtStatus status = QueryPolygons(center, halfExtents, filter, query);
+            DtStatus status = QueryPolygons(center, halfExtents, filter, ref query);
             if (status.Failed())
             {
                 return status;
@@ -603,7 +603,8 @@ public DtStatus FindNearestPoly(RcVec3f center, RcVec3f halfExtents, IDtQueryFil
         }
 
         /// Queries polygons within a tile.
-        protected void QueryPolygonsInTile(DtMeshTile tile, RcVec3f qmin, RcVec3f qmax, IDtQueryFilter filter, IDtPolyQuery query)
+        protected void QueryPolygonsInTile<TQuery>(DtMeshTile tile, RcVec3f qmin, RcVec3f qmax, IDtQueryFilter filter, ref TQuery query)
+            where TQuery : IDtPolyQuery
         {
             const int batchSize = 32;
             Span<long> polyRefs = stackalloc long[batchSize];
@@ -759,7 +760,7 @@ public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents,
                 return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
 
             DtCollectPolysQuery collector = new DtCollectPolysQuery(polys, maxPolys);
-            DtStatus status = QueryPolygons(center, halfExtents, filter, collector);
+            DtStatus status = QueryPolygons(center, halfExtents, filter, ref collector);
             if (status.Failed())
                 return status;
 
@@ -781,7 +782,8 @@ public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents,
         ///  @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)
+        public DtStatus QueryPolygons<TQuery>(RcVec3f center, RcVec3f halfExtents, IDtQueryFilter filter, ref TQuery query)
+            where TQuery : IDtPolyQuery
         {
             if (!center.IsFinite() || !halfExtents.IsFinite() || null == filter)
             {
@@ -807,7 +809,7 @@ public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, IDtQueryFilte
                     int nneis = m_nav.GetTilesAt(x, y, neis, MAX_NEIS);
                     for (int j = 0; j < nneis; ++j)
                     {
-                        QueryPolygonsInTile(neis[j], bmin, bmax, filter, query);
+                        QueryPolygonsInTile(neis[j], bmin, bmax, filter, ref query);
                     }
                 }
             }