diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs index f2e9700..76020ba 100644 --- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs @@ -317,11 +317,12 @@ namespace DotRecast.Detour.Crowd public bool MovePosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFilter filter) { // Move along navmesh and update new position. - var visited = new List(); - var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, ref visited); + const int MAX_VISITED = 16; + Span visited = stackalloc long[MAX_VISITED]; + 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, visited.Count); + m_npath = DtPathUtils.MergeCorridorStartMoved(ref m_path, m_npath, m_maxPath, visited, nvisited); // Adjust the position to stay on top of the navmesh. m_pos = result; @@ -359,11 +360,13 @@ namespace DotRecast.Detour.Crowd public bool MoveTargetPosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFilter filter) { // Move along navmesh and update new position. - var visited = new List(); - var status = navquery.MoveAlongSurface(m_path[^1], m_target, npos, filter, out var result, ref visited); + const int MAX_VISITED = 16; + Span visited = stackalloc long[MAX_VISITED]; + int nvisited = 0; + 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, visited.Count); + m_npath = DtPathUtils.MergeCorridorEndMoved(ref m_path, m_npath, m_maxPath, visited, nvisited); // TODO: should we do that? // Adjust the position to stay on top of the navmesh. diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index b3a6ab0..55dad90 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -1790,12 +1790,11 @@ namespace DotRecast.Detour /// @returns The status flags for the query. public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos, IDtQueryFilter filter, - out RcVec3f resultPos, ref List visited) + out RcVec3f resultPos, Span visited, out int visitedCount, int maxVisitedSize) { resultPos = RcVec3f.Zero; - if (null != visited) - visited.Clear(); + visitedCount = 0; // Validate input if (!m_nav.IsValidPolyRef(startRef) || !startPos.IsFinite() @@ -1804,6 +1803,8 @@ namespace DotRecast.Detour return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } + DtStatus status = DtStatus.DT_SUCCESS; + m_tinyNodePool.Clear(); DtNode startNode = m_tinyNodePool.GetNode(startRef); @@ -1938,6 +1939,7 @@ namespace DotRecast.Detour } } + int n = 0; if (bestNode != null) { // Reverse the path. @@ -1955,14 +1957,21 @@ namespace DotRecast.Detour node = prev; do { - visited.Add(node.id); + visited[n++] = node.id; + if (n >= maxVisitedSize) + { + status |= DtStatus.DT_BUFFER_TOO_SMALL;; + break; + } + node = m_tinyNodePool.GetNodeAtIdx(node.pidx); } while (node != null); } resultPos = bestPos; + visitedCount = n; - return DtStatus.DT_SUCCESS; + return status; } protected DtStatus GetPortalPoints(long from, long to, out RcVec3f left, out RcVec3f right, out int fromType, out int toType) diff --git a/src/DotRecast.Detour/DtPathUtils.cs b/src/DotRecast.Detour/DtPathUtils.cs index 9fb31a2..fea9700 100644 --- a/src/DotRecast.Detour/DtPathUtils.cs +++ b/src/DotRecast.Detour/DtPathUtils.cs @@ -146,7 +146,7 @@ namespace DotRecast.Detour return npath; } - public static int MergeCorridorStartMoved(ref List path, int npath, int maxPath, List visited, int nvisited) + public static int MergeCorridorStartMoved(ref List path, int npath, int maxPath, Span visited, int nvisited) { int furthestPath = -1; int furthestVisited = -1; @@ -193,7 +193,7 @@ namespace DotRecast.Detour return result.Count; } - public static int MergeCorridorEndMoved(ref List path, int npath, int maxPath, List visited, int nvisited) + public static int MergeCorridorEndMoved(ref List path, int npath, int maxPath, Span visited, int nvisited) { int furthestPath = -1; int furthestVisited = -1; @@ -226,7 +226,10 @@ namespace DotRecast.Detour // Concatenate paths. List result = path.GetRange(0, furthestPath); - result.AddRange(visited.GetRange(furthestVisited, nvisited - furthestVisited)); + foreach (var v in visited.Slice(furthestVisited, nvisited - furthestVisited)) + { + result.Add(v); + } path = result; return result.Count; diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs index 0b226fe..ef8b369 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs @@ -37,16 +37,16 @@ namespace DotRecast.Recast.Toolset.Tools pathIterPolys.Clear(); pathIterPolyCount = 0; - + smoothPath.Clear(); var opt = new DtFindPathOption(enableRaycast ? DtFindPathOptions.DT_FINDPATH_ANY_ANGLE : 0, float.MaxValue); navQuery.FindPath(startRef, endRef, startPt, endPt, filter, ref pathIterPolys, opt); if (0 >= pathIterPolys.Count) return DtStatus.DT_FAILURE; - + pathIterPolyCount = pathIterPolys.Count; - + // Iterate over the path to find smooth path on the detail mesh surface. navQuery.ClosestPointOnPoly(startRef, startPt, out var iterPos, out var _); navQuery.ClosestPointOnPoly(pathIterPolys[pathIterPolys.Count - 1], endPt, out var targetPos, out var _); @@ -56,8 +56,10 @@ namespace DotRecast.Recast.Toolset.Tools smoothPath.Clear(); smoothPath.Add(iterPos); - var visited = new List(); - + + Span visited = stackalloc long[16]; + int nvisited = 0; + // Move towards target a small advancement at a time until target reached or // when ran out of memory to store the path. @@ -69,7 +71,7 @@ namespace DotRecast.Recast.Toolset.Tools { break; } - + bool endOfPath = (steerPosFlag & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0 ? true : false; @@ -89,15 +91,15 @@ namespace DotRecast.Recast.Toolset.Tools { len = STEP_SIZE / len; } - + RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len); // Move - navQuery.MoveAlongSurface(pathIterPolys[0], iterPos, moveTgt, filter, out var result, ref visited); + navQuery.MoveAlongSurface(pathIterPolys[0], iterPos, moveTgt, filter, out var result, visited, out nvisited, 16); iterPos = result; - pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(ref pathIterPolys, pathIterPolyCount, MAX_POLYS, visited, visited.Count); + pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(ref pathIterPolys, pathIterPolyCount, MAX_POLYS, visited, nvisited); pathIterPolyCount = DtPathUtils.FixupShortcuts(ref pathIterPolys, pathIterPolyCount, navQuery); var status = navQuery.GetPolyHeight(pathIterPolys[0], result, out var h); diff --git a/test/DotRecast.Detour.Test/MoveAlongSurfaceTest.cs b/test/DotRecast.Detour.Test/MoveAlongSurfaceTest.cs index 4a84028..26179a1 100644 --- a/test/DotRecast.Detour.Test/MoveAlongSurfaceTest.cs +++ b/test/DotRecast.Detour.Test/MoveAlongSurfaceTest.cs @@ -16,13 +16,13 @@ freely, subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +using System; using System.Collections.Generic; using DotRecast.Core.Numerics; using NUnit.Framework; namespace DotRecast.Detour.Test; - public class MoveAlongSurfaceTest : AbstractDetourTest { private static readonly long[][] VISITED = @@ -69,20 +69,21 @@ public class MoveAlongSurfaceTest : AbstractDetourTest public void TestMoveAlongSurface() { IDtQueryFilter filter = new DtQueryDefaultFilter(); - var visited = new List(); + const int MAX_VISITED = 32; + Span visited = stackalloc long[MAX_VISITED]; for (int i = 0; i < startRefs.Length; i++) { long startRef = startRefs[i]; RcVec3f startPos = startPoss[i]; RcVec3f endPos = endPoss[i]; - var status = query.MoveAlongSurface(startRef, startPos, endPos, filter, out var result, ref visited); + var status = query.MoveAlongSurface(startRef, startPos, endPos, filter, out var result, visited, out var nvisited, MAX_VISITED); Assert.That(status.Succeeded(), Is.True); Assert.That(result.X, Is.EqualTo(POSITION[i].X).Within(0.01f)); Assert.That(result.Y, Is.EqualTo(POSITION[i].Y).Within(0.01f)); Assert.That(result.Z, Is.EqualTo(POSITION[i].Z).Within(0.01f)); - Assert.That(visited.Count, Is.EqualTo(VISITED[i].Length)); + Assert.That(nvisited, Is.EqualTo(VISITED[i].Length)); for (int j = 0; j < 3; j++) { Assert.That(visited[j], Is.EqualTo(VISITED[i][j]));