diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs index e338797..0a8a02c 100644 --- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs @@ -70,126 +70,6 @@ namespace DotRecast.Detour.Crowd private RcVec3f m_target = new RcVec3f(); private List m_path; - protected List MergeCorridorStartMoved(List path, List visited) - { - int furthestPath = -1; - int furthestVisited = -1; - - // Find furthest common polygon. - for (int i = path.Count - 1; i >= 0; --i) - { - bool found = false; - for (int j = visited.Count - 1; j >= 0; --j) - { - if (path[i] == visited[j]) - { - furthestPath = i; - furthestVisited = j; - found = true; - } - } - - if (found) - { - break; - } - } - - // If no intersection found just return current path. - if (furthestPath == -1 || furthestVisited == -1) - { - return path; - } - - // Concatenate paths. - - // Adjust beginning of the buffer to include the visited. - List result = new List(); - // Store visited - for (int i = visited.Count - 1; i > furthestVisited; --i) - { - result.Add(visited[i]); - } - - result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath)); - return result; - } - - protected List MergeCorridorEndMoved(List path, List visited) - { - int furthestPath = -1; - int furthestVisited = -1; - - // Find furthest common polygon. - for (int i = 0; i < path.Count; ++i) - { - bool found = false; - for (int j = visited.Count - 1; j >= 0; --j) - { - if (path[i] == visited[j]) - { - furthestPath = i; - furthestVisited = j; - found = true; - } - } - - if (found) - { - break; - } - } - - // If no intersection found just return current path. - if (furthestPath == -1 || furthestVisited == -1) - { - return path; - } - - // Concatenate paths. - List result = path.GetRange(0, furthestPath); - result.AddRange(visited.GetRange(furthestVisited, visited.Count - furthestVisited)); - return result; - } - - protected List MergeCorridorStartShortcut(List path, List visited) - { - int furthestPath = -1; - int furthestVisited = -1; - - // Find furthest common polygon. - for (int i = path.Count - 1; i >= 0; --i) - { - bool found = false; - for (int j = visited.Count - 1; j >= 0; --j) - { - if (path[i] == visited[j]) - { - furthestPath = i; - furthestVisited = j; - found = true; - } - } - - if (found) - { - break; - } - } - - // If no intersection found just return current path. - if (furthestPath == -1 || furthestVisited <= 0) - { - return path; - } - - // 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; - } /** * Allocates the corridor's path buffer. @@ -321,7 +201,7 @@ namespace DotRecast.Detour.Crowd { if (rayHit.path.Count > 1 && rayHit.t > 0.99f) { - m_path = MergeCorridorStartShortcut(m_path, rayHit.path); + m_path = PathUtils.MergeCorridorStartShortcut(m_path, rayHit.path); } } } @@ -356,7 +236,7 @@ namespace DotRecast.Detour.Crowd if (status.Succeeded() && res.Count > 0) { - m_path = MergeCorridorStartShortcut(m_path, res); + m_path = PathUtils.MergeCorridorStartShortcut(m_path, res); return true; } @@ -387,7 +267,7 @@ namespace DotRecast.Detour.Crowd refs[1] = polyRef; DtNavMesh nav = navquery.GetAttachedNavMesh(); - var startEnd = nav.GetOffMeshConnectionPolyEndPoints(refs[0], refs[1], ref startPos, ref endPos); + var startEnd = nav.GetOffMeshConnectionPolyEndPoints(refs[0], refs[1], ref startPos, ref endPos); if (startEnd.Succeeded()) { m_pos = endPos; @@ -426,7 +306,7 @@ namespace DotRecast.Detour.Crowd var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, out var visited); if (status.Succeeded()) { - m_path = MergeCorridorStartMoved(m_path, visited); + m_path = PathUtils.MergeCorridorStartMoved(m_path, visited); // Adjust the position to stay on top of the navmesh. m_pos = result; @@ -465,7 +345,7 @@ namespace DotRecast.Detour.Crowd var status = navquery.MoveAlongSurface(m_path[m_path.Count - 1], m_target, npos, filter, out var result, out var visited); if (status.Succeeded()) { - m_path = MergeCorridorEndMoved(m_path, visited); + m_path = PathUtils.MergeCorridorEndMoved(m_path, visited); // TODO: should we do that? // Adjust the position to stay on top of the navmesh. /* diff --git a/src/DotRecast.Detour/PathUtils.cs b/src/DotRecast.Detour/PathUtils.cs index b8bc2a2..0a9f400 100644 --- a/src/DotRecast.Detour/PathUtils.cs +++ b/src/DotRecast.Detour/PathUtils.cs @@ -83,53 +83,6 @@ namespace DotRecast.Detour return (dx * dx + dz * dz) < r * r && Math.Abs(dy) < h; } - public static List FixupCorridor(List path, List visited) - { - int furthestPath = -1; - int furthestVisited = -1; - - // Find furthest common polygon. - for (int i = path.Count - 1; i >= 0; --i) - { - bool found = false; - for (int j = visited.Count - 1; j >= 0; --j) - { - if (path[i] == visited[j]) - { - furthestPath = i; - furthestVisited = j; - found = true; - } - } - - if (found) - break; - } - - // If no intersection found just return current path. - if (furthestPath == -1 || furthestVisited == -1) - return path; - - // Concatenate paths. - - // Adjust beginning of the buffer to include the visited. - int req = visited.Count - furthestVisited; - int orig = Math.Min(furthestPath + 1, path.Count); - int size = Math.Max(0, path.Count - orig); - List fixupPath = new List(); - // Store visited - for (int i = 0; i < req; ++i) - { - fixupPath.Add(visited[(visited.Count - 1) - i]); - } - - for (int i = 0; i < size; i++) - { - fixupPath.Add(path[orig + i]); - } - - return fixupPath; - } // This function checks if the path has a small U-turn, that is, // a polygon further in the path is adjacent to the first polygon @@ -194,5 +147,126 @@ namespace DotRecast.Detour return path; } + + public static List MergeCorridorStartMoved(List path, List visited) + { + int furthestPath = -1; + int furthestVisited = -1; + + // Find furthest common polygon. + for (int i = path.Count - 1; i >= 0; --i) + { + bool found = false; + for (int j = visited.Count - 1; j >= 0; --j) + { + if (path[i] == visited[j]) + { + furthestPath = i; + furthestVisited = j; + found = true; + } + } + + if (found) + { + break; + } + } + + // If no intersection found just return current path. + if (furthestPath == -1 || furthestVisited == -1) + { + return path; + } + + // Concatenate paths. + + // Adjust beginning of the buffer to include the visited. + List result = new List(); + // Store visited + for (int i = visited.Count - 1; i > furthestVisited; --i) + { + result.Add(visited[i]); + } + + result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath)); + return result; + } + + public static List MergeCorridorEndMoved(List path, List visited) + { + int furthestPath = -1; + int furthestVisited = -1; + + // Find furthest common polygon. + for (int i = 0; i < path.Count; ++i) + { + bool found = false; + for (int j = visited.Count - 1; j >= 0; --j) + { + if (path[i] == visited[j]) + { + furthestPath = i; + furthestVisited = j; + found = true; + } + } + + if (found) + { + break; + } + } + + // If no intersection found just return current path. + if (furthestPath == -1 || furthestVisited == -1) + { + return path; + } + + // Concatenate paths. + List result = path.GetRange(0, furthestPath); + result.AddRange(visited.GetRange(furthestVisited, visited.Count - furthestVisited)); + return result; + } + + public static List MergeCorridorStartShortcut(List path, List visited) + { + int furthestPath = -1; + int furthestVisited = -1; + + // Find furthest common polygon. + for (int i = path.Count - 1; i >= 0; --i) + { + bool found = false; + for (int j = visited.Count - 1; j >= 0; --j) + { + if (path[i] == visited[j]) + { + furthestPath = i; + furthestVisited = j; + found = true; + } + } + + if (found) + { + break; + } + } + + // If no intersection found just return current path. + if (furthestPath == -1 || furthestVisited <= 0) + { + return path; + } + + // 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; + } } } \ No newline at end of file diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index ce2bd8f..0554b48 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -2,7 +2,6 @@ using System; using System.Collections.Generic; using DotRecast.Core; using DotRecast.Detour; - using DotRecast.Recast.DemoTool.Builder; using DotRecast.Recast.Demo.Draw; using DotRecast.Recast.DemoTool; @@ -200,10 +199,9 @@ public class TestNavmeshTool : IRcTool new(enableRaycast ? DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE : 0, float.MaxValue)); if (0 < m_polys.Count) { - List polys = new(m_polys); // Iterate over the path to find smooth path on the detail mesh surface. m_navQuery.ClosestPointOnPoly(m_startRef, m_spos, out var iterPos, out var _); - m_navQuery.ClosestPointOnPoly(polys[polys.Count - 1], m_epos, out var targetPos, out var _); + m_navQuery.ClosestPointOnPoly(m_polys[m_polys.Count - 1], m_epos, out var targetPos, out var _); float STEP_SIZE = 0.5f; float SLOP = 0.01f; @@ -213,11 +211,10 @@ public class TestNavmeshTool : IRcTool // Move towards target a small advancement at a time until target reached or // when ran out of memory to store the path. - while (0 < polys.Count && m_smoothPath.Count < MAX_SMOOTH) + while (0 < m_polys.Count && m_smoothPath.Count < MAX_SMOOTH) { // Find location to steer towards. - SteerTarget steerTarget = PathUtils.GetSteerTarget(m_navQuery, iterPos, targetPos, - SLOP, polys); + SteerTarget steerTarget = PathUtils.GetSteerTarget(m_navQuery, iterPos, targetPos, SLOP, m_polys); if (null == steerTarget) { break; @@ -247,14 +244,14 @@ public class TestNavmeshTool : IRcTool RcVec3f moveTgt = RcVec3f.Mad(iterPos, delta, len); // Move - m_navQuery.MoveAlongSurface(polys[0], iterPos, moveTgt, m_filter, out var result, out var visited); + m_navQuery.MoveAlongSurface(m_polys[0], iterPos, moveTgt, m_filter, out var result, out var visited); iterPos = result; - polys = PathUtils.FixupCorridor(polys, visited); - polys = PathUtils.FixupShortcuts(polys, m_navQuery); + m_polys = PathUtils.MergeCorridorStartMoved(m_polys, visited); + m_polys = PathUtils.FixupShortcuts(m_polys, m_navQuery); - var status = m_navQuery.GetPolyHeight(polys[0], result, out var h); + var status = m_navQuery.GetPolyHeight(m_polys[0], result, out var h); if (status.Succeeded()) { iterPos.y = h; @@ -280,16 +277,16 @@ public class TestNavmeshTool : IRcTool // Advance the path up to and over the off-mesh connection. long prevRef = 0; - long polyRef = polys[0]; + long polyRef = m_polys[0]; int npos = 0; - while (npos < polys.Count && polyRef != steerTarget.steerPosRef) + while (npos < m_polys.Count && polyRef != steerTarget.steerPosRef) { prevRef = polyRef; - polyRef = polys[npos]; + polyRef = m_polys[npos]; npos++; } - polys = polys.GetRange(npos, polys.Count - npos); + m_polys = m_polys.GetRange(npos, m_polys.Count - npos); // Handle the connection. var status2 = m_navMesh.GetOffMeshConnectionPolyEndPoints(prevRef, polyRef, ref startPos, ref endPos); @@ -307,7 +304,7 @@ public class TestNavmeshTool : IRcTool // Move position at the other side of the off-mesh link. iterPos = endPos; - m_navQuery.GetPolyHeight(polys[0], iterPos, out var eh); + m_navQuery.GetPolyHeight(m_polys[0], iterPos, out var eh); iterPos.y = eh; } } @@ -357,6 +354,7 @@ public class TestNavmeshTool : IRcTool { m_polys = null; m_straightPath = null; + if (m_sposSet && m_eposSet && m_startRef != 0 && m_endRef != 0) { m_pathFindStatus = m_navQuery.InitSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, m_filter,