Increase max query filter types count

src/DotRecast.Core/Collections/RcSortedQueue.cs

@@ -88,7 +88,7 @@ namespace DotRecast.Core.Collections
                 return false;
 
             //int idx = _items.BinarySearch(item, _comparer); // don't use this! Because reference types can be reused externally.
-            int idx = _items.FindLastIndex(x => item.Equals(x));
+            int idx = _items.LastIndexOf(item);
             if (0 > idx)
                 return false;
 

src/DotRecast.Core/Numerics/RcVec.cs

@@ -205,7 +205,7 @@ namespace DotRecast.Core.Numerics
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static float Dist2DSqr(RcVec3f p, float[] verts, int i)
+        public static float Dist2DSqr(RcVec3f p, Span<float> verts, int i)
         {
             float dx = verts[i] - p.X;
             float dz = verts[i + 2] - p.Z;

src/DotRecast.Detour.Crowd/DtCrowd.cs

@@ -425,11 +425,13 @@ namespace DotRecast.Detour.Crowd
             // Optimize path topology.
             UpdateTopologyOptimization(agents, dt);
 
+            #region Disabled for performance issues
             // Register agents to proximity grid.
-            BuildProximityGrid(agents);
+            // BuildProximityGrid(agents);
 
             // Get nearby navmesh segments and agents to collide with.
-            BuildNeighbours(agents);
+            // BuildNeighbours(agents);
+            #endregion
 
             // Find next corner to steer to.
             FindCorners(agents, debug);

src/DotRecast.Detour.Crowd/DtCrowdConfig.cs

@@ -25,7 +25,7 @@ namespace DotRecast.Detour.Crowd
 
         public int pathQueueSize = 32; // Max number of path requests in the queue
         public int maxFindPathIterations = 100; // Max number of sliced path finding iterations executed per update (used to handle longer paths and replans)
-        public int maxTargetFindPathIterations = 20; // Max number of sliced path finding iterations executed per agent to find the initial path to target
+        public int maxTargetFindPathIterations = 100; // Max number of sliced path finding iterations executed per agent to find the initial path to target
         public float topologyOptimizationTimeThreshold = 0.5f; // Min time between topology optimizations (in seconds)
         public int checkLookAhead = 10; // The number of polygons from the beginning of the corridor to check to ensure path validity
         public float targetReplanDelay = 1.0f; // Min time between target re-planning (in seconds)

src/DotRecast.Detour.Crowd/DtCrowdConst.cs

@@ -25,7 +25,7 @@ namespace DotRecast.Detour.Crowd
         /// @ingroup crowd
         /// @see dtQueryFilter, dtCrowd::GetFilter() dtCrowd::GetEditableFilter(),
         /// dtCrowdAgentParams::queryFilterType
-        public const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 16;
+        public const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 32;
         
         public const int MAX_ITERS_PER_UPDATE = 100;
         public const int MAX_PATHQUEUE_NODES = 4096;

src/DotRecast.Detour/DtConvexConvexIntersections.cs

@@ -27,7 +27,7 @@ namespace DotRecast.Detour
     {
         private const float EPSILON = 0.0001f;
 
-        public static float[] Intersect(Span<float> p, Span<float> q)
+        public static Span<float> Intersect(Span<float> p, Span<float> q, Span<float> buffer)
         {
             int n = p.Length / 3;
             int m = q.Length / 3;
@@ -95,7 +95,7 @@ namespace DotRecast.Detour
                 /* Special case: A & B parallel and separated. */
                 if (parallel && aHB < 0f && bHA < 0f)
                 {
-                    return null;
+                    return Span<float>.Empty;
                 }
                 /* Special case: A & B collinear. */
                 else if (parallel && MathF.Abs(aHB) < EPSILON && MathF.Abs(bHA) < EPSILON)
@@ -168,8 +168,9 @@ namespace DotRecast.Detour
                 return null;
             }
 
-            float[] copied = inters.Slice(0, ii).ToArray();
-            return copied;
+            Span<float> result = buffer.Slice(0, ii);
+            inters.Slice(0, ii).CopyTo(result);
+            return result;
         }
 
         private static int AddVertex(Span<float> inters, int ii, RcVec3f p)

src/DotRecast.Detour/DtNavMeshQuery.cs

@@ -233,6 +233,7 @@ namespace DotRecast.Detour
             IDtQueryFilter filter, IRcRand frand, IDtPolygonByCircleConstraint constraint,
             out long randomRef, out RcVec3f randomPt)
         {
+            const int MAX_VERT_BUFFER_SIZE = 128;
             randomRef = startRef;
             randomPt = centerPos;
 
@@ -266,9 +267,13 @@ namespace DotRecast.Detour
             float radiusSqr = maxRadius * maxRadius;
             float areaSum = 0.0f;
             
+            using RcRentedArray<float> polyVertsBuffer = RcRentedArray.Rent<float>(MAX_VERT_BUFFER_SIZE);
+            using RcRentedArray<float> randomPolyVertsBuffer = RcRentedArray.Rent<float>(MAX_VERT_BUFFER_SIZE);
+            using RcRentedArray<float> constrainedVertsBuffer = RcRentedArray.Rent<float>(MAX_VERT_BUFFER_SIZE);
+            
             DtPoly randomPoly = null;
             long randomPolyRef = 0;
-            float[] randomPolyVerts = null;
+            Span<float> randomPolyVerts = Span<float>.Empty;
 
             while (!m_openList.IsEmpty())
             {
@@ -286,14 +291,14 @@ namespace DotRecast.Detour
                 {
                     // Calc area of the polygon.
                     float polyArea = 0.0f;
-                    float[] polyVerts = new float[bestPoly.vertCount * 3];
+                    Span<float> polyVerts = polyVertsBuffer.AsSpan().Slice(0, bestPoly.vertCount * 3);
                     for (int j = 0; j < bestPoly.vertCount; ++j)
                     {
-                        RcArrays.Copy(bestTile.data.verts, bestPoly.verts[j] * 3, polyVerts, j * 3, 3);
+                        RcSpans.Copy(bestTile.data.verts, bestPoly.verts[j] * 3, polyVerts, j * 3, 3);
                     }
 
-                    float[] constrainedVerts = constraint.Apply(polyVerts, centerPos, maxRadius);
-                    if (constrainedVerts != null)
+                    Span<float> constrainedVerts = constraint.Apply(polyVerts, centerPos, maxRadius, constrainedVertsBuffer.AsSpan());
+                    if (!constrainedVerts.IsEmpty)
                     {
                         int vertCount = constrainedVerts.Length / 3;
                         for (int j = 2; j < vertCount; ++j)
@@ -311,7 +316,8 @@ namespace DotRecast.Detour
                         {
                             randomPoly = bestPoly;
                             randomPolyRef = bestRef;
-                            randomPolyVerts = constrainedVerts;
+                            randomPolyVerts = randomPolyVertsBuffer.AsSpan().Slice(0, constrainedVerts.Length);
+                            constrainedVerts.CopyTo(randomPolyVerts);
                         }
                     }
                 }
@@ -1631,20 +1637,20 @@ namespace DotRecast.Detour
             DtStatus stat = DtStatus.DT_STATUS_NOTHING;
 
             // TODO: Should this be callers responsibility?
-            var closestStartPosRes = ClosestPointOnPolyBoundary(path[0], startPos, out var closestStartPos);
-            if (closestStartPosRes.Failed())
-            {
-                return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
-            }
-
-            var closestEndPosRes = ClosestPointOnPolyBoundary(path[pathSize - 1], endPos, out var closestEndPos);
-            if (closestEndPosRes.Failed())
-            {
-                return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
-            }
+            // var closestStartPosRes = ClosestPointOnPolyBoundary(path[0], startPos, out var closestStartPos);
+            // if (closestStartPosRes.Failed())
+            // {
+            //     return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
+            // }
+            //
+            // var closestEndPosRes = ClosestPointOnPolyBoundary(path[pathSize - 1], endPos, out var closestEndPos);
+            // if (closestEndPosRes.Failed())
+            // {
+            //     return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
+            // }
 
             // Add start point.
-            stat = AppendVertex(closestStartPos, DtStraightPathFlags.DT_STRAIGHTPATH_START, path[0], straightPath, ref straightPathCount, maxStraightPath);
+            stat = AppendVertex(startPos, DtStraightPathFlags.DT_STRAIGHTPATH_START, path[0], straightPath, ref straightPathCount, maxStraightPath);
             if (!stat.InProgress())
             {
                 return stat;
@@ -1652,7 +1658,7 @@ namespace DotRecast.Detour
 
             if (pathSize > 1)
             {
-                RcVec3f portalApex = closestStartPos;
+                RcVec3f portalApex = startPos;
                 RcVec3f portalLeft = portalApex;
                 RcVec3f portalRight = portalApex;
                 int apexIndex = 0;
@@ -1681,21 +1687,21 @@ namespace DotRecast.Detour
                         {
                             // Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
                             // Clamp the end point to path[i], and return the path so far.
-                            var cpStatus = ClosestPointOnPolyBoundary(path[i], endPos, out closestEndPos);
-                            if (cpStatus.Failed())
-                            {
-                                return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
-                            }
+                            // var cpStatus = ClosestPointOnPolyBoundary(path[i], endPos, out closestEndPos);
+                            // if (cpStatus.Failed())
+                            // {
+                            //     return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
+                            // }
 
                             // Append portals along the current straight path segment.
                             if ((options & (DtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS | DtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
                             {
                                 // Ignore status return value as we're just about to return anyway.
-                                AppendPortals(apexIndex, i, closestEndPos, path, straightPath, ref straightPathCount, maxStraightPath, options);
+                                AppendPortals(apexIndex, i, endPos, path, straightPath, ref straightPathCount, maxStraightPath, options);
                             }
 
                             // Ignore status return value as we're just about to return anyway.
-                            AppendVertex(closestEndPos, 0, path[i], straightPath, ref straightPathCount, maxStraightPath);
+                            AppendVertex(endPos, 0, path[i], straightPath, ref straightPathCount, maxStraightPath);
 
                             return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM | (straightPathCount >= maxStraightPath ? DtStatus.DT_BUFFER_TOO_SMALL : DtStatus.DT_STATUS_NOTHING);
                         }
@@ -1713,8 +1719,8 @@ namespace DotRecast.Detour
                     else
                     {
                         // End of the path.
-                        left = closestEndPos;
-                        right = closestEndPos;
+                        left = endPos;
+                        right = endPos;
                         toType = DtPolyTypes.DT_POLYTYPE_GROUND;
                     }
 
@@ -1834,7 +1840,7 @@ namespace DotRecast.Detour
                 // Append portals along the current straight path segment.
                 if ((options & (DtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS | DtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
                 {
-                    stat = AppendPortals(apexIndex, pathSize - 1, closestEndPos, path, straightPath, ref straightPathCount, maxStraightPath, options);
+                    stat = AppendPortals(apexIndex, pathSize - 1, endPos, path, straightPath, ref straightPathCount, maxStraightPath, options);
                     if (!stat.InProgress())
                     {
                         return stat;
@@ -1843,7 +1849,7 @@ namespace DotRecast.Detour
             }
 
             // Ignore status return value as we're just about to return anyway.
-            AppendVertex(closestEndPos, DtStraightPathFlags.DT_STRAIGHTPATH_END, 0, straightPath, ref straightPathCount, maxStraightPath);
+            AppendVertex(endPos, DtStraightPathFlags.DT_STRAIGHTPATH_END, 0, straightPath, ref straightPathCount, maxStraightPath);
             return DtStatus.DT_SUCCESS | (straightPathCount >= maxStraightPath ? DtStatus.DT_BUFFER_TOO_SMALL : DtStatus.DT_STATUS_NOTHING);
         }
 
@@ -2071,19 +2077,11 @@ namespace DotRecast.Detour
             fromType = 0;
             toType = 0;
 
-            var status = m_nav.GetTileAndPolyByRef(from, out var fromTile, out var fromPoly);
-            if (status.Failed())
-            {
-                return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
-            }
+            m_nav.GetTileAndPolyByRefUnsafe(from, out var fromTile, out var fromPoly);
 
             fromType = fromPoly.GetPolyType();
 
-            status = m_nav.GetTileAndPolyByRef(to, out var toTile, out var toPoly);
-            if (status.Failed())
-            {
-                return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
-            }
+            m_nav.GetTileAndPolyByRefUnsafe(to, out var toTile, out var toPoly);
 
             toType = toPoly.GetPolyType();
 

src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs

@@ -1,3 +1,4 @@
+using System;
 using DotRecast.Core.Numerics;
 
 namespace DotRecast.Detour
@@ -10,9 +11,11 @@ namespace DotRecast.Detour
         {
         }
 
-        public float[] Apply(float[] polyVerts, RcVec3f circleCenter, float radius)
+        public Span<float> Apply(Span<float> polyVerts, RcVec3f circleCenter, float radius, Span<float> resultBuffer)
         {
-            return polyVerts;
+            var result = resultBuffer.Slice(0, polyVerts.Length);
+            polyVerts.CopyTo(result);
+            return result;
         }
     }
 }

src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs

@@ -39,8 +39,7 @@ namespace DotRecast.Detour
             }
         }
 
-
-        public float[] Apply(float[] verts, RcVec3f center, float radius)
+        public Span<float> Apply(Span<float> verts, RcVec3f center, float radius, Span<float> resultBuffer)
         {
             float radiusSqr = radius * radius;
             int outsideVertex = -1;
@@ -56,19 +55,30 @@ namespace DotRecast.Detour
             if (outsideVertex == -1)
             {
                 // polygon inside circle
-                return verts;
+                var result = resultBuffer.Slice(0, verts.Length);
+                verts.CopyTo(result);
+                return result;
             }
 
             Span<float> qCircle = stackalloc float[UnitCircle.Length];
             ScaleCircle(UnitCircle, center, radius, qCircle);
-            float[] intersection = DtConvexConvexIntersections.Intersect(verts, qCircle);
-            if (intersection == null && DtUtils.PointInPolygon(center, verts, verts.Length / 3))
+            Span<float> intersection = DtConvexConvexIntersections.Intersect(verts, qCircle, resultBuffer);
+            if (intersection.IsEmpty && DtUtils.PointInPolygon(center, verts, verts.Length / 3))
             {
                 // circle inside polygon
-                return qCircle.ToArray();
+                var result = resultBuffer.Slice(0, qCircle.Length);
+                qCircle.CopyTo(result);
+                return result;
             }
 
-            return intersection;
+            if(!intersection.IsEmpty)
+            {
+                var result = resultBuffer.Slice(0, intersection.Length);
+                // No need to copy, data is already in buffer
+                return result;
+            }
+            
+            return Span<float>.Empty;
         }
     }
 }

src/DotRecast.Detour/IDtPolygonByCircleConstraint.cs

@@ -24,6 +24,6 @@ namespace DotRecast.Detour
 {
     public interface IDtPolygonByCircleConstraint
     {
-        float[] Apply(float[] polyVerts, RcVec3f circleCenter, float radius);
+        Span<float> Apply(Span<float> polyVerts, RcVec3f circleCenter, float radius, Span<float> resultBuffer);
     }
 }

test/DotRecast.Detour.Test/ConvexConvexIntersectionTest.cs

@@ -17,6 +17,7 @@ freely, subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
 using NUnit.Framework;
 
 namespace DotRecast.Detour.Test;
@@ -29,9 +30,10 @@ public class ConvexConvexIntersectionTest
     {
         float[] p = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
         float[] q = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
-        float[] intersection = DtConvexConvexIntersections.Intersect(p, q);
+        float[] buffer = new float[128];
+        Span<float> intersection = DtConvexConvexIntersections.Intersect(p, q, buffer);
         Assert.That(intersection.Length, Is.EqualTo(5 * 3));
-        Assert.That(intersection, Is.EqualTo(p));
+        Assert.That(intersection.ToArray(), Is.EquivalentTo(p));
     }
 
     [Test]
@@ -39,8 +41,9 @@ public class ConvexConvexIntersectionTest
     {
         float[] p = { -5, 0, -5, -5, 0, 4, 1, 0, 4, 1, 0, -5 };
         float[] q = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
-        float[] intersection = DtConvexConvexIntersections.Intersect(p, q);
+        float[] buffer = new float[128];
+        Span<float> intersection = DtConvexConvexIntersections.Intersect(p, q, buffer);
         Assert.That(intersection.Length, Is.EqualTo(5 * 3));
-        Assert.That(intersection, Is.EqualTo(new[] { 1, 0, 3, 1, 0, -3.4f, -2, 0, -4, -4, 0, 0, -3, 0, 3 }));
+        Assert.That(intersection.ToArray(), Is.EquivalentTo(new[] { 1, 0, 3, 1, 0, -3.4f, -2, 0, -4, -4, 0, 0, -3, 0, 3 }));
     }
 }

test/DotRecast.Detour.Test/PolygonByCircleConstraintTest.cs

@@ -17,6 +17,7 @@ freely, subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+using System;
 using DotRecast.Core.Numerics;
 using NUnit.Framework;
 
@@ -32,9 +33,12 @@ public class PolygonByCircleConstraintTest
     {
         float[] polygon = { -2, 0, 2, 2, 0, 2, 2, 0, -2, -2, 0, -2 };
         RcVec3f center = new RcVec3f(1, 0, 1);
-        float[] constrained = _constraint.Apply(polygon, center, 6);
+        var radius = 6;
         
-        Assert.That(constrained, Is.EqualTo(polygon));
+        float[] buffer = new float[128];
+        Span<float> constrained = _constraint.Apply(polygon, center, radius, buffer);
+
+        Assert.That(constrained.ToArray(), Is.EquivalentTo(polygon));
     }
 
     [Test]
@@ -43,10 +47,13 @@ public class PolygonByCircleConstraintTest
         int expectedSize = 21;
         float[] polygon = { -2, 0, 2, 2, 0, 2, 2, 0, -2, -2, 0, -2 };
         RcVec3f center = new RcVec3f(2, 0, 0);
+        var radius = 3;
+
+        float[] buffer = new float[128];
+        Span<float> constrained = _constraint.Apply(polygon, center, radius, buffer);
 
-        float[] constrained = _constraint.Apply(polygon, center, 3);
         Assert.That(constrained.Length, Is.EqualTo(expectedSize));
-        Assert.That(constrained, Is.SupersetOf(new[] { 2f, 0f, 2f, 2f, 0f, -2f }));
+        Assert.That(constrained.ToArray(), Is.SupersetOf(new[] { 2f, 0f, 2f, 2f, 0f, -2f }));
     }
 
     [Test]
@@ -55,7 +62,10 @@ public class PolygonByCircleConstraintTest
         int expectedSize = 12 * 3;
         float[] polygon = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
         RcVec3f center = new RcVec3f(-1, 0, -1);
-        float[] constrained = _constraint.Apply(polygon, center, 2);
+        var radius = 2;
+        
+        float[] buffer = new float[128];
+        Span<float> constrained = _constraint.Apply(polygon, center, radius, buffer);
 
         Assert.That(constrained.Length, Is.EqualTo(expectedSize));
 
@@ -73,10 +83,13 @@ public class PolygonByCircleConstraintTest
         int expectedSize = 9 * 3;
         float[] polygon = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
         RcVec3f center = new RcVec3f(-2, 0, -1);
-        float[] constrained = _constraint.Apply(polygon, center, 3);
+        var radius = 3;
+        
+        float[] buffer = new float[128];
+        Span<float> constrained = _constraint.Apply(polygon, center, radius, buffer);
 
         Assert.That(constrained.Length, Is.EqualTo(expectedSize));
-        Assert.That(constrained, Is.SupersetOf(new[] { -2f, 0f, -4f, -4f, 0f, 0f, -3.4641016f, 0.0f, 1.60769534f, -2.0f, 0.0f, 2.0f }));
+        Assert.That(constrained.ToArray(), Is.SupersetOf(new[] { -2f, 0f, -4f, -4f, 0f, 0f, -3.4641016f, 0.0f, 1.60769534f, -2.0f, 0.0f, 2.0f }));
     }
 
     [Test]
@@ -85,9 +98,12 @@ public class PolygonByCircleConstraintTest
         int expectedSize = 7 * 3;
         float[] polygon = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 };
         RcVec3f center = new RcVec3f(4, 0, 0);
-        float[] constrained = _constraint.Apply(polygon, center, 4);
+        var radius = 4;
+        
+        float[] buffer = new float[128];
+        Span<float> constrained = _constraint.Apply(polygon, center, radius, buffer);
 
         Assert.That(constrained.Length, Is.EqualTo(expectedSize));
-        Assert.That(constrained, Is.SupersetOf(new[] { 1.53589869f, 0f, 3f, 2f, 0f, 3f, 3f, 0f, -3f }));
+        Assert.That(constrained.ToArray(), Is.SupersetOf(new[] { 1.53589869f, 0f, 3f, 2f, 0f, 3f, 3f, 0f, -3f }));
     }
 }