diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs index 2e1d00c..c4be0a6 100644 --- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs @@ -34,6 +34,7 @@ namespace DotRecast.Detour.Crowd private RcVec3f m_target; private List m_path; + private int m_npath; private int m_maxPath; /** @@ -88,7 +89,8 @@ namespace DotRecast.Detour.Crowd /// @return True if the initialization succeeded. public bool Init(int maxPath) { - m_path = new List(); + m_path = new List(maxPath); + m_npath = 0; m_maxPath = maxPath; return true; } @@ -107,6 +109,7 @@ namespace DotRecast.Detour.Crowd m_target = pos; m_path.Clear(); m_path.Add(refs); + m_npath = 1; } /** @@ -215,7 +218,7 @@ namespace DotRecast.Detour.Crowd { if (res.Count > 1 && t > 0.99f) { - m_path = DtPathUtils.MergeCorridorStartShortcut(m_path, m_path.Count, m_maxPath, res); + m_npath = DtPathUtils.MergeCorridorStartShortcut(ref m_path, m_npath, m_maxPath, res); } } } @@ -247,7 +250,7 @@ namespace DotRecast.Detour.Crowd if (status.Succeeded() && res.Count > 0) { - m_path = DtPathUtils.MergeCorridorStartShortcut(m_path, m_path.Count, m_maxPath, res); + m_npath = DtPathUtils.MergeCorridorStartShortcut(ref m_path, m_npath, m_maxPath, res); return true; } @@ -274,6 +277,8 @@ namespace DotRecast.Detour.Crowd // Prune path m_path = m_path.GetRange(npos, m_path.Count - npos); + m_npath -= npos; + refs[0] = prevRef; refs[1] = polyRef; @@ -316,7 +321,7 @@ namespace DotRecast.Detour.Crowd var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, ref visited); if (status.Succeeded()) { - m_path = DtPathUtils.MergeCorridorStartMoved(m_path, m_path.Count, m_maxPath, visited); + m_npath = DtPathUtils.MergeCorridorStartMoved(ref m_path, m_npath, m_maxPath, visited); // Adjust the position to stay on top of the navmesh. m_pos = result; @@ -358,7 +363,8 @@ namespace DotRecast.Detour.Crowd var status = navquery.MoveAlongSurface(m_path[^1], m_target, npos, filter, out var result, ref visited); if (status.Succeeded()) { - m_path = DtPathUtils.MergeCorridorEndMoved(m_path, m_path.Count, m_maxPath, visited); + m_npath = DtPathUtils.MergeCorridorEndMoved(ref m_path, m_npath, m_maxPath, visited); + // TODO: should we do that? // Adjust the position to stay on top of the navmesh. /* @@ -386,6 +392,7 @@ namespace DotRecast.Detour.Crowd { m_target = target; m_path = new List(path); + m_npath = path.Count; } public void FixPathStart(long safeRef, RcVec3f safePos) @@ -393,17 +400,19 @@ namespace DotRecast.Detour.Crowd m_pos = safePos; if (m_path.Count < 3 && m_path.Count > 0) { - long p = m_path[m_path.Count - 1]; + long p = m_path[m_npath - 1]; m_path.Clear(); m_path.Add(safeRef); m_path.Add(0L); m_path.Add(p); + m_npath = 3; } else { m_path.Clear(); m_path.Add(safeRef); m_path.Add(0L); + m_npath = 2; } } @@ -427,11 +436,13 @@ namespace DotRecast.Detour.Crowd m_pos = RcVecUtils.Create(safePos); m_path.Clear(); m_path.Add(safeRef); + m_npath = 1; } else if (n < m_path.Count) { // The path is partially usable. m_path = m_path.GetRange(0, n); + m_npath = n; } // Clamp target pos to last poly diff --git a/src/DotRecast.Detour/DtPathUtils.cs b/src/DotRecast.Detour/DtPathUtils.cs index f1c37ae..998754a 100644 --- a/src/DotRecast.Detour/DtPathUtils.cs +++ b/src/DotRecast.Detour/DtPathUtils.cs @@ -20,6 +20,7 @@ freely, subject to the following restrictions: using System; using System.Collections.Generic; +using System.Linq; using DotRecast.Core.Numerics; namespace DotRecast.Detour @@ -88,11 +89,11 @@ namespace DotRecast.Detour // +-S-+-T-+ // |:::| | <-- the step can end up in here, resulting U-turn path. // +---+---+ - public static List FixupShortcuts(List path, int npath, DtNavMeshQuery navQuery) + public static int FixupShortcuts(ref List path, int npath, DtNavMeshQuery navQuery) { - if (path.Count < 3) + if (npath < 3) { - return path; + return npath; } // Get connected polygons @@ -103,7 +104,7 @@ namespace DotRecast.Detour var status = navQuery.GetAttachedNavMesh().GetTileAndPolyByRef(path[0], out var tile, out var poly); if (status.Failed()) { - return path; + return npath; } @@ -121,7 +122,7 @@ namespace DotRecast.Detour // in the path, short cut to that polygon directly. const int maxLookAhead = 6; int cut = 0; - for (int i = Math.Min(maxLookAhead, path.Count) - 1; i > 1 && cut == 0; i--) + for (int i = Math.Min(maxLookAhead, npath) - 1; i > 1 && cut == 0; i--) { for (int j = 0; j < nneis; j++) { @@ -137,20 +138,22 @@ namespace DotRecast.Detour { List shortcut = new List(); shortcut.Add(path[0]); - shortcut.AddRange(path.GetRange(cut, path.Count - cut)); - return shortcut; + shortcut.AddRange(path.GetRange(cut, npath - cut)); + + path = shortcut; + return shortcut.Count; } - return path; + return npath; } - public static List MergeCorridorStartMoved(List path, int npath, int maxPath, List visited) + public static int MergeCorridorStartMoved(ref List path, int npath, int maxPath, List visited) { int furthestPath = -1; int furthestVisited = -1; // Find furthest common polygon. - for (int i = path.Count - 1; i >= 0; --i) + for (int i = npath - 1; i >= 0; --i) { bool found = false; for (int j = visited.Count - 1; j >= 0; --j) @@ -172,7 +175,7 @@ namespace DotRecast.Detour // If no intersection found just return current path. if (furthestPath == -1 || furthestVisited == -1) { - return path; + return npath; } // Concatenate paths. @@ -185,17 +188,19 @@ namespace DotRecast.Detour result.Add(visited[i]); } - result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath)); - return result; + result.AddRange(path.GetRange(furthestPath, npath - furthestPath)); + + path = result; + return result.Count; } - public static List MergeCorridorEndMoved(List path, int npath, int maxPath, List visited) + public static int MergeCorridorEndMoved(ref List path, int npath, int maxPath, List visited) { int furthestPath = -1; int furthestVisited = -1; // Find furthest common polygon. - for (int i = 0; i < path.Count; ++i) + for (int i = 0; i < npath; ++i) { bool found = false; for (int j = visited.Count - 1; j >= 0; --j) @@ -217,22 +222,24 @@ namespace DotRecast.Detour // If no intersection found just return current path. if (furthestPath == -1 || furthestVisited == -1) { - return path; + return npath; } // Concatenate paths. List result = path.GetRange(0, furthestPath); result.AddRange(visited.GetRange(furthestVisited, visited.Count - furthestVisited)); - return result; + + path = result; + return result.Count; } - public static List MergeCorridorStartShortcut(List path, int npath, int maxPath, List visited) + public static int MergeCorridorStartShortcut(ref List path, int npath, int maxPath, List visited) { int furthestPath = -1; int furthestVisited = -1; // Find furthest common polygon. - for (int i = path.Count - 1; i >= 0; --i) + for (int i = npath - 1; i >= 0; --i) { bool found = false; for (int j = visited.Count - 1; j >= 0; --j) @@ -254,15 +261,17 @@ namespace DotRecast.Detour // If no intersection found just return current path. if (furthestPath == -1 || furthestVisited <= 0) { - return path; + return npath; } // Concatenate paths. // Adjust beginning of the buffer to include the visited. List result = visited.GetRange(0, furthestVisited); - result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath)); - return result; + result.AddRange(path.GetRange(furthestPath, npath - furthestPath)); + + path = result; + return result.Count; } } } \ No newline at end of file diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs index b9552dd..d692d46 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs @@ -661,7 +661,7 @@ public class TestNavmeshSampleTool : ISampleTool if (_mode == RcTestNavmeshToolMode.PATHFIND_FOLLOW) { _tool.FindFollowPath(navMesh, navQuery, m_startRef, m_endRef, m_spos, m_epos, m_filter, _enableRaycast, - ref m_polys, ref m_smoothPath); + ref m_polys, m_polys?.Count ?? 0, ref m_smoothPath); } else if (_mode == RcTestNavmeshToolMode.PATHFIND_STRAIGHT) { diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs index 1c77eea..11cbbf1 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs @@ -22,7 +22,7 @@ namespace DotRecast.Recast.Toolset.Tools } public DtStatus FindFollowPath(DtNavMesh navMesh, DtNavMeshQuery navQuery, long startRef, long endRef, RcVec3f startPt, RcVec3f endPt, IDtQueryFilter filter, bool enableRaycast, - ref List pathIterPolys, ref List smoothPath) + ref List pathIterPolys, int pathIterPolyCount, ref List smoothPath) { if (startRef == 0 || endRef == 0) { @@ -36,27 +36,32 @@ namespace DotRecast.Recast.Toolset.Tools smoothPath ??= new List(); 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 _); - float STEP_SIZE = 0.5f; - float SLOP = 0.01f; + const float STEP_SIZE = 0.5f; + const float SLOP = 0.01f; smoothPath.Clear(); smoothPath.Add(iterPos); var visited = new List(); + // Move towards target a small advancement at a time until target reached or // when ran out of memory to store the path. - while (0 < pathIterPolys.Count && smoothPath.Count < MAX_SMOOTH) + while (0 < pathIterPolyCount && smoothPath.Count < MAX_SMOOTH) { // Find location to steer towards. if (!DtPathUtils.GetSteerTarget(navQuery, iterPos, targetPos, SLOP, @@ -64,7 +69,7 @@ namespace DotRecast.Recast.Toolset.Tools { break; } - + bool endOfPath = (steerPosFlag & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0 ? true : false; @@ -84,7 +89,7 @@ namespace DotRecast.Recast.Toolset.Tools { len = STEP_SIZE / len; } - + RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len); // Move @@ -92,8 +97,8 @@ namespace DotRecast.Recast.Toolset.Tools iterPos = result; - pathIterPolys = DtPathUtils.MergeCorridorStartMoved(pathIterPolys, pathIterPolys.Count, MAX_POLYS, visited); - pathIterPolys = DtPathUtils.FixupShortcuts(pathIterPolys, pathIterPolys.Count, navQuery); + pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(ref pathIterPolys, pathIterPolyCount, MAX_POLYS, visited); + pathIterPolyCount = DtPathUtils.FixupShortcuts(ref pathIterPolys, pathIterPolyCount, navQuery); var status = navQuery.GetPolyHeight(pathIterPolys[0], result, out var h); if (status.Succeeded()) @@ -123,7 +128,7 @@ namespace DotRecast.Recast.Toolset.Tools long prevRef = 0; long polyRef = pathIterPolys[0]; int npos = 0; - while (npos < pathIterPolys.Count && polyRef != steerPosRef) + while (npos < pathIterPolyCount && polyRef != steerPosRef) { prevRef = polyRef; polyRef = pathIterPolys[npos]; @@ -131,6 +136,7 @@ namespace DotRecast.Recast.Toolset.Tools } pathIterPolys = pathIterPolys.GetRange(npos, pathIterPolys.Count - npos); + pathIterPolyCount -= npos; // Handle the connection. var status2 = navMesh.GetOffMeshConnectionPolyEndPoints(prevRef, polyRef, ref startPos, ref endPos);