From 9ebaa3fc6522379301add42731ee757d3ff1bd11 Mon Sep 17 00:00:00 2001 From: ikpil Date: Sun, 7 Jul 2024 14:58:38 +0900 Subject: [PATCH] Changed to limit neighbor search to a maximum count and use array for memory efficiency in `DtCrowd.AddNeighbour()` --- CHANGELOG.md | 1 + src/DotRecast.Detour.Crowd/DtCrowd.cs | 68 +++++++++++++++---- src/DotRecast.Detour.Crowd/DtCrowdAgent.cs | 9 ++- .../Tools/CrowdSampleTool.cs | 2 +- 4 files changed, 63 insertions(+), 17 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 537a55a..479876f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Changed `new float[]` to `stackalloc float[]` in `DtConvexConvexIntersections.Intersect()` - Changed agents management from list to dictionary in `DtCrowd` - Changed to efficiently stack nearby DtCrowdAgents in `DtCrowd.GetNeighbours()` +- Changed to limit neighbor search to a maximum count and use array for memory efficiency in `DtCrowd.AddNeighbour()` ### Removed - Removed RcMeshDetails.VdistSq2(float[], float[]) diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index da29d83..d90f4ec 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -20,7 +20,7 @@ freely, subject to the following restrictions: using System; using System.Collections.Generic; -using System.Threading.Tasks; +using System.Diagnostics; using DotRecast.Core; using DotRecast.Core.Collections; using DotRecast.Core.Numerics; @@ -258,6 +258,7 @@ namespace DotRecast.Detour.Crowd ag.topologyOptTime = 0; ag.targetReplanTime = 0; + ag.nneis = 0; ag.dvel = RcVec3f.Zero; ag.nvel = RcVec3f.Zero; @@ -897,21 +898,63 @@ namespace DotRecast.Detour.Crowd } // Query neighbour agents - GetNeighbours(ag.npos, ag.option.height, ag.option.collisionQueryRange, ag, ref ag.neis, _grid); + ag.nneis = GetNeighbours(ag.npos, ag.option.height, ag.option.collisionQueryRange, ag, ag.neis, DtCrowdConst.DT_CROWDAGENT_MAX_NEIGHBOURS, _grid); } } - - private int GetNeighbours(RcVec3f pos, float height, float range, DtCrowdAgent skip, ref List result, DtProximityGrid grid) + public static int AddNeighbour(DtCrowdAgent idx, float dist, Span neis, int nneis, int maxNeis) { - result.Clear(); + // Insert neighbour based on the distance. + int nei = 0; + if (0 == nneis) + { + nei = nneis; + } + else if (dist >= neis[nneis - 1].dist) + { + if (nneis >= maxNeis) + return nneis; + nei = nneis; + } + else + { + int i; + for (i = 0; i < nneis; ++i) + { + if (dist <= neis[i].dist) + { + break; + } + } + + int tgt = i + 1; + int n = Math.Min(nneis - i, maxNeis - tgt); + + Debug.Assert(tgt + n <= maxNeis); + + if (n > 0) + { + RcSpans.Move(neis, i, tgt, n); + } + + nei = i; + } + + neis[nei] = new DtCrowdNeighbour(idx, dist); + + return Math.Min(nneis + 1, maxNeis); + } + + private int GetNeighbours(RcVec3f pos, float height, float range, DtCrowdAgent skip, DtCrowdNeighbour[] result, int maxResult, DtProximityGrid grid) + { + int n = 0; const int MAX_NEIS = 32; Span ids = stackalloc int[MAX_NEIS]; int nids = grid.QueryItems(pos.X - range, pos.Z - range, pos.X + range, pos.Z + range, ids, ids.Length); - + for (int i = 0; i < nids; ++i) { var ag = GetAgent(ids[i]); @@ -934,11 +977,10 @@ namespace DotRecast.Detour.Crowd continue; } - result.Add(new DtCrowdNeighbour(ag, distSqr)); + n = AddNeighbour(ag, distSqr, result, n, maxResult); } - result.Sort((o1, o2) => o1.dist.CompareTo(o2.dist)); - return result.Count; + return n; } private void FindCorners(IList agents, DtCrowdAgentDebugInfo debug) @@ -1028,7 +1070,7 @@ namespace DotRecast.Detour.Crowd ag.state = DtCrowdAgentState.DT_CROWDAGENT_STATE_OFFMESH; ag.ncorners = 0; - ag.neis.Clear(); + ag.nneis = 0; continue; } else @@ -1093,7 +1135,7 @@ namespace DotRecast.Detour.Crowd float w = 0; RcVec3f disp = new RcVec3f(); - for (int j = 0; j < ag.neis.Count; ++j) + for (int j = 0; j < ag.nneis; ++j) { DtCrowdAgent nei = ag.neis[j].agent; @@ -1155,7 +1197,7 @@ namespace DotRecast.Detour.Crowd _obstacleQuery.Reset(); // Add neighbours as obstacles. - for (int j = 0; j < ag.neis.Count; ++j) + for (int j = 0; j < ag.nneis; ++j) { DtCrowdAgent nei = ag.neis[j].agent; _obstacleQuery.AddCircle(nei.npos, nei.option.radius, nei.vel, nei.dvel); @@ -1243,7 +1285,7 @@ namespace DotRecast.Detour.Crowd float w = 0; - for (int j = 0; j < ag.neis.Count; ++j) + for (int j = 0; j < ag.nneis; ++j) { DtCrowdAgent nei = ag.neis[j].agent; long idx1 = nei.idx; diff --git a/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs b/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs index 613f1c6..608b830 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowdAgent.cs @@ -37,16 +37,19 @@ namespace DotRecast.Detour.Crowd public bool partial; /// The path corridor the agent is using. - public DtPathCorridor corridor; + public readonly DtPathCorridor corridor; /// The local boundary data for the agent. - public DtLocalBoundary boundary; + public readonly DtLocalBoundary boundary; /// Time since the agent's path corridor was optimized. public float topologyOptTime; /// The known neighbors of the agent. - public List neis = new List(); + public readonly DtCrowdNeighbour[] neis = new DtCrowdNeighbour[DtCrowdConst.DT_CROWDAGENT_MAX_NEIGHBOURS]; + + /// The number of neighbors. + public int nneis; /// The desired speed. public float desiredSpeed; diff --git a/src/DotRecast.Recast.Demo/Tools/CrowdSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/CrowdSampleTool.cs index 917e95f..2100bd8 100644 --- a/src/DotRecast.Recast.Demo/Tools/CrowdSampleTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/CrowdSampleTool.cs @@ -325,7 +325,7 @@ public class CrowdSampleTool : ISampleTool 2.0f); dd.Begin(LINES, 2.0f); - for (int j = 0; j < ag.neis.Count; ++j) + for (int j = 0; j < ag.nneis; ++j) { DtCrowdAgent nei = ag.neis[j].agent; if (nei != null)