added corridor maxpath for SOH issue

- #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 @@ public class DtCrowd
 
         private DtProximityGrid _grid;
 
+        private int _maxPathResult;
         private readonly RcVec3f _agentPlacementHalfExtents;
 
         private readonly IDtQueryFilter[] _filters;
@@ -166,6 +167,7 @@ public DtCrowd(DtCrowdConfig config, DtNavMesh nav, Func<int, IDtQueryFilter> qu
             }
 
             // Allocate temp buffer for merging paths.
+            _maxPathResult = 256;
             _pathQ = new DtPathQueue(config);
             _agents = new List<DtCrowdAgent>();
 
@@ -223,19 +225,20 @@ public void UpdateAgentParameters(DtCrowdAgent agent, DtCrowdAgentParams option)
             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 @@ private void CheckPathValidity(IList<DtCrowdAgent> agents, float dt)
                     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

src/DotRecast.Detour.Crowd/DtCrowdConfig.cs

@@ -23,52 +23,16 @@ public class DtCrowdConfig
     {
         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)
         {

src/DotRecast.Detour.Crowd/DtCrowdConst.cs

@@ -26,5 +26,9 @@ public static class DtCrowdConst
         /// @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 @@ public class DtPathCorridor
     {
         private RcVec3f m_pos;
         private RcVec3f m_target;
+
         private List<long> m_path;
+        private int m_maxPath;
 
         /**
         @class dtPathCorridor
@@ -76,7 +78,6 @@ the more likely the path will become non-optimal. This issue can be addressed by
         */
         public DtPathCorridor()
         {
-            m_path = new List<long>();
         }
 
         /// @par
@@ -87,7 +88,8 @@ public DtPathCorridor()
         /// @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 @@ public bool Init(int maxPath)
         ///  @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 @@ This is the function used to plan local movement within the corridor. One or mor
         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 @@ public void OptimizePathVisibility(RcVec3f next, float pathOptimizationRange, Dt
             {
                 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 @@ public bool OptimizePathTopology(DtNavMeshQuery navquery, IDtQueryFilter filter,
 
             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 @@ public bool MovePosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFilter f
             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 @@ public bool MoveTargetPosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFi
             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 LinkedList<DtPathQuery> queue = new LinkedList<DtPathQuery>();
+        private readonly DtCrowdConfig m_config;
+        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
-            // 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 @@ public void Update(DtNavMesh navMesh)
 
                 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 @@ public DtPathQueryResult Request(long startRef, long endRef, RcVec3f startPos, R
             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 @@ public static List<long> FixupShortcuts(List<long> path, DtNavMeshQuery navQuery
             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 @@ public static List<long> MergeCorridorStartMoved(List<long> path, List<long> vis
             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 @@ public static List<long> MergeCorridorEndMoved(List<long> path, List<long> visit
             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;