From c7c6e53d61852a6553e90f7ee684c8e7c13b69dc Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 11:32:38 +0300 Subject: [PATCH 01/22] Made list in DtPathCorridor.cs readonly --- src/DotRecast.Detour.Crowd/DtPathCorridor.cs | 23 +++++++++++-------- src/DotRecast.Detour/DtPathUtils.cs | 15 +++++++----- .../Tools/RcTestNavMeshTool.cs | 2 +- 3 files changed, 24 insertions(+), 16 deletions(-) diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs index 37787ae0..d3b04c64 100644 --- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs @@ -33,7 +33,7 @@ public class DtPathCorridor private RcVec3f m_pos; private RcVec3f m_target; - private List m_path; + private List m_path = new List(); private int m_npath; private int m_maxPath; @@ -89,7 +89,8 @@ public DtPathCorridor() /// @return True if the initialization succeeded. public bool Init(int maxPath) { - m_path = new List(maxPath); + if (m_path.Capacity < maxPath) + m_path.Capacity = maxPath; m_npath = 0; m_maxPath = maxPath; return true; @@ -218,7 +219,7 @@ public void OptimizePathVisibility(RcVec3f next, float pathOptimizationRange, Dt { if (res.Count > 1 && t > 0.99f) { - m_npath = DtPathUtils.MergeCorridorStartShortcut(ref m_path, m_npath, m_maxPath, res, res.Count); + m_npath = DtPathUtils.MergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, res.Count); } } } @@ -250,7 +251,7 @@ public bool OptimizePathTopology(DtNavMeshQuery navquery, IDtQueryFilter filter, if (status.Succeeded() && res.Count > 0) { - m_npath = DtPathUtils.MergeCorridorStartShortcut(ref m_path, m_npath, m_maxPath, res, res.Count); + m_npath = DtPathUtils.MergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, res.Count); return true; } @@ -276,7 +277,7 @@ public bool MoveOverOffmeshConnection(long offMeshConRef, long[] refs, ref RcVec } // Prune path - m_path = m_path.GetRange(npos, m_npath - npos); + m_path.RemoveRange(0, npos); m_npath -= npos; refs[0] = prevRef; @@ -322,7 +323,7 @@ public bool MovePosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFilter f var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, visited, out var nvisited, MAX_VISITED); if (status.Succeeded()) { - m_npath = DtPathUtils.MergeCorridorStartMoved(ref m_path, m_npath, m_maxPath, visited, nvisited); + m_npath = DtPathUtils.MergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited); // Adjust the position to stay on top of the navmesh. m_pos = result; @@ -366,7 +367,7 @@ public bool MoveTargetPosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFi var status = navquery.MoveAlongSurface(m_path[^1], m_target, npos, filter, out var result, visited, out nvisited, MAX_VISITED); if (status.Succeeded()) { - m_npath = DtPathUtils.MergeCorridorEndMoved(ref m_path, m_npath, m_maxPath, visited, nvisited); + m_npath = DtPathUtils.MergeCorridorEndMoved(m_path, m_npath, m_maxPath, visited, nvisited); // TODO: should we do that? // Adjust the position to stay on top of the navmesh. @@ -394,7 +395,11 @@ public bool MoveTargetPosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFi public void SetCorridor(RcVec3f target, List path) { m_target = target; - m_path = new List(path); + if(path != m_path) + { + m_path.Clear(); + m_path.AddRange(path); + } m_npath = path.Count; } @@ -444,7 +449,7 @@ public bool TrimInvalidPath(long safeRef, float[] safePos, DtNavMeshQuery navque else if (n < m_npath) { // The path is partially usable. - m_path = m_path.GetRange(0, n); + m_path.RemoveRange(n, m_path.Count - n); m_npath = n; } diff --git a/src/DotRecast.Detour/DtPathUtils.cs b/src/DotRecast.Detour/DtPathUtils.cs index 24dd8a63..688f2a04 100644 --- a/src/DotRecast.Detour/DtPathUtils.cs +++ b/src/DotRecast.Detour/DtPathUtils.cs @@ -148,7 +148,7 @@ public static int FixupShortcuts(ref List path, int npath, DtNavMeshQuery return npath; } - public static int MergeCorridorStartMoved(ref List path, int npath, int maxPath, Span visited, int nvisited) + public static int MergeCorridorStartMoved(List path, int npath, int maxPath, Span visited, int nvisited) { int furthestPath = -1; int furthestVisited = -1; @@ -191,11 +191,12 @@ public static int MergeCorridorStartMoved(ref List path, int npath, int ma result.AddRange(path.GetRange(furthestPath, npath - furthestPath)); - path = result; + path.Clear(); + path.AddRange(result); return result.Count; } - public static int MergeCorridorEndMoved(ref List path, int npath, int maxPath, Span visited, int nvisited) + public static int MergeCorridorEndMoved(List path, int npath, int maxPath, Span visited, int nvisited) { int furthestPath = -1; int furthestVisited = -1; @@ -233,11 +234,12 @@ public static int MergeCorridorEndMoved(ref List path, int npath, int maxP result.Add(v); } - path = result; + path.Clear(); + path.AddRange(path); return result.Count; } - public static int MergeCorridorStartShortcut(ref List path, int npath, int maxPath, List visited, int nvisited) + public static int MergeCorridorStartShortcut(List path, int npath, int maxPath, List visited, int nvisited) { int furthestPath = -1; int furthestVisited = -1; @@ -274,7 +276,8 @@ public static int MergeCorridorStartShortcut(ref List path, int npath, int List result = visited.GetRange(0, furthestVisited); result.AddRange(path.GetRange(furthestPath, npath - furthestPath)); - path = result; + path.Clear(); + path.AddRange(result); return result.Count; } } diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs index 4199c74f..256a1dfe 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs @@ -98,7 +98,7 @@ public DtStatus FindFollowPath(DtNavMesh navMesh, DtNavMeshQuery navQuery, long iterPos = result; - pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(ref pathIterPolys, pathIterPolyCount, MAX_POLYS, visited, nvisited); + pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(pathIterPolys, pathIterPolyCount, MAX_POLYS, visited, nvisited); pathIterPolyCount = DtPathUtils.FixupShortcuts(ref pathIterPolys, pathIterPolyCount, navQuery); var status = navQuery.GetPolyHeight(pathIterPolys[0], result, out var h); From dbc92a9aef3dd25fb25b70be22b0e59897acb9f0 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 11:54:10 +0300 Subject: [PATCH 02/22] RcRentedArray AsSpan --- src/DotRecast.Core/Buffers/RcRentedArray.cs | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/DotRecast.Core/Buffers/RcRentedArray.cs b/src/DotRecast.Core/Buffers/RcRentedArray.cs index 68f12c78..e7809f6f 100644 --- a/src/DotRecast.Core/Buffers/RcRentedArray.cs +++ b/src/DotRecast.Core/Buffers/RcRentedArray.cs @@ -43,6 +43,11 @@ public T[] AsArray() return _array; } + public Span AsSpan() + { + return new Span(_array, 0, Length); + } + public void Dispose() { From 2c6f6a50cc9c4e41855d5c5987b6cbcc2cebef81 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 12:09:29 +0300 Subject: [PATCH 03/22] CollectionExtensions.cs List.AddRange(Span) --- src/DotRecast.Core/Collections/CollectionExtensions.cs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/DotRecast.Core/Collections/CollectionExtensions.cs b/src/DotRecast.Core/Collections/CollectionExtensions.cs index 82f5f5c4..c610899d 100644 --- a/src/DotRecast.Core/Collections/CollectionExtensions.cs +++ b/src/DotRecast.Core/Collections/CollectionExtensions.cs @@ -45,5 +45,11 @@ public static void Shuffle(this IList list) (list[k], list[n]) = (list[n], list[k]); } } + + public static void AddRange(this IList list, Span span) + { + foreach (var i in span) + list.Add(i); + } } } \ No newline at end of file From 49da3fb4544fc32b14a192bde122efb5c76d35f5 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 12:09:48 +0300 Subject: [PATCH 04/22] Removed allocations from MergeCorridorStartMoved --- src/DotRecast.Detour/DtPathUtils.cs | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/DotRecast.Detour/DtPathUtils.cs b/src/DotRecast.Detour/DtPathUtils.cs index 688f2a04..2d77f6ed 100644 --- a/src/DotRecast.Detour/DtPathUtils.cs +++ b/src/DotRecast.Detour/DtPathUtils.cs @@ -20,7 +20,10 @@ 3. This notice may not be removed or altered from any source distribution. using System; using System.Collections.Generic; +using DotRecast.Core.Buffers; +using DotRecast.Core.Collections; using DotRecast.Core.Numerics; +using CollectionExtensions = DotRecast.Core.Collections.CollectionExtensions; namespace DotRecast.Detour { @@ -180,20 +183,20 @@ public static int MergeCorridorStartMoved(List path, int npath, int maxPat } // Concatenate paths. - + var endIndex = nvisited - 1; + var length1 = endIndex - furthestVisited; + var length2 = npath - furthestPath; + using var result = RcRentedArray.Rent(length1 + length2); // Adjust beginning of the buffer to include the visited. - List result = new List(); // Store visited - for (int i = nvisited - 1; i > furthestVisited; --i) - { - result.Add(visited[i]); - } - - result.AddRange(path.GetRange(furthestPath, npath - furthestPath)); + for (int i = 0; i < length1; ++i) + result[i] = visited[endIndex - i]; + + path.CopyTo(furthestPath, result.AsArray(), length1, length2); path.Clear(); - path.AddRange(result); - return result.Count; + CollectionExtensions.AddRange(path, result.AsSpan()); + return result.Length; } public static int MergeCorridorEndMoved(List path, int npath, int maxPath, Span visited, int nvisited) From 4dee6b20b5838ac5cce908d21009806d1e53a769 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 12:48:02 +0300 Subject: [PATCH 05/22] typo fix --- src/DotRecast.Detour/DtPathUtils.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DotRecast.Detour/DtPathUtils.cs b/src/DotRecast.Detour/DtPathUtils.cs index 2d77f6ed..c58c353c 100644 --- a/src/DotRecast.Detour/DtPathUtils.cs +++ b/src/DotRecast.Detour/DtPathUtils.cs @@ -238,7 +238,7 @@ public static int MergeCorridorEndMoved(List path, int npath, int maxPath, } path.Clear(); - path.AddRange(path); + path.AddRange(result); return result.Count; } From 05613f196f56ed591959bc69b1fbe27721638f23 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 12:52:33 +0300 Subject: [PATCH 06/22] MergeCorridorStartShortcut alloc fix --- src/DotRecast.Detour/DtPathUtils.cs | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/DotRecast.Detour/DtPathUtils.cs b/src/DotRecast.Detour/DtPathUtils.cs index c58c353c..f5e9d14d 100644 --- a/src/DotRecast.Detour/DtPathUtils.cs +++ b/src/DotRecast.Detour/DtPathUtils.cs @@ -276,12 +276,15 @@ public static int MergeCorridorStartShortcut(List path, int npath, int max // Concatenate paths. // Adjust beginning of the buffer to include the visited. - List result = visited.GetRange(0, furthestVisited); - result.AddRange(path.GetRange(furthestPath, npath - furthestPath)); + var length1 = furthestVisited; + var length2 = npath - furthestPath; + using var result = RcRentedArray.Rent(length1 + length2); + visited.CopyTo(0, result.AsArray(), 0, length1); + path.CopyTo(furthestPath, result.AsArray(), length1, length2); path.Clear(); - path.AddRange(result); - return result.Count; + CollectionExtensions.AddRange(path, result.AsSpan()); + return result.Length; } } } \ No newline at end of file From 418a39a576737373fbf463c1c2146ec8012404ee Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 12:57:19 +0300 Subject: [PATCH 07/22] MergeCorridorEndMoved alloc fix --- src/DotRecast.Detour/DtPathUtils.cs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/DotRecast.Detour/DtPathUtils.cs b/src/DotRecast.Detour/DtPathUtils.cs index f5e9d14d..254b4392 100644 --- a/src/DotRecast.Detour/DtPathUtils.cs +++ b/src/DotRecast.Detour/DtPathUtils.cs @@ -231,15 +231,15 @@ public static int MergeCorridorEndMoved(List path, int npath, int maxPath, } // Concatenate paths. - List result = path.GetRange(0, furthestPath); - foreach (var v in visited.Slice(furthestVisited, nvisited - furthestVisited)) - { - result.Add(v); - } + var length1 = furthestPath; + var length2 = nvisited - furthestVisited; + using var result = RcRentedArray.Rent(length1 + length2); + path.CopyTo(0, result.AsArray(), 0, length1); + visited.Slice(furthestVisited, nvisited - furthestVisited).CopyTo(result.AsSpan().Slice(length1, length2)); path.Clear(); - path.AddRange(result); - return result.Count; + CollectionExtensions.AddRange(path, result.AsSpan()); + return path.Count; } public static int MergeCorridorStartShortcut(List path, int npath, int maxPath, List visited, int nvisited) @@ -284,7 +284,7 @@ public static int MergeCorridorStartShortcut(List path, int npath, int max path.Clear(); CollectionExtensions.AddRange(path, result.AsSpan()); - return result.Length; + return path.Count; } } } \ No newline at end of file From 5c0ba9dba1adadbc4984e5721c71933312370544 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 13:14:32 +0300 Subject: [PATCH 08/22] Reuse grid instead of creating new --- src/DotRecast.Detour.Crowd/DtCrowd.cs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index d90f4ec1..c0054244 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -129,7 +129,7 @@ public class DtCrowd private readonly DtObstacleAvoidanceParams[] _obstacleQueryParams; private readonly DtObstacleAvoidanceQuery _obstacleQuery; - private DtProximityGrid _grid; + private readonly DtProximityGrid _grid; private int _maxPathResult; private readonly RcVec3f _agentPlacementHalfExtents; @@ -174,6 +174,7 @@ public DtCrowd(DtCrowdConfig config, DtNavMesh nav, Func qu _agentIdx = new RcAtomicInteger(0); _agents = new Dictionary(); _activeAgents = new List(); + _grid = new DtProximityGrid(_config.maxAgentRadius * 3); // The navQuery is mostly used for local searches, no need for large node pool. SetNavMesh(nav); @@ -864,7 +865,7 @@ private void BuildProximityGrid(IList agents) { using var timer = _telemetry.ScopedTimer(DtCrowdTimerLabel.BuildProximityGrid); - _grid = new DtProximityGrid(_config.maxAgentRadius * 3); + _grid.Clear(); for (var i = 0; i < agents.Count; i++) { From b2a217d4a3248d8b76b86f9021b49280901daf09 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 13:21:41 +0300 Subject: [PATCH 09/22] Object pool --- src/DotRecast.Core/Buffers/RcObjectPool.cs | 30 ++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 src/DotRecast.Core/Buffers/RcObjectPool.cs diff --git a/src/DotRecast.Core/Buffers/RcObjectPool.cs b/src/DotRecast.Core/Buffers/RcObjectPool.cs new file mode 100644 index 00000000..c37cf609 --- /dev/null +++ b/src/DotRecast.Core/Buffers/RcObjectPool.cs @@ -0,0 +1,30 @@ +using System; +using System.Collections.Generic; + +namespace DotRecast.Core.Buffers +{ + // This implementation is thread unsafe + public class RcObjectPool where T : class + { + private readonly Queue _items = new Queue(); + private readonly Func _createFunc; + + public RcObjectPool(Func createFunc) + { + _createFunc = createFunc; + } + + public T Get() + { + if (_items.TryDequeue(out var result)) + return result; + + return _createFunc(); + } + + public void Return(T obj) + { + _items.Enqueue(obj); + } + } +} \ No newline at end of file From 592ecebe1e99f6770113429bbc4ddfa6f571db40 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 13:25:13 +0300 Subject: [PATCH 10/22] Proximity grid list reuse --- src/DotRecast.Detour.Crowd/DtProximityGrid.cs | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/DotRecast.Detour.Crowd/DtProximityGrid.cs b/src/DotRecast.Detour.Crowd/DtProximityGrid.cs index 3cd182cf..e3949823 100644 --- a/src/DotRecast.Detour.Crowd/DtProximityGrid.cs +++ b/src/DotRecast.Detour.Crowd/DtProximityGrid.cs @@ -22,6 +22,7 @@ 3. This notice may not be removed or altered from any source distribution. using System.Collections.Generic; using System.Linq; using System.Runtime.CompilerServices; +using DotRecast.Core.Buffers; namespace DotRecast.Detour.Crowd { @@ -30,12 +31,14 @@ public class DtProximityGrid private readonly float _cellSize; private readonly float _invCellSize; private readonly Dictionary> _items; + private readonly RcObjectPool> _listPool; public DtProximityGrid(float cellSize) { _cellSize = cellSize; _invCellSize = 1.0f / cellSize; _items = new Dictionary>(); + _listPool = new RcObjectPool>(() => new List()); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -57,6 +60,8 @@ public static void DecomposeKey(long key, out int x, out int y) public void Clear() { + foreach (var pair in _items) + _listPool.Return(pair.Value); _items.Clear(); } @@ -74,7 +79,7 @@ public void AddItem(DtCrowdAgent agent, float minx, float miny, float maxx, floa long key = CombineKey(x, y); if (!_items.TryGetValue(key, out var ids)) { - ids = new List(); + ids = _listPool.Get(); _items.Add(key, ids); } From 815a83e3cb87841a20dace847ae4927ef6456735 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 13:42:43 +0300 Subject: [PATCH 11/22] Use precached queue instead of linked list --- src/DotRecast.Detour/DtNavMeshQuery.cs | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index ee0e699a..7ff5d4b5 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -1837,6 +1837,7 @@ public virtual DtStatus FindStraightPath(RcVec3f startPos, RcVec3f endPos, return DtStatus.DT_SUCCESS | (straightPathCount >= maxStraightPath ? DtStatus.DT_BUFFER_TOO_SMALL : DtStatus.DT_STATUS_NOTHING); } + private readonly Queue MoveAlongSurface_queue = new Queue(); /// @par /// /// This method is optimized for small delta movement and a small number of @@ -1892,8 +1893,8 @@ public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos startNode.total = 0; startNode.id = startRef; startNode.flags = DtNodeFlags.DT_NODE_CLOSED; - LinkedList stack = new LinkedList(); - stack.AddLast(startNode); + MoveAlongSurface_queue.Clear(); + MoveAlongSurface_queue.Enqueue(startNode); RcVec3f bestPos = new RcVec3f(); float bestDist = float.MaxValue; @@ -1909,11 +1910,10 @@ public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos const int MAX_NEIS = 8; Span neis = stackalloc long[MAX_NEIS]; - while (0 < stack.Count) + while (0 < MoveAlongSurface_queue.Count) { // Pop front. - DtNode curNode = stack.First?.Value; - stack.RemoveFirst(); + DtNode curNode = MoveAlongSurface_queue.Dequeue(); // Get poly and tile. // The API input has been checked already, skip checking internal data. @@ -2012,7 +2012,7 @@ public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos // Mark as the node as visited and push to queue. neighbourNode.pidx = m_tinyNodePool.GetNodeIdx(curNode); neighbourNode.flags |= DtNodeFlags.DT_NODE_CLOSED; - stack.AddLast(neighbourNode); + MoveAlongSurface_queue.Enqueue(neighbourNode); } } } @@ -2903,6 +2903,7 @@ public DtStatus FindPolysAroundShape(long startRef, RcVec3f[] verts, IDtQueryFil return DtStatus.DT_SUCCESS; } + private readonly Queue FindLocalNeighbourhood_queue = new Queue(); /// @par /// /// This method is optimized for a small search radius and small number of result @@ -2954,8 +2955,8 @@ public DtStatus FindLocalNeighbourhood(long startRef, RcVec3f centerPos, float r startNode.pidx = 0; startNode.id = startRef; startNode.flags = DtNodeFlags.DT_NODE_CLOSED; - LinkedList stack = new LinkedList(); - stack.AddLast(startNode); + FindLocalNeighbourhood_queue.Clear(); + FindLocalNeighbourhood_queue.Enqueue(startNode); resultRef.Add(startNode.id); resultParent.Add(0L); @@ -2965,11 +2966,11 @@ public DtStatus FindLocalNeighbourhood(long startRef, RcVec3f centerPos, float r Span pa = stackalloc float[m_nav.GetMaxVertsPerPoly() * 3]; Span pb = stackalloc float[m_nav.GetMaxVertsPerPoly() * 3]; - while (0 < stack.Count) + while (0 < FindLocalNeighbourhood_queue.Count) { // Pop front. - DtNode curNode = stack.First?.Value; - stack.RemoveFirst(); + + DtNode curNode = FindLocalNeighbourhood_queue.Dequeue(); // Get poly and tile. // The API input has been checked already, skip checking internal data. @@ -3082,7 +3083,7 @@ public DtStatus FindLocalNeighbourhood(long startRef, RcVec3f centerPos, float r resultRef.Add(neighbourRef); resultParent.Add(curRef); - stack.AddLast(neighbourNode); + FindLocalNeighbourhood_queue.Enqueue(neighbourNode); } } From 1fa0320845409380171b62570e5fd6e83045b26d Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 13:55:02 +0300 Subject: [PATCH 12/22] precache collections --- src/DotRecast.Detour.Crowd/DtCrowd.cs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index c0054244..d148a867 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -565,14 +565,18 @@ private void CheckPathValidity(IList agents, float dt) } } + private readonly RcSortedQueue UpdateMoveRequest_queue = new RcSortedQueue((a1, a2) => a2.targetReplanTime.CompareTo(a1.targetReplanTime)); + private readonly List UpdateMoveRequest_reqPath = new List(); private void UpdateMoveRequest(IList agents, float dt) { using var timer = _telemetry.ScopedTimer(DtCrowdTimerLabel.UpdateMoveRequest); - RcSortedQueue queue = new RcSortedQueue((a1, a2) => a2.targetReplanTime.CompareTo(a1.targetReplanTime)); + RcSortedQueue queue = UpdateMoveRequest_queue; + queue.Clear(); // Fire off new requests. - List reqPath = new List(); + List reqPath = UpdateMoveRequest_reqPath; + reqPath.Clear(); for (var i = 0; i < agents.Count; i++) { var ag = agents[i]; From d2082c658644ab7a97faf52eb83604249e0ccabc Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 14:20:38 +0300 Subject: [PATCH 13/22] Added clear --- src/DotRecast.Detour.Crowd/DtProximityGrid.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/DotRecast.Detour.Crowd/DtProximityGrid.cs b/src/DotRecast.Detour.Crowd/DtProximityGrid.cs index e3949823..f4814972 100644 --- a/src/DotRecast.Detour.Crowd/DtProximityGrid.cs +++ b/src/DotRecast.Detour.Crowd/DtProximityGrid.cs @@ -80,6 +80,7 @@ public void AddItem(DtCrowdAgent agent, float minx, float miny, float maxx, floa if (!_items.TryGetValue(key, out var ids)) { ids = _listPool.Get(); + ids.Clear(); _items.Add(key, ids); } From 4824f29db7cc99ff3acecde9bfe07a6bb50d65a5 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 15:51:02 +0300 Subject: [PATCH 14/22] Non alloc raycast --- src/DotRecast.Detour/DtNavMeshQuery.cs | 20 ++++++++++++++------ src/DotRecast.Detour/DtRaycastHit.cs | 9 ++++++++- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 7ff5d4b5..b11bd885 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -21,7 +21,9 @@ 3. This notice may not be removed or altered from any source distribution. using System; using System.Collections.Generic; using DotRecast.Core; +using DotRecast.Core.Buffers; using DotRecast.Core.Numerics; +using CollectionExtensions = DotRecast.Core.Collections.CollectionExtensions; namespace DotRecast.Detour { @@ -32,6 +34,8 @@ namespace DotRecast.Detour /// @ingroup detour public class DtNavMeshQuery { + public const int MAX_PATH_LENGTH = 256; + protected readonly DtNavMesh m_nav; //< Pointer to navmesh data. protected DtQueryData m_query; //< Sliced query state. @@ -877,7 +881,8 @@ public DtStatus FindPath(long startRef, long endRef, RcVec3f startPos, RcVec3f e float lastBestNodeCost = startNode.total; DtRaycastHit rayHit = new DtRaycastHit(); - rayHit.path = new List(); + using var pathRent = RcRentedArray.Rent(MAX_PATH_LENGTH); + rayHit.path = pathRent.AsArray(); while (!m_openList.IsEmpty()) { // Remove node from open list and put it in closed list. @@ -1166,7 +1171,8 @@ public virtual DtStatus UpdateSlicedFindPath(int maxIter, out int doneIters) } var rayHit = new DtRaycastHit(); - rayHit.path = new List(); + using var pathRent = RcRentedArray.Rent(MAX_PATH_LENGTH); + rayHit.path = pathRent.AsArray(); int iter = 0; while (iter < maxIter && !m_openList.IsEmpty()) @@ -2258,13 +2264,15 @@ public DtStatus Raycast(long startRef, RcVec3f startPos, RcVec3f endPos, out float t, out RcVec3f hitNormal, ref List path) { DtRaycastHit hit = new DtRaycastHit(); - hit.path = path; + using var pathRent = RcRentedArray.Rent(MAX_PATH_LENGTH); + hit.path = pathRent.AsArray(); DtStatus status = Raycast(startRef, startPos, endPos, filter, 0, ref hit, 0); t = hit.t; hitNormal = hit.hitNormal; - path = hit.path; + path.Clear(); + CollectionExtensions.AddRange(path, new Span(hit.path, 0, hit.pathCount)); return status; } @@ -2330,7 +2338,7 @@ public DtStatus Raycast(long startRef, RcVec3f startPos, RcVec3f endPos, } hit.t = 0; - hit.path.Clear(); + hit.pathCount = 0; hit.pathCost = 0; Span verts = stackalloc RcVec3f[m_nav.GetMaxVertsPerPoly() + 1]; @@ -2383,7 +2391,7 @@ public DtStatus Raycast(long startRef, RcVec3f startPos, RcVec3f endPos, } // Store visited polygons. - hit.path.Add(curRef); + hit.AddPathNode(curRef); // Ray end is completely inside the polygon. if (segMax == -1) diff --git a/src/DotRecast.Detour/DtRaycastHit.cs b/src/DotRecast.Detour/DtRaycastHit.cs index 58c79ac6..22154e29 100644 --- a/src/DotRecast.Detour/DtRaycastHit.cs +++ b/src/DotRecast.Detour/DtRaycastHit.cs @@ -38,9 +38,16 @@ public struct DtRaycastHit public int hitEdgeIndex; /// Pointer to an array of reference ids of the visited polygons. [opt] - public List path; + public long[] path; + + public int pathCount; /// The cost of the path until hit. public float pathCost; + + public void AddPathNode(long nodeRef) + { + path[pathCount++] = nodeRef; + } } } \ No newline at end of file From 12e09475f091794201180fa7ccc27a246bb28a23 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 17:15:32 +0300 Subject: [PATCH 15/22] Replace predicate find with simple find --- src/DotRecast.Core/Collections/RcSortedQueue.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DotRecast.Core/Collections/RcSortedQueue.cs b/src/DotRecast.Core/Collections/RcSortedQueue.cs index 0c06aad9..cb09d63a 100644 --- a/src/DotRecast.Core/Collections/RcSortedQueue.cs +++ b/src/DotRecast.Core/Collections/RcSortedQueue.cs @@ -88,7 +88,7 @@ public bool Remove(T item) 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; From 088edcd655fd4b80a718f0ec765ce361c3c03174 Mon Sep 17 00:00:00 2001 From: wrenge Date: Thu, 14 Nov 2024 12:40:03 +0300 Subject: [PATCH 16/22] Intersection query alloc fix --- src/DotRecast.Core/Numerics/RcVec.cs | 2 +- .../DtConvexConvexIntersections.cs | 9 ++--- src/DotRecast.Detour/DtNavMeshQuery.cs | 20 +++++++---- .../DtNoOpDtPolygonByCircleConstraint.cs | 7 ++-- .../DtStrictDtPolygonByCircleConstraint.cs | 24 +++++++++---- .../IDtPolygonByCircleConstraint.cs | 2 +- .../ConvexConvexIntersectionTest.cs | 11 +++--- .../PolygonByCircleConstraintTest.cs | 34 ++++++++++++++----- 8 files changed, 74 insertions(+), 35 deletions(-) diff --git a/src/DotRecast.Core/Numerics/RcVec.cs b/src/DotRecast.Core/Numerics/RcVec.cs index 69aae7d0..0569071d 100644 --- a/src/DotRecast.Core/Numerics/RcVec.cs +++ b/src/DotRecast.Core/Numerics/RcVec.cs @@ -205,7 +205,7 @@ public static float Dist2DSqr(RcVec3f v1, RcVec3f v2) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float Dist2DSqr(RcVec3f p, float[] verts, int i) + public static float Dist2DSqr(RcVec3f p, Span verts, int i) { float dx = verts[i] - p.X; float dz = verts[i + 2] - p.Z; diff --git a/src/DotRecast.Detour/DtConvexConvexIntersections.cs b/src/DotRecast.Detour/DtConvexConvexIntersections.cs index 4e219ee9..59aa4b8f 100644 --- a/src/DotRecast.Detour/DtConvexConvexIntersections.cs +++ b/src/DotRecast.Detour/DtConvexConvexIntersections.cs @@ -27,7 +27,7 @@ public static class DtConvexConvexIntersections { private const float EPSILON = 0.0001f; - public static float[] Intersect(Span p, Span q) + public static Span Intersect(Span p, Span q, Span buffer) { int n = p.Length / 3; int m = q.Length / 3; @@ -95,7 +95,7 @@ public static float[] Intersect(Span p, Span q) /* Special case: A & B parallel and separated. */ if (parallel && aHB < 0f && bHA < 0f) { - return null; + return Span.Empty; } /* Special case: A & B collinear. */ else if (parallel && MathF.Abs(aHB) < EPSILON && MathF.Abs(bHA) < EPSILON) @@ -168,8 +168,9 @@ public static float[] Intersect(Span p, Span q) return null; } - float[] copied = inters.Slice(0, ii).ToArray(); - return copied; + Span result = buffer.Slice(0, ii); + inters.Slice(0, ii).CopyTo(result); + return result; } private static int AddVertex(Span inters, int ii, RcVec3f p) diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index b11bd885..4a1ce9fc 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -233,6 +233,7 @@ public DtStatus FindRandomPointAroundCircle(long startRef, RcVec3f centerPos, fl IDtQueryFilter filter, IRcRand frand, IDtPolygonByCircleConstraint constraint, out long randomRef, out RcVec3f randomPt) { + const int MAX_VERT_BUFFER_SIZE = 128; randomRef = startRef; randomPt = centerPos; @@ -265,10 +266,14 @@ public DtStatus FindRandomPointAroundCircle(long startRef, RcVec3f centerPos, fl float radiusSqr = maxRadius * maxRadius; float areaSum = 0.0f; - + + using RcRentedArray polyVertsBuffer = RcRentedArray.Rent(MAX_VERT_BUFFER_SIZE); + using RcRentedArray randomPolyVertsBuffer = RcRentedArray.Rent(MAX_VERT_BUFFER_SIZE); + using RcRentedArray constrainedVertsBuffer = RcRentedArray.Rent(MAX_VERT_BUFFER_SIZE); + DtPoly randomPoly = null; long randomPolyRef = 0; - float[] randomPolyVerts = null; + Span randomPolyVerts = Span.Empty; while (!m_openList.IsEmpty()) { @@ -286,14 +291,14 @@ public DtStatus FindRandomPointAroundCircle(long startRef, RcVec3f centerPos, fl { // Calc area of the polygon. float polyArea = 0.0f; - float[] polyVerts = new float[bestPoly.vertCount * 3]; + Span 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 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 @@ public DtStatus FindRandomPointAroundCircle(long startRef, RcVec3f centerPos, fl { randomPoly = bestPoly; randomPolyRef = bestRef; - randomPolyVerts = constrainedVerts; + randomPolyVerts = randomPolyVertsBuffer.AsSpan().Slice(0, constrainedVerts.Length); + constrainedVerts.CopyTo(randomPolyVerts); } } } diff --git a/src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs b/src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs index e5d26510..3a67655f 100644 --- a/src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs +++ b/src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs @@ -1,3 +1,4 @@ +using System; using DotRecast.Core.Numerics; namespace DotRecast.Detour @@ -10,9 +11,11 @@ private DtNoOpDtPolygonByCircleConstraint() { } - public float[] Apply(float[] polyVerts, RcVec3f circleCenter, float radius) + public Span Apply(Span polyVerts, RcVec3f circleCenter, float radius, Span resultBuffer) { - return polyVerts; + var result = resultBuffer.Slice(0, polyVerts.Length); + polyVerts.CopyTo(result); + return result; } } } \ No newline at end of file diff --git a/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs b/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs index 47bf30e7..61cc62ad 100644 --- a/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs +++ b/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs @@ -39,8 +39,7 @@ public static void ScaleCircle(Span src, RcVec3f center, float radius, Sp } } - - public float[] Apply(float[] verts, RcVec3f center, float radius) + public Span Apply(Span verts, RcVec3f center, float radius, Span resultBuffer) { float radiusSqr = radius * radius; int outsideVertex = -1; @@ -56,19 +55,30 @@ public float[] Apply(float[] verts, RcVec3f center, float radius) if (outsideVertex == -1) { // polygon inside circle - return verts; + var result = resultBuffer.Slice(0, verts.Length); + verts.CopyTo(result); + return result; } Span 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 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.Empty; } } } \ No newline at end of file diff --git a/src/DotRecast.Detour/IDtPolygonByCircleConstraint.cs b/src/DotRecast.Detour/IDtPolygonByCircleConstraint.cs index 9f46ff3a..138e508e 100644 --- a/src/DotRecast.Detour/IDtPolygonByCircleConstraint.cs +++ b/src/DotRecast.Detour/IDtPolygonByCircleConstraint.cs @@ -24,6 +24,6 @@ namespace DotRecast.Detour { public interface IDtPolygonByCircleConstraint { - float[] Apply(float[] polyVerts, RcVec3f circleCenter, float radius); + Span Apply(Span polyVerts, RcVec3f circleCenter, float radius, Span resultBuffer); } } \ No newline at end of file diff --git a/test/DotRecast.Detour.Test/ConvexConvexIntersectionTest.cs b/test/DotRecast.Detour.Test/ConvexConvexIntersectionTest.cs index f78f3312..71f71d7c 100644 --- a/test/DotRecast.Detour.Test/ConvexConvexIntersectionTest.cs +++ b/test/DotRecast.Detour.Test/ConvexConvexIntersectionTest.cs @@ -17,6 +17,7 @@ misrepresented as being the original software. 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 void ShouldHandleSamePolygonIntersection() { 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 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 void ShouldHandleIntersection() { 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 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 })); } } \ No newline at end of file diff --git a/test/DotRecast.Detour.Test/PolygonByCircleConstraintTest.cs b/test/DotRecast.Detour.Test/PolygonByCircleConstraintTest.cs index 7e7c231e..4329506c 100644 --- a/test/DotRecast.Detour.Test/PolygonByCircleConstraintTest.cs +++ b/test/DotRecast.Detour.Test/PolygonByCircleConstraintTest.cs @@ -17,6 +17,7 @@ misrepresented as being the original software. 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 void ShouldHandlePolygonFullyInsideCircle() { 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; + + float[] buffer = new float[128]; + Span constrained = _constraint.Apply(polygon, center, radius, buffer); - Assert.That(constrained, Is.EqualTo(polygon)); + Assert.That(constrained.ToArray(), Is.EquivalentTo(polygon)); } [Test] @@ -43,10 +47,13 @@ public void ShouldHandleVerticalSegment() 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 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 void ShouldHandleCircleFullyInsidePolygon() 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 constrained = _constraint.Apply(polygon, center, radius, buffer); Assert.That(constrained.Length, Is.EqualTo(expectedSize)); @@ -73,10 +83,13 @@ public void ShouldHandleCircleInsidePolygon() 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 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 void ShouldHandleCircleOutsidePolygon() 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 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 })); } } \ No newline at end of file From 18b544845f39121e461e9c7aaa54a09bbb9984e2 Mon Sep 17 00:00:00 2001 From: wrenge Date: Wed, 13 Nov 2024 09:35:27 +0300 Subject: [PATCH 17/22] Pass delegate --- src/DotRecast.Core/Collections/RcSortedQueue.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/DotRecast.Core/Collections/RcSortedQueue.cs b/src/DotRecast.Core/Collections/RcSortedQueue.cs index cb09d63a..db822dd8 100644 --- a/src/DotRecast.Core/Collections/RcSortedQueue.cs +++ b/src/DotRecast.Core/Collections/RcSortedQueue.cs @@ -27,12 +27,12 @@ public class RcSortedQueue { private bool _dirty; private readonly List _items; - private readonly Comparer _comparer; + private readonly Comparison _comparison; public RcSortedQueue(Comparison comp) { _items = new List(); - _comparer = Comparer.Create((x, y) => comp.Invoke(x, y) * -1); + _comparison = (x, y) => comp(x, y) * -1; } public int Count() @@ -55,7 +55,7 @@ private void Balance() { if (_dirty) { - _items.Sort(_comparer); // reverse + _items.Sort(_comparison); // reverse _dirty = false; } } From 7aeb31d369c53067e7bf52ab448dbbc8bb5d6a4b Mon Sep 17 00:00:00 2001 From: wrenge Date: Mon, 11 Nov 2024 14:26:35 +0300 Subject: [PATCH 18/22] Span access --- src/DotRecast.Core/Buffers/RcRentedArray.cs | 2 +- src/DotRecast.Detour/DtNavMeshQuery.cs | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/DotRecast.Core/Buffers/RcRentedArray.cs b/src/DotRecast.Core/Buffers/RcRentedArray.cs index e7809f6f..208b9516 100644 --- a/src/DotRecast.Core/Buffers/RcRentedArray.cs +++ b/src/DotRecast.Core/Buffers/RcRentedArray.cs @@ -1,4 +1,4 @@ -using System; +using System; using System.Buffers; using System.Runtime.CompilerServices; diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 4a1ce9fc..7200a57c 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -607,7 +607,8 @@ protected void QueryPolygonsInTile(DtMeshTile tile, RcVec3f qmin, RcVec3f qmax, { const int batchSize = 32; Span polyRefs = stackalloc long[batchSize]; - DtPoly[] polys = new DtPoly[batchSize]; + using RcRentedArray polysRent = RcRentedArray.Rent(batchSize); + Span polys = polysRent.AsSpan(); int n = 0; if (tile.data.bvTree != null) From a7b9b772c4cc308cf3c2179001a18ef1b52eab39 Mon Sep 17 00:00:00 2001 From: wrenge Date: Tue, 12 Nov 2024 11:18:14 +0300 Subject: [PATCH 19/22] Use rented array instead of allocating (cherry picked from commit ff930712ee2c806753f39b7565f50e03a988691f) --- src/DotRecast.Detour/DtNavMesh.cs | 5 +++++ src/DotRecast.Detour/DtNavMeshQuery.cs | 3 ++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/DotRecast.Detour/DtNavMesh.cs b/src/DotRecast.Detour/DtNavMesh.cs index a4791238..54a2db37 100644 --- a/src/DotRecast.Detour/DtNavMesh.cs +++ b/src/DotRecast.Detour/DtNavMesh.cs @@ -1365,6 +1365,11 @@ int GetNeighbourTilesAt(int x, int y, int side, DtMeshTile[] tiles, int maxTiles } public int GetTilesAt(int x, int y, DtMeshTile[] tiles, int maxTiles) + { + return GetTilesAt(x, y, (Span)tiles, maxTiles); + } + + public int GetTilesAt(int x, int y, Span tiles, int maxTiles) { int n = 0; diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 7200a57c..4c636e5c 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -797,7 +797,8 @@ public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, IDtQueryFilte m_nav.CalcTileLoc(bmax, out var maxx, out var maxy); const int MAX_NEIS = 32; - DtMeshTile[] neis = new DtMeshTile[MAX_NEIS]; + using RcRentedArray neisRent = RcRentedArray.Rent(MAX_NEIS); + Span neis = neisRent.AsSpan(); for (int y = miny; y <= maxy; ++y) { From a6db4344e4b9dbaa1413b0be4e94063784f3ed43 Mon Sep 17 00:00:00 2001 From: wrenge Date: Tue, 12 Nov 2024 16:56:11 +0300 Subject: [PATCH 20/22] Queries change (cherry picked from commit 2397f23fc301f4c32408045ab55b19f88ee08599) --- .../Jumplink/NavMeshGroundSampler.cs | 21 ++------- src/DotRecast.Detour/DtCallbackPolyQuery.cs | 2 +- .../DtFindNearestPolyQuery.cs | 5 ++- .../DtHeightSamplePolyQuery.cs | 45 +++++++++++++++++++ src/DotRecast.Detour/DtNavMeshQuery.cs | 12 ++--- 5 files changed, 61 insertions(+), 24 deletions(-) create mode 100644 src/DotRecast.Detour/DtHeightSamplePolyQuery.cs diff --git a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs index b14de89d..934d445a 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs +++ b/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; } diff --git a/src/DotRecast.Detour/DtCallbackPolyQuery.cs b/src/DotRecast.Detour/DtCallbackPolyQuery.cs index 77f626f1..045da0b6 100644 --- a/src/DotRecast.Detour/DtCallbackPolyQuery.cs +++ b/src/DotRecast.Detour/DtCallbackPolyQuery.cs @@ -2,7 +2,7 @@ namespace DotRecast.Detour { - public class DtCallbackPolyQuery : IDtPolyQuery + public struct DtCallbackPolyQuery : IDtPolyQuery { private readonly Action _callback; diff --git a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs index b0c4cf1c..597caf9b 100644 --- a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs +++ b/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 refs, int count) diff --git a/src/DotRecast.Detour/DtHeightSamplePolyQuery.cs b/src/DotRecast.Detour/DtHeightSamplePolyQuery.cs new file mode 100644 index 00000000..674f0aa7 --- /dev/null +++ b/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 poly, Span 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; + } + } +} \ No newline at end of file diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 4c636e5c..0ebaeb68 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/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(DtMeshTile tile, RcVec3f qmin, RcVec3f qmax, IDtQueryFilter filter, ref TQuery query) + where TQuery : IDtPolyQuery { const int batchSize = 32; Span 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(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); } } } From 1315de063fd410bb015eba5621c22cdb82399e05 Mon Sep 17 00:00:00 2001 From: wrenge Date: Mon, 11 Nov 2024 14:19:43 +0300 Subject: [PATCH 21/22] Replaced arrays with spans in query. Replaced array allocation with buffer rent in query. --- src/DotRecast.Detour/DtCallbackPolyQuery.cs | 2 +- src/DotRecast.Detour/DtCollectPolysQuery.cs | 2 +- src/DotRecast.Detour/DtFindNearestPolyQuery.cs | 2 +- src/DotRecast.Detour/IDtPolyQuery.cs | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/DotRecast.Detour/DtCallbackPolyQuery.cs b/src/DotRecast.Detour/DtCallbackPolyQuery.cs index 045da0b6..d5325ad9 100644 --- a/src/DotRecast.Detour/DtCallbackPolyQuery.cs +++ b/src/DotRecast.Detour/DtCallbackPolyQuery.cs @@ -11,7 +11,7 @@ public DtCallbackPolyQuery(Action callback) _callback = callback; } - public void Process(DtMeshTile tile, DtPoly[] poly, Span refs, int count) + public void Process(DtMeshTile tile, Span poly, Span refs, int count) { for (int i = 0; i < count; ++i) { diff --git a/src/DotRecast.Detour/DtCollectPolysQuery.cs b/src/DotRecast.Detour/DtCollectPolysQuery.cs index e98daa74..29710d4f 100644 --- a/src/DotRecast.Detour/DtCollectPolysQuery.cs +++ b/src/DotRecast.Detour/DtCollectPolysQuery.cs @@ -26,7 +26,7 @@ public bool Overflowed() return m_overflow; } - public void Process(DtMeshTile tile, DtPoly[] poly, Span refs, int count) + public void Process(DtMeshTile tile, Span poly, Span refs, int count) { int numLeft = m_maxPolys - m_numCollected; int toCopy = count; diff --git a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs index 597caf9b..56ac604f 100644 --- a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs +++ b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs @@ -23,7 +23,7 @@ public DtFindNearestPolyQuery(DtNavMeshQuery query, RcVec3f center) _overPoly = default; } - public void Process(DtMeshTile tile, DtPoly[] poly, Span refs, int count) + public void Process(DtMeshTile tile, Span poly, Span refs, int count) { for (int i = 0; i < count; ++i) { diff --git a/src/DotRecast.Detour/IDtPolyQuery.cs b/src/DotRecast.Detour/IDtPolyQuery.cs index 890d171b..5a6db49f 100644 --- a/src/DotRecast.Detour/IDtPolyQuery.cs +++ b/src/DotRecast.Detour/IDtPolyQuery.cs @@ -9,6 +9,6 @@ public interface IDtPolyQuery { /// 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, Span refs, int count); + void Process(DtMeshTile tile, Span poly, Span refs, int count); } } \ No newline at end of file From 96ffed87e376bb0bc6745ffe3ecd75973b5c4fe0 Mon Sep 17 00:00:00 2001 From: wrenge Date: Tue, 26 Nov 2024 20:50:44 +0300 Subject: [PATCH 22/22] Rented array struct --- src/DotRecast.Core/Buffers/RcRentedArray.cs | 83 +++++++++++++++++-- test/DotRecast.Core.Test/RcRentedArrayTest.cs | 15 ++++ 2 files changed, 91 insertions(+), 7 deletions(-) diff --git a/src/DotRecast.Core/Buffers/RcRentedArray.cs b/src/DotRecast.Core/Buffers/RcRentedArray.cs index 208b9516..cc49483d 100644 --- a/src/DotRecast.Core/Buffers/RcRentedArray.cs +++ b/src/DotRecast.Core/Buffers/RcRentedArray.cs @@ -1,31 +1,96 @@ using System; using System.Buffers; +using System.Collections.Generic; using System.Runtime.CompilerServices; +using System.Threading; namespace DotRecast.Core.Buffers { public static class RcRentedArray { + private sealed class RentIdPool + { + private int[] _generations; + private readonly Queue _freeIds; + private int _maxId; + + public RentIdPool(int capacity) + { + _generations = new int[capacity]; + _freeIds = new Queue(capacity); + } + + internal RentIdGen AcquireId() + { + if (!_freeIds.TryDequeue(out int id)) + { + id = _maxId++; + if(_generations.Length <= id) + Array.Resize(ref _generations, _generations.Length << 1); + } + + return new RentIdGen(id, _generations[id]); + } + + internal void ReturnId(int id) + { + _generations[id]++; + _freeIds.Enqueue(id); + } + + internal int GetGeneration(int id) + { + return _generations.Length <= id ? 0 : _generations[id]; + } + } + + public const int START_RENT_ID_POOL_CAPACITY = 16; + private static readonly ThreadLocal _rentPool = new ThreadLocal(() => new RentIdPool(START_RENT_ID_POOL_CAPACITY)); + public static RcRentedArray Rent(int minimumLength) { var array = ArrayPool.Shared.Rent(minimumLength); - return new RcRentedArray(ArrayPool.Shared, array, minimumLength); + return new RcRentedArray(ArrayPool.Shared, _rentPool.Value.AcquireId(), array, minimumLength); + } + + internal static bool IsDisposed(RentIdGen rentIdGen) + { + return _rentPool.Value.GetGeneration(rentIdGen.Id) != rentIdGen.Gen; + } + + internal static void ReturnId(RentIdGen rentIdGen) + { + _rentPool.Value.ReturnId(rentIdGen.Id); + } + } + + public readonly struct RentIdGen + { + public readonly int Id; + public readonly int Gen; + + public RentIdGen(int id, int gen) + { + Id = id; + Gen = gen; } } - public class RcRentedArray : IDisposable + public struct RcRentedArray : IDisposable { private ArrayPool _owner; private T[] _array; + private readonly RentIdGen _rentIdGen; public int Length { get; } - public bool IsDisposed => null == _owner || null == _array; + public bool IsDisposed => null == _owner || null == _array || RcRentedArray.IsDisposed(_rentIdGen); - internal RcRentedArray(ArrayPool owner, T[] array, int length) + internal RcRentedArray(ArrayPool owner, RentIdGen rentIdGen, T[] array, int length) { _owner = owner; _array = array; Length = length; + _rentIdGen = rentIdGen; } public ref T this[int index] @@ -34,6 +99,8 @@ public ref T this[int index] get { RcThrowHelper.ThrowExceptionIfIndexOutOfRange(index, Length); + if (IsDisposed) + throw new NullReferenceException(); return ref _array[index]; } } @@ -51,12 +118,14 @@ public Span AsSpan() public void Dispose() { - if (null != _owner && null != _array) + if (null != _owner && null != _array && !RcRentedArray.IsDisposed(_rentIdGen)) { + RcRentedArray.ReturnId(_rentIdGen); _owner.Return(_array, true); - _owner = null; - _array = null; } + + _owner = null; + _array = null; } } } \ No newline at end of file diff --git a/test/DotRecast.Core.Test/RcRentedArrayTest.cs b/test/DotRecast.Core.Test/RcRentedArrayTest.cs index 6ba6febf..9372d72a 100644 --- a/test/DotRecast.Core.Test/RcRentedArrayTest.cs +++ b/test/DotRecast.Core.Test/RcRentedArrayTest.cs @@ -103,4 +103,19 @@ public void TestDispose() Assert.That(r1.IsDisposed, Is.EqualTo(true)); Assert.That(r1.AsArray(), Is.Null); } + + [Test] + public void TestIdPoolCapacity() + { + var buffer = new RcRentedArray[RcRentedArray.START_RENT_ID_POOL_CAPACITY + 2]; + for (int i = 0; i < buffer.Length; i++) + { + Assert.DoesNotThrow(() => buffer[i] = RcRentedArray.Rent(4)); + } + + for (int i = 0; i < buffer.Length; i++) + { + Assert.DoesNotThrow(() => buffer[i].Dispose()); + } + } } \ No newline at end of file