added corridor maxpath for SOH issue

- https://github.com/ikpil/DotRecast/issues/41

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
 

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<DtCrowdAgent>();
 
@@ -223,19 +225,20 @@ namespace DotRecast.Detour.Crowd
             agent.option = option;
         }
 
-        /**
+        /// @par
-     * Adds a new agent to the crowd.
+        ///
-     *
+        /// The agent's position will be constrained to the surface of the navigation mesh.
-     * @param pos
+        /// Adds a new agent to the crowd.
-     *            The requested position of the agent. [(x, y, z)]
+        ///  @param[in]		pos		The requested position of the agent. [(x, y, z)]
-     * @param params
+        ///  @param[in]		params	The configuration of the agent.
-     *            The configuration of the agent.
+        /// @return The index of the agent in the agent pool. Or -1 if the agent could not be added.
-     * @return The newly created agent object
-     */
         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
+                // If the end of the path is near and it is not the requested location, replan.
-                // location, replan.
                 if (ag.targetState == DtMoveRequestState.DT_CROWDAGENT_TARGET_VALID)
                 {
                     if (ag.targetReplanTime > _config.targetReplanDelay && ag.corridor.GetPathCount() < _config.checkLookAhead

src/DotRecast.Detour.Crowd/DtCrowdConfig.cs

@@ -23,52 +23,16 @@ namespace DotRecast.Detour.Crowd
     {
         public readonly float maxAgentRadius;
 
-        /**
+        public int pathQueueSize = 32; // Max number of path requests in the queue
-     * 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 int pathQueueSize = 32;
+        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)
-     * Max number of sliced path finding iterations executed per update (used to handle longer paths and replans)
+        public int maxTopologyOptimizationIterations = 32; // Max number of sliced path finding iterations executed per topology optimization per agent
-     */
-        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 float collisionResolveFactor = 0.7f;
-
+        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
-     * 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 DtCrowdConfig(float maxAgentRadius)
         {

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;
     }
 }

src/DotRecast.Detour.Crowd/DtPathCorridor.cs

@@ -32,7 +32,9 @@ namespace DotRecast.Detour.Crowd
     {
         private RcVec3f m_pos;
         private RcVec3f m_target;
+
         private List<long> m_path;
+        private int m_maxPath;
 
         /**
         @class dtPathCorridor
@@ -76,7 +78,6 @@ namespace DotRecast.Detour.Crowd
         */
         public DtPathCorridor()
         {
-            m_path = new List<long>();
         }
 
         /// @par
@@ -87,7 +88,8 @@ namespace DotRecast.Detour.Crowd
         /// @return True if the initialization succeeded.
         public bool Init(int maxPath)
         {
-            // ...
+            m_path = new List<long>();
+            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<DtStraightPath> 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.
                 /*

src/DotRecast.Detour.Crowd/DtPathQueue.cs

@@ -26,28 +26,29 @@ namespace DotRecast.Detour.Crowd
 {
     public class DtPathQueue
     {
-        private readonly DtCrowdConfig config;
+        private readonly DtCrowdConfig m_config;
-        private readonly LinkedList<DtPathQuery> queue = new LinkedList<DtPathQuery>();
+        private readonly LinkedList<DtPathQuery> m_queue;
 
         public DtPathQueue(DtCrowdConfig config)
         {
-            this.config = config;
+            m_config = config;
+            m_queue = new LinkedList<DtPathQuery>();
         }
 
         public void Update(DtNavMesh navMesh)
         {
-            // Update path request until there is nothing to update or up to maxIters pathfinder iterations has been
+            // Update path request until there is nothing to update
-            // consumed.
+            // or upto maxIters pathfinder iterations has been consumed.
-            int iterCount = config.maxFindPathIterations;
+            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;
         }
     }

src/DotRecast.Detour/DtPathUtils.cs

@@ -141,7 +141,7 @@ namespace DotRecast.Detour
             return path;
         }
 
-        public static List<long> MergeCorridorStartMoved(List<long> path, List<long> visited)
+        public static List<long> MergeCorridorStartMoved(List<long> path, int maxPath, List<long> visited)
         {
             int furthestPath = -1;
             int furthestVisited = -1;
@@ -186,7 +186,7 @@ namespace DotRecast.Detour
             return result;
         }
 
-        public static List<long> MergeCorridorEndMoved(List<long> path, List<long> visited)
+        public static List<long> MergeCorridorEndMoved(List<long> path, int maxPath, List<long> visited)
         {
             int furthestPath = -1;
             int furthestVisited = -1;
@@ -223,7 +223,7 @@ namespace DotRecast.Detour
             return result;
         }
 
-        public static List<long> MergeCorridorStartShortcut(List<long> path, List<long> visited)
+        public static List<long> MergeCorridorStartShortcut(List<long> path, int maxPath, List<long> visited)
         {
             int furthestPath = -1;
             int furthestVisited = -1;

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<long> polys, ref List<RcVec3f> smoothPath)
+            ref List<long> pathIterPolys, ref List<RcVec3f> smoothPath)
         {
             if (startRef == 0 || endRef == 0)
             {
-                polys?.Clear();
+                pathIterPolys?.Clear();
                 smoothPath?.Clear();
 
                 return DtStatus.DT_FAILURE;
             }
 
-            polys ??= new List<long>();
+            pathIterPolys ??= new List<long>();
             smoothPath ??= new List<RcVec3f>();
 
-            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);
+            navQuery.FindPath(startRef, endRef, startPt, endPt, filter, ref pathIterPolys, opt);
-            if (0 >= polys.Count)
+            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);
+                pathIterPolys = DtPathUtils.MergeCorridorStartMoved(pathIterPolys, MAX_POLYS, visited);
-                polys = DtPathUtils.FixupShortcuts(polys, navQuery);
+                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;
                     }
                 }

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));
     }