From d0bec3714e9333954772614c653922aa0b1ad123 Mon Sep 17 00:00:00 2001 From: ikpil Date: Fri, 16 Feb 2024 00:26:23 +0900 Subject: [PATCH] added corridor maxpath for SOH issue - https://github.com/ikpil/DotRecast/issues/41 --- CHANGELOG.md | 1 + src/DotRecast.Detour.Crowd/DtCrowd.cs | 26 ++++----- src/DotRecast.Detour.Crowd/DtCrowdConfig.cs | 54 ++++--------------- src/DotRecast.Detour.Crowd/DtCrowdConst.cs | 4 ++ src/DotRecast.Detour.Crowd/DtPathCorridor.cs | 20 +++---- src/DotRecast.Detour.Crowd/DtPathQueue.cs | 23 ++++---- src/DotRecast.Detour/DtPathUtils.cs | 6 +-- .../Tools/RcTestNavMeshTool.cs | 36 ++++++------- .../PathCorridorTest.cs | 1 + 9 files changed, 73 insertions(+), 98 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0fc7b05..9518633 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Fixed ### Changed +- Added DtPathCorridor.Init(int maxPath) function to allow setting the maximum path [@ikpil](https://github.com/ikpil) ### Removed diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index eba48e3..3888e89 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -129,6 +129,7 @@ namespace DotRecast.Detour.Crowd private DtProximityGrid _grid; + private int _maxPathResult; private readonly RcVec3f _agentPlacementHalfExtents; private readonly IDtQueryFilter[] _filters; @@ -166,6 +167,7 @@ namespace DotRecast.Detour.Crowd } // Allocate temp buffer for merging paths. + _maxPathResult = 256; _pathQ = new DtPathQueue(config); _agents = new List(); @@ -223,19 +225,20 @@ namespace DotRecast.Detour.Crowd agent.option = option; } - /** - * Adds a new agent to the crowd. - * - * @param pos - * The requested position of the agent. [(x, y, z)] - * @param params - * The configuration of the agent. - * @return The newly created agent object - */ + /// @par + /// + /// The agent's position will be constrained to the surface of the navigation mesh. + /// Adds a new agent to the crowd. + /// @param[in] pos The requested position of the agent. [(x, y, z)] + /// @param[in] params The configuration of the agent. + /// @return The index of the agent in the agent pool. Or -1 if the agent could not be added. public DtCrowdAgent AddAgent(RcVec3f pos, DtCrowdAgentParams option) { - DtCrowdAgent ag = new DtCrowdAgent(_agentId.GetAndIncrement()); + int idx = _agentId.GetAndIncrement(); + DtCrowdAgent ag = new DtCrowdAgent(idx); + ag.corridor.Init(_maxPathResult); _agents.Add(ag); + UpdateAgentParameters(ag, option); // Find nearest position on navmesh and place the agent there. @@ -523,8 +526,7 @@ namespace DotRecast.Detour.Crowd replan = true; } - // If the end of the path is near and it is not the requested - // location, replan. + // If the end of the path is near and it is not the requested location, replan. if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_VALID) { if (ag.targetReplanTime > _config.targetReplanDelay && ag.corridor.GetPathCount() < _config.checkLookAhead diff --git a/src/DotRecast.Detour.Crowd/DtCrowdConfig.cs b/src/DotRecast.Detour.Crowd/DtCrowdConfig.cs index 251e22c..4424e24 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowdConfig.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowdConfig.cs @@ -23,52 +23,16 @@ namespace DotRecast.Detour.Crowd { public readonly float maxAgentRadius; - /** - * Max number of path requests in the queue - */ - public int pathQueueSize = 32; - - /** - * Max number of sliced path finding iterations executed per update (used to handle longer paths and replans) - */ - public int maxFindPathIterations = 100; - - /** - * Max number of sliced path finding iterations executed per agent to find the initial path to target - */ - public int maxTargetFindPathIterations = 20; - - /** - * Min time between topology optimizations (in seconds) - */ - public float topologyOptimizationTimeThreshold = 0.5f; - - /** - * The number of polygons from the beginning of the corridor to check to ensure path validity - */ - public int checkLookAhead = 10; - - /** - * Min time between target re-planning (in seconds) - */ - public float targetReplanDelay = 1.0f; - - /** - * Max number of sliced path finding iterations executed per topology optimization per agent - */ - public int maxTopologyOptimizationIterations = 32; - + 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 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) + public int maxTopologyOptimizationIterations = 32; // Max number of sliced path finding iterations executed per topology optimization per agent public float collisionResolveFactor = 0.7f; - - /** - * Max number of neighbour agents to consider in obstacle avoidance processing - */ - public int maxObstacleAvoidanceCircles = 6; - - /** - * Max number of neighbour segments to consider in obstacle avoidance processing - */ - public int maxObstacleAvoidanceSegments = 8; + public int maxObstacleAvoidanceCircles = 6; // Max number of neighbour agents to consider in obstacle avoidance processing + public int maxObstacleAvoidanceSegments = 8; // Max number of neighbour segments to consider in obstacle avoidance processing public DtCrowdConfig(float maxAgentRadius) { diff --git a/src/DotRecast.Detour.Crowd/DtCrowdConst.cs b/src/DotRecast.Detour.Crowd/DtCrowdConst.cs index c20e319..fc1a836 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowdConst.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowdConst.cs @@ -26,5 +26,9 @@ /// @see dtQueryFilter, dtCrowd::GetFilter() dtCrowd::GetEditableFilter(), /// dtCrowdAgentParams::queryFilterType public const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 16; + + public const int MAX_ITERS_PER_UPDATE = 100; + public const int MAX_PATHQUEUE_NODES = 4096; + public const int MAX_COMMON_NODES = 512; } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs index fb2a10d..e3a815d 100644 --- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs @@ -32,7 +32,9 @@ namespace DotRecast.Detour.Crowd { private RcVec3f m_pos; private RcVec3f m_target; + private List m_path; + private int m_maxPath; /** @class dtPathCorridor @@ -76,7 +78,6 @@ namespace DotRecast.Detour.Crowd */ public DtPathCorridor() { - m_path = new List(); } /// @par @@ -87,7 +88,8 @@ namespace DotRecast.Detour.Crowd /// @return True if the initialization succeeded. public bool Init(int maxPath) { - // ... + m_path = new List(); + m_maxPath = maxPath; return true; } @@ -101,10 +103,10 @@ namespace DotRecast.Detour.Crowd /// @param[in] pos The new position in the corridor. [(x, y, z)] public void Reset(long refs, RcVec3f pos) { - m_path.Clear(); - m_path.Add(refs); m_pos = pos; m_target = pos; + m_path.Clear(); + m_path.Add(refs); } /** @@ -131,7 +133,7 @@ namespace DotRecast.Detour.Crowd public int FindCorners(ref List corners, int maxCorners, DtNavMeshQuery navquery, IDtQueryFilter filter) { const float MIN_TARGET_DIST = 0.01f; - + var result = navquery.FindStraightPath(m_pos, m_target, m_path, ref corners, maxCorners, 0); if (result.Succeeded()) { @@ -213,7 +215,7 @@ namespace DotRecast.Detour.Crowd { if (res.Count > 1 && t > 0.99f) { - m_path = DtPathUtils.MergeCorridorStartShortcut(m_path, res); + m_path = DtPathUtils.MergeCorridorStartShortcut(m_path, m_maxPath, res); } } } @@ -245,7 +247,7 @@ namespace DotRecast.Detour.Crowd if (status.Succeeded() && res.Count > 0) { - m_path = DtPathUtils.MergeCorridorStartShortcut(m_path, res); + m_path = DtPathUtils.MergeCorridorStartShortcut(m_path, m_maxPath, res); return true; } @@ -314,7 +316,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, visited); + m_path = DtPathUtils.MergeCorridorStartMoved(m_path, m_maxPath, visited); // Adjust the position to stay on top of the navmesh. m_pos = result; @@ -356,7 +358,7 @@ namespace DotRecast.Detour.Crowd var status = navquery.MoveAlongSurface(m_path[m_path.Count - 1], m_target, npos, filter, out var result, ref visited); if (status.Succeeded()) { - m_path = DtPathUtils.MergeCorridorEndMoved(m_path, visited); + m_path = DtPathUtils.MergeCorridorEndMoved(m_path, m_maxPath, visited); // TODO: should we do that? // Adjust the position to stay on top of the navmesh. /* diff --git a/src/DotRecast.Detour.Crowd/DtPathQueue.cs b/src/DotRecast.Detour.Crowd/DtPathQueue.cs index 5e8843b..fed69d9 100644 --- a/src/DotRecast.Detour.Crowd/DtPathQueue.cs +++ b/src/DotRecast.Detour.Crowd/DtPathQueue.cs @@ -26,28 +26,29 @@ namespace DotRecast.Detour.Crowd { public class DtPathQueue { - private readonly DtCrowdConfig config; - private readonly LinkedList queue = new LinkedList(); + private readonly DtCrowdConfig m_config; + private readonly LinkedList m_queue; public DtPathQueue(DtCrowdConfig config) { - this.config = config; + m_config = config; + m_queue = new LinkedList(); } public void Update(DtNavMesh navMesh) { - // Update path request until there is nothing to update or up to maxIters pathfinder iterations has been - // consumed. - int iterCount = config.maxFindPathIterations; + // Update path request until there is nothing to update + // or upto maxIters pathfinder iterations has been consumed. + int iterCount = m_config.maxFindPathIterations; while (iterCount > 0) { - DtPathQuery q = queue.First?.Value; + DtPathQuery q = m_queue.First?.Value; if (q == null) { break; } - queue.RemoveFirst(); + m_queue.RemoveFirst(); // Handle query start. if (q.result.status.IsEmpty()) @@ -70,14 +71,14 @@ namespace DotRecast.Detour.Crowd if (!(q.result.status.Failed() || q.result.status.Succeeded())) { - queue.AddFirst(q); + m_queue.AddFirst(q); } } } public DtPathQueryResult Request(long startRef, long endRef, RcVec3f startPos, RcVec3f endPos, IDtQueryFilter filter) { - if (queue.Count >= config.pathQueueSize) + if (m_queue.Count >= m_config.pathQueueSize) { return null; } @@ -88,7 +89,7 @@ namespace DotRecast.Detour.Crowd q.endPos = endPos; q.endRef = endRef; q.filter = filter; - queue.AddLast(q); + m_queue.AddLast(q); return q.result; } } diff --git a/src/DotRecast.Detour/DtPathUtils.cs b/src/DotRecast.Detour/DtPathUtils.cs index 8b0845f..1e47677 100644 --- a/src/DotRecast.Detour/DtPathUtils.cs +++ b/src/DotRecast.Detour/DtPathUtils.cs @@ -141,7 +141,7 @@ namespace DotRecast.Detour return path; } - public static List MergeCorridorStartMoved(List path, List visited) + public static List MergeCorridorStartMoved(List path, int maxPath, List visited) { int furthestPath = -1; int furthestVisited = -1; @@ -186,7 +186,7 @@ namespace DotRecast.Detour return result; } - public static List MergeCorridorEndMoved(List path, List visited) + public static List MergeCorridorEndMoved(List path, int maxPath, List visited) { int furthestPath = -1; int furthestVisited = -1; @@ -223,7 +223,7 @@ namespace DotRecast.Detour return result; } - public static List MergeCorridorStartShortcut(List path, List visited) + public static List MergeCorridorStartShortcut(List path, int maxPath, List visited) { int furthestPath = -1; int furthestVisited = -1; diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs index 62d20c7..1e48e14 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs @@ -22,30 +22,30 @@ 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 polys, ref List smoothPath) + ref List pathIterPolys, ref List smoothPath) { if (startRef == 0 || endRef == 0) { - polys?.Clear(); + pathIterPolys?.Clear(); smoothPath?.Clear(); return DtStatus.DT_FAILURE; } - polys ??= new List(); + pathIterPolys ??= new List(); smoothPath ??= new List(); - polys.Clear(); + pathIterPolys.Clear(); smoothPath.Clear(); var opt = new DtFindPathOption(enableRaycast ? DtFindPathOptions.DT_FINDPATH_ANY_ANGLE : 0, float.MaxValue); - navQuery.FindPath(startRef, endRef, startPt, endPt, filter, ref polys, opt); - if (0 >= polys.Count) + navQuery.FindPath(startRef, endRef, startPt, endPt, filter, ref pathIterPolys, opt); + if (0 >= pathIterPolys.Count) return DtStatus.DT_FAILURE; // Iterate over the path to find smooth path on the detail mesh surface. navQuery.ClosestPointOnPoly(startRef, startPt, out var iterPos, out var _); - navQuery.ClosestPointOnPoly(polys[polys.Count - 1], endPt, out var targetPos, out var _); + navQuery.ClosestPointOnPoly(pathIterPolys[pathIterPolys.Count - 1], endPt, out var targetPos, out var _); float STEP_SIZE = 0.5f; float SLOP = 0.01f; @@ -56,11 +56,11 @@ namespace DotRecast.Recast.Toolset.Tools // 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 && smoothPath.Count < MAX_SMOOTH) + while (0 < pathIterPolys.Count && smoothPath.Count < MAX_SMOOTH) { // Find location to steer towards. if (!DtPathUtils.GetSteerTarget(navQuery, iterPos, targetPos, SLOP, - polys, out var steerPos, out var steerPosFlag, out var steerPosRef)) + pathIterPolys, out var steerPos, out var steerPosFlag, out var steerPosRef)) { break; } @@ -88,14 +88,14 @@ namespace DotRecast.Recast.Toolset.Tools RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len); // Move - navQuery.MoveAlongSurface(polys[0], iterPos, moveTgt, filter, out var result, ref visited); + navQuery.MoveAlongSurface(pathIterPolys[0], iterPos, moveTgt, filter, out var result, ref visited); iterPos = result; - polys = DtPathUtils.MergeCorridorStartMoved(polys, visited); - polys = DtPathUtils.FixupShortcuts(polys, navQuery); + pathIterPolys = DtPathUtils.MergeCorridorStartMoved(pathIterPolys, MAX_POLYS, visited); + pathIterPolys = DtPathUtils.FixupShortcuts(pathIterPolys, navQuery); - var status = navQuery.GetPolyHeight(polys[0], result, out var h); + var status = navQuery.GetPolyHeight(pathIterPolys[0], result, out var h); if (status.Succeeded()) { iterPos.Y = h; @@ -121,16 +121,16 @@ namespace DotRecast.Recast.Toolset.Tools // Advance the path up to and over the off-mesh connection. long prevRef = 0; - long polyRef = polys[0]; + long polyRef = pathIterPolys[0]; int npos = 0; - while (npos < polys.Count && polyRef != steerPosRef) + while (npos < pathIterPolys.Count && polyRef != steerPosRef) { prevRef = polyRef; - polyRef = polys[npos]; + polyRef = pathIterPolys[npos]; npos++; } - polys = polys.GetRange(npos, polys.Count - npos); + pathIterPolys = pathIterPolys.GetRange(npos, pathIterPolys.Count - npos); // Handle the connection. var status2 = navMesh.GetOffMeshConnectionPolyEndPoints(prevRef, polyRef, ref startPos, ref endPos); @@ -148,7 +148,7 @@ namespace DotRecast.Recast.Toolset.Tools // Move position at the other side of the off-mesh link. iterPos = endPos; - navQuery.GetPolyHeight(polys[0], iterPos, out var eh); + navQuery.GetPolyHeight(pathIterPolys[0], iterPos, out var eh); iterPos.Y = eh; } } diff --git a/test/DotRecast.Detour.Crowd.Test/PathCorridorTest.cs b/test/DotRecast.Detour.Crowd.Test/PathCorridorTest.cs index 45ef3cb..fdc10c1 100644 --- a/test/DotRecast.Detour.Crowd.Test/PathCorridorTest.cs +++ b/test/DotRecast.Detour.Crowd.Test/PathCorridorTest.cs @@ -34,6 +34,7 @@ public class PathCorridorTest [SetUp] public void SetUp() { + corridor.Init(256); corridor.Reset(0, new RcVec3f(10, 20, 30)); }