bit
/
DotRecastNetSim
forked from mirror/DotRecast
40
0
Fork 1
Code Pull Requests Releases Activity

added corridor maxpath for SOH issue 

- https://github.com/ikpil/DotRecast/issues/41
This commit is contained in:
ikpil 2024-02-16 00:26:23 +09:00
parent 41ab26c03f
commit d0bec3714e
9 changed files with 73 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CHANGELOG.md
View File

@ -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

26
src/DotRecast.Detour.Crowd/DtCrowd.cs
View File

@ -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;
}
/**
* 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

54
src/DotRecast.Detour.Crowd/DtCrowdConfig.cs
View File

@ -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)
{

4
src/DotRecast.Detour.Crowd/DtCrowdConst.cs
View File

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

18
src/DotRecast.Detour.Crowd/DtPathCorridor.cs
View File

@ -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);
}
/**
@ -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.
/*

23
src/DotRecast.Detour.Crowd/DtPathQueue.cs
View File

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

6
src/DotRecast.Detour/DtPathUtils.cs
View File

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

36
src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
View File

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

1
test/DotRecast.Detour.Crowd.Test/PathCorridorTest.cs
View File

@ -34,6 +34,7 @@ public class PathCorridorTest
[SetUp]
public void SetUp()
{
corridor.Init(256);
corridor.Reset(0, new RcVec3f(10, 20, 30));
}