diff --git a/src/DotRecast.Core/Buffers/RcObjectPool.cs b/src/DotRecast.Core/Buffers/RcObjectPool.cs new file mode 100644 index 00000000..c37cf609 --- /dev/null +++ b/src/DotRecast.Core/Buffers/RcObjectPool.cs @@ -0,0 +1,30 @@ +using System; +using System.Collections.Generic; + +namespace DotRecast.Core.Buffers +{ + // This implementation is thread unsafe + public class RcObjectPool where T : class + { + private readonly Queue _items = new Queue(); + private readonly Func _createFunc; + + public RcObjectPool(Func createFunc) + { + _createFunc = createFunc; + } + + public T Get() + { + if (_items.TryDequeue(out var result)) + return result; + + return _createFunc(); + } + + public void Return(T obj) + { + _items.Enqueue(obj); + } + } +} \ No newline at end of file diff --git a/src/DotRecast.Core/Buffers/RcRentedArray.cs b/src/DotRecast.Core/Buffers/RcRentedArray.cs index 68f12c78..cc49483d 100644 --- a/src/DotRecast.Core/Buffers/RcRentedArray.cs +++ b/src/DotRecast.Core/Buffers/RcRentedArray.cs @@ -1,31 +1,96 @@ -using System; +using System; using System.Buffers; +using System.Collections.Generic; using System.Runtime.CompilerServices; +using System.Threading; namespace DotRecast.Core.Buffers { public static class RcRentedArray { + private sealed class RentIdPool + { + private int[] _generations; + private readonly Queue _freeIds; + private int _maxId; + + public RentIdPool(int capacity) + { + _generations = new int[capacity]; + _freeIds = new Queue(capacity); + } + + internal RentIdGen AcquireId() + { + if (!_freeIds.TryDequeue(out int id)) + { + id = _maxId++; + if(_generations.Length <= id) + Array.Resize(ref _generations, _generations.Length << 1); + } + + return new RentIdGen(id, _generations[id]); + } + + internal void ReturnId(int id) + { + _generations[id]++; + _freeIds.Enqueue(id); + } + + internal int GetGeneration(int id) + { + return _generations.Length <= id ? 0 : _generations[id]; + } + } + + public const int START_RENT_ID_POOL_CAPACITY = 16; + private static readonly ThreadLocal _rentPool = new ThreadLocal(() => new RentIdPool(START_RENT_ID_POOL_CAPACITY)); + public static RcRentedArray Rent(int minimumLength) { var array = ArrayPool.Shared.Rent(minimumLength); - return new RcRentedArray(ArrayPool.Shared, array, minimumLength); + return new RcRentedArray(ArrayPool.Shared, _rentPool.Value.AcquireId(), array, minimumLength); + } + + internal static bool IsDisposed(RentIdGen rentIdGen) + { + return _rentPool.Value.GetGeneration(rentIdGen.Id) != rentIdGen.Gen; + } + + internal static void ReturnId(RentIdGen rentIdGen) + { + _rentPool.Value.ReturnId(rentIdGen.Id); } } - public class RcRentedArray : IDisposable + public readonly struct RentIdGen + { + public readonly int Id; + public readonly int Gen; + + public RentIdGen(int id, int gen) + { + Id = id; + Gen = gen; + } + } + + public struct RcRentedArray : IDisposable { private ArrayPool _owner; private T[] _array; + private readonly RentIdGen _rentIdGen; public int Length { get; } - public bool IsDisposed => null == _owner || null == _array; + public bool IsDisposed => null == _owner || null == _array || RcRentedArray.IsDisposed(_rentIdGen); - internal RcRentedArray(ArrayPool owner, T[] array, int length) + internal RcRentedArray(ArrayPool owner, RentIdGen rentIdGen, T[] array, int length) { _owner = owner; _array = array; Length = length; + _rentIdGen = rentIdGen; } public ref T this[int index] @@ -34,6 +99,8 @@ public ref T this[int index] get { RcThrowHelper.ThrowExceptionIfIndexOutOfRange(index, Length); + if (IsDisposed) + throw new NullReferenceException(); return ref _array[index]; } } @@ -43,15 +110,22 @@ public T[] AsArray() return _array; } + public Span AsSpan() + { + return new Span(_array, 0, Length); + } + public void Dispose() { - if (null != _owner && null != _array) + if (null != _owner && null != _array && !RcRentedArray.IsDisposed(_rentIdGen)) { + RcRentedArray.ReturnId(_rentIdGen); _owner.Return(_array, true); - _owner = null; - _array = null; } + + _owner = null; + _array = null; } } } \ No newline at end of file diff --git a/src/DotRecast.Core/Collections/CollectionExtensions.cs b/src/DotRecast.Core/Collections/CollectionExtensions.cs index 82f5f5c4..c610899d 100644 --- a/src/DotRecast.Core/Collections/CollectionExtensions.cs +++ b/src/DotRecast.Core/Collections/CollectionExtensions.cs @@ -45,5 +45,11 @@ public static void Shuffle(this IList list) (list[k], list[n]) = (list[n], list[k]); } } + + public static void AddRange(this IList list, Span span) + { + foreach (var i in span) + list.Add(i); + } } } \ No newline at end of file diff --git a/src/DotRecast.Core/Collections/RcSortedQueue.cs b/src/DotRecast.Core/Collections/RcSortedQueue.cs index 0c06aad9..db822dd8 100644 --- a/src/DotRecast.Core/Collections/RcSortedQueue.cs +++ b/src/DotRecast.Core/Collections/RcSortedQueue.cs @@ -27,12 +27,12 @@ public class RcSortedQueue { private bool _dirty; private readonly List _items; - private readonly Comparer _comparer; + private readonly Comparison _comparison; public RcSortedQueue(Comparison comp) { _items = new List(); - _comparer = Comparer.Create((x, y) => comp.Invoke(x, y) * -1); + _comparison = (x, y) => comp(x, y) * -1; } public int Count() @@ -55,7 +55,7 @@ private void Balance() { if (_dirty) { - _items.Sort(_comparer); // reverse + _items.Sort(_comparison); // reverse _dirty = false; } } @@ -88,7 +88,7 @@ public bool Remove(T item) return false; //int idx = _items.BinarySearch(item, _comparer); // don't use this! Because reference types can be reused externally. - int idx = _items.FindLastIndex(x => item.Equals(x)); + int idx = _items.LastIndexOf(item); if (0 > idx) return false; diff --git a/src/DotRecast.Core/Numerics/RcVec.cs b/src/DotRecast.Core/Numerics/RcVec.cs index 69aae7d0..0569071d 100644 --- a/src/DotRecast.Core/Numerics/RcVec.cs +++ b/src/DotRecast.Core/Numerics/RcVec.cs @@ -205,7 +205,7 @@ public static float Dist2DSqr(RcVec3f v1, RcVec3f v2) } [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float Dist2DSqr(RcVec3f p, float[] verts, int i) + public static float Dist2DSqr(RcVec3f p, Span verts, int i) { float dx = verts[i] - p.X; float dz = verts[i + 2] - p.Z; diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index d90f4ec1..d148a867 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -129,7 +129,7 @@ public class DtCrowd private readonly DtObstacleAvoidanceParams[] _obstacleQueryParams; private readonly DtObstacleAvoidanceQuery _obstacleQuery; - private DtProximityGrid _grid; + private readonly DtProximityGrid _grid; private int _maxPathResult; private readonly RcVec3f _agentPlacementHalfExtents; @@ -174,6 +174,7 @@ public DtCrowd(DtCrowdConfig config, DtNavMesh nav, Func qu _agentIdx = new RcAtomicInteger(0); _agents = new Dictionary(); _activeAgents = new List(); + _grid = new DtProximityGrid(_config.maxAgentRadius * 3); // The navQuery is mostly used for local searches, no need for large node pool. SetNavMesh(nav); @@ -564,14 +565,18 @@ private void CheckPathValidity(IList agents, float dt) } } + private readonly RcSortedQueue UpdateMoveRequest_queue = new RcSortedQueue((a1, a2) => a2.targetReplanTime.CompareTo(a1.targetReplanTime)); + private readonly List UpdateMoveRequest_reqPath = new List(); private void UpdateMoveRequest(IList agents, float dt) { using var timer = _telemetry.ScopedTimer(DtCrowdTimerLabel.UpdateMoveRequest); - RcSortedQueue queue = new RcSortedQueue((a1, a2) => a2.targetReplanTime.CompareTo(a1.targetReplanTime)); + RcSortedQueue queue = UpdateMoveRequest_queue; + queue.Clear(); // Fire off new requests. - List reqPath = new List(); + List reqPath = UpdateMoveRequest_reqPath; + reqPath.Clear(); for (var i = 0; i < agents.Count; i++) { var ag = agents[i]; @@ -864,7 +869,7 @@ private void BuildProximityGrid(IList agents) { using var timer = _telemetry.ScopedTimer(DtCrowdTimerLabel.BuildProximityGrid); - _grid = new DtProximityGrid(_config.maxAgentRadius * 3); + _grid.Clear(); for (var i = 0; i < agents.Count; i++) { diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs index 37787ae0..d3b04c64 100644 --- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs @@ -33,7 +33,7 @@ public class DtPathCorridor private RcVec3f m_pos; private RcVec3f m_target; - private List m_path; + private List m_path = new List(); private int m_npath; private int m_maxPath; @@ -89,7 +89,8 @@ public DtPathCorridor() /// @return True if the initialization succeeded. public bool Init(int maxPath) { - m_path = new List(maxPath); + if (m_path.Capacity < maxPath) + m_path.Capacity = maxPath; m_npath = 0; m_maxPath = maxPath; return true; @@ -218,7 +219,7 @@ public void OptimizePathVisibility(RcVec3f next, float pathOptimizationRange, Dt { if (res.Count > 1 && t > 0.99f) { - m_npath = DtPathUtils.MergeCorridorStartShortcut(ref m_path, m_npath, m_maxPath, res, res.Count); + m_npath = DtPathUtils.MergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, res.Count); } } } @@ -250,7 +251,7 @@ public bool OptimizePathTopology(DtNavMeshQuery navquery, IDtQueryFilter filter, if (status.Succeeded() && res.Count > 0) { - m_npath = DtPathUtils.MergeCorridorStartShortcut(ref m_path, m_npath, m_maxPath, res, res.Count); + m_npath = DtPathUtils.MergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, res.Count); return true; } @@ -276,7 +277,7 @@ public bool MoveOverOffmeshConnection(long offMeshConRef, long[] refs, ref RcVec } // Prune path - m_path = m_path.GetRange(npos, m_npath - npos); + m_path.RemoveRange(0, npos); m_npath -= npos; refs[0] = prevRef; @@ -322,7 +323,7 @@ public bool MovePosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFilter f var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, visited, out var nvisited, MAX_VISITED); if (status.Succeeded()) { - m_npath = DtPathUtils.MergeCorridorStartMoved(ref m_path, m_npath, m_maxPath, visited, nvisited); + m_npath = DtPathUtils.MergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited); // Adjust the position to stay on top of the navmesh. m_pos = result; @@ -366,7 +367,7 @@ public bool MoveTargetPosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFi var status = navquery.MoveAlongSurface(m_path[^1], m_target, npos, filter, out var result, visited, out nvisited, MAX_VISITED); if (status.Succeeded()) { - m_npath = DtPathUtils.MergeCorridorEndMoved(ref m_path, m_npath, m_maxPath, visited, nvisited); + m_npath = DtPathUtils.MergeCorridorEndMoved(m_path, m_npath, m_maxPath, visited, nvisited); // TODO: should we do that? // Adjust the position to stay on top of the navmesh. @@ -394,7 +395,11 @@ public bool MoveTargetPosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFi public void SetCorridor(RcVec3f target, List path) { m_target = target; - m_path = new List(path); + if(path != m_path) + { + m_path.Clear(); + m_path.AddRange(path); + } m_npath = path.Count; } @@ -444,7 +449,7 @@ public bool TrimInvalidPath(long safeRef, float[] safePos, DtNavMeshQuery navque else if (n < m_npath) { // The path is partially usable. - m_path = m_path.GetRange(0, n); + m_path.RemoveRange(n, m_path.Count - n); m_npath = n; } diff --git a/src/DotRecast.Detour.Crowd/DtProximityGrid.cs b/src/DotRecast.Detour.Crowd/DtProximityGrid.cs index 3cd182cf..f4814972 100644 --- a/src/DotRecast.Detour.Crowd/DtProximityGrid.cs +++ b/src/DotRecast.Detour.Crowd/DtProximityGrid.cs @@ -22,6 +22,7 @@ 3. This notice may not be removed or altered from any source distribution. using System.Collections.Generic; using System.Linq; using System.Runtime.CompilerServices; +using DotRecast.Core.Buffers; namespace DotRecast.Detour.Crowd { @@ -30,12 +31,14 @@ public class DtProximityGrid private readonly float _cellSize; private readonly float _invCellSize; private readonly Dictionary> _items; + private readonly RcObjectPool> _listPool; public DtProximityGrid(float cellSize) { _cellSize = cellSize; _invCellSize = 1.0f / cellSize; _items = new Dictionary>(); + _listPool = new RcObjectPool>(() => new List()); } [MethodImpl(MethodImplOptions.AggressiveInlining)] @@ -57,6 +60,8 @@ public static void DecomposeKey(long key, out int x, out int y) public void Clear() { + foreach (var pair in _items) + _listPool.Return(pair.Value); _items.Clear(); } @@ -74,7 +79,8 @@ public void AddItem(DtCrowdAgent agent, float minx, float miny, float maxx, floa long key = CombineKey(x, y); if (!_items.TryGetValue(key, out var ids)) { - ids = new List(); + ids = _listPool.Get(); + ids.Clear(); _items.Add(key, ids); } diff --git a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs index b14de89d..934d445a 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs @@ -52,25 +52,12 @@ private bool GetNavMeshHeight(DtNavMeshQuery navMeshQuery, RcVec3f pt, float cs, RcVec3f halfExtents = new RcVec3f { X = cs, Y = heightRange, Z = cs }; float maxHeight = pt.Y + heightRange; - RcAtomicBoolean found = new RcAtomicBoolean(); - RcAtomicFloat minHeight = new RcAtomicFloat(pt.Y); + var query = new DtHeightSamplePolyQuery(navMeshQuery, pt, pt.Y, maxHeight); + navMeshQuery.QueryPolygons(pt, halfExtents, DtQueryNoOpFilter.Shared, ref query); - navMeshQuery.QueryPolygons(pt, halfExtents, DtQueryNoOpFilter.Shared, new DtCallbackPolyQuery((tile, poly, refs) => + if (query.Found) { - var status = navMeshQuery.GetPolyHeight(refs, pt, out var h); - if (status.Succeeded()) - { - if (h > minHeight.Get() && h < maxHeight) - { - minHeight.Exchange(h); - found.Set(true); - } - } - })); - - if (found.Get()) - { - height = minHeight.Get(); + height = query.MinHeight; return true; } diff --git a/src/DotRecast.Detour/DtCallbackPolyQuery.cs b/src/DotRecast.Detour/DtCallbackPolyQuery.cs index 77f626f1..d5325ad9 100644 --- a/src/DotRecast.Detour/DtCallbackPolyQuery.cs +++ b/src/DotRecast.Detour/DtCallbackPolyQuery.cs @@ -2,7 +2,7 @@ namespace DotRecast.Detour { - public class DtCallbackPolyQuery : IDtPolyQuery + public struct DtCallbackPolyQuery : IDtPolyQuery { private readonly Action _callback; @@ -11,7 +11,7 @@ public DtCallbackPolyQuery(Action callback) _callback = callback; } - public void Process(DtMeshTile tile, DtPoly[] poly, Span refs, int count) + public void Process(DtMeshTile tile, Span poly, Span refs, int count) { for (int i = 0; i < count; ++i) { diff --git a/src/DotRecast.Detour/DtCollectPolysQuery.cs b/src/DotRecast.Detour/DtCollectPolysQuery.cs index e98daa74..29710d4f 100644 --- a/src/DotRecast.Detour/DtCollectPolysQuery.cs +++ b/src/DotRecast.Detour/DtCollectPolysQuery.cs @@ -26,7 +26,7 @@ public bool Overflowed() return m_overflow; } - public void Process(DtMeshTile tile, DtPoly[] poly, Span refs, int count) + public void Process(DtMeshTile tile, Span poly, Span refs, int count) { int numLeft = m_maxPolys - m_numCollected; int toCopy = count; diff --git a/src/DotRecast.Detour/DtConvexConvexIntersections.cs b/src/DotRecast.Detour/DtConvexConvexIntersections.cs index 4e219ee9..59aa4b8f 100644 --- a/src/DotRecast.Detour/DtConvexConvexIntersections.cs +++ b/src/DotRecast.Detour/DtConvexConvexIntersections.cs @@ -27,7 +27,7 @@ public static class DtConvexConvexIntersections { private const float EPSILON = 0.0001f; - public static float[] Intersect(Span p, Span q) + public static Span Intersect(Span p, Span q, Span buffer) { int n = p.Length / 3; int m = q.Length / 3; @@ -95,7 +95,7 @@ public static float[] Intersect(Span p, Span q) /* Special case: A & B parallel and separated. */ if (parallel && aHB < 0f && bHA < 0f) { - return null; + return Span.Empty; } /* Special case: A & B collinear. */ else if (parallel && MathF.Abs(aHB) < EPSILON && MathF.Abs(bHA) < EPSILON) @@ -168,8 +168,9 @@ public static float[] Intersect(Span p, Span q) return null; } - float[] copied = inters.Slice(0, ii).ToArray(); - return copied; + Span result = buffer.Slice(0, ii); + inters.Slice(0, ii).CopyTo(result); + return result; } private static int AddVertex(Span inters, int ii, RcVec3f p) diff --git a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs index b0c4cf1c..56ac604f 100644 --- a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs +++ b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs @@ -3,7 +3,7 @@ namespace DotRecast.Detour { - public class DtFindNearestPolyQuery : IDtPolyQuery + public struct DtFindNearestPolyQuery : IDtPolyQuery { private readonly DtNavMeshQuery _query; private readonly RcVec3f _center; @@ -18,9 +18,12 @@ public DtFindNearestPolyQuery(DtNavMeshQuery query, RcVec3f center) _center = center; _nearestDistanceSqr = float.MaxValue; _nearestPoint = center; + + _nearestRef = default; + _overPoly = default; } - public void Process(DtMeshTile tile, DtPoly[] poly, Span refs, int count) + public void Process(DtMeshTile tile, Span poly, Span refs, int count) { for (int i = 0; i < count; ++i) { diff --git a/src/DotRecast.Detour/DtHeightSamplePolyQuery.cs b/src/DotRecast.Detour/DtHeightSamplePolyQuery.cs new file mode 100644 index 00000000..674f0aa7 --- /dev/null +++ b/src/DotRecast.Detour/DtHeightSamplePolyQuery.cs @@ -0,0 +1,45 @@ +using System; +using DotRecast.Core; +using DotRecast.Core.Numerics; + +namespace DotRecast.Detour +{ + public struct DtHeightSamplePolyQuery : IDtPolyQuery + { + private readonly DtNavMeshQuery _navMeshQuery; + private readonly RcVec3f _pt; + private readonly float _maxHeight; + public float MinHeight { get; private set; } + public bool Found { get; private set; } + + public DtHeightSamplePolyQuery(DtNavMeshQuery navMeshQuery, RcVec3f pt, float minHeight, float maxHeight) + { + _navMeshQuery = navMeshQuery; + _pt = pt; + MinHeight = minHeight; + _maxHeight = maxHeight; + Found = default; + } + + public void Process(DtMeshTile tile, Span poly, Span refs, int count) + { + for (int i = 0; i < count; i++) + { + ProcessSingle(refs[i]); + } + } + + private void ProcessSingle(long refs) + { + var status = _navMeshQuery.GetPolyHeight(refs, _pt, out var h); + if (!status.Succeeded()) + return; + + if (!(h > MinHeight) || !(h < _maxHeight)) + return; + + MinHeight = h; + Found = true; + } + } +} \ No newline at end of file diff --git a/src/DotRecast.Detour/DtNavMesh.cs b/src/DotRecast.Detour/DtNavMesh.cs index a4791238..54a2db37 100644 --- a/src/DotRecast.Detour/DtNavMesh.cs +++ b/src/DotRecast.Detour/DtNavMesh.cs @@ -1365,6 +1365,11 @@ int GetNeighbourTilesAt(int x, int y, int side, DtMeshTile[] tiles, int maxTiles } public int GetTilesAt(int x, int y, DtMeshTile[] tiles, int maxTiles) + { + return GetTilesAt(x, y, (Span)tiles, maxTiles); + } + + public int GetTilesAt(int x, int y, Span tiles, int maxTiles) { int n = 0; diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index ee0e699a..0ebaeb68 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -21,7 +21,9 @@ 3. This notice may not be removed or altered from any source distribution. using System; using System.Collections.Generic; using DotRecast.Core; +using DotRecast.Core.Buffers; using DotRecast.Core.Numerics; +using CollectionExtensions = DotRecast.Core.Collections.CollectionExtensions; namespace DotRecast.Detour { @@ -32,6 +34,8 @@ namespace DotRecast.Detour /// @ingroup detour public class DtNavMeshQuery { + public const int MAX_PATH_LENGTH = 256; + protected readonly DtNavMesh m_nav; //< Pointer to navmesh data. protected DtQueryData m_query; //< Sliced query state. @@ -229,6 +233,7 @@ public DtStatus FindRandomPointAroundCircle(long startRef, RcVec3f centerPos, fl IDtQueryFilter filter, IRcRand frand, IDtPolygonByCircleConstraint constraint, out long randomRef, out RcVec3f randomPt) { + const int MAX_VERT_BUFFER_SIZE = 128; randomRef = startRef; randomPt = centerPos; @@ -261,10 +266,14 @@ public DtStatus FindRandomPointAroundCircle(long startRef, RcVec3f centerPos, fl float radiusSqr = maxRadius * maxRadius; float areaSum = 0.0f; - + + using RcRentedArray polyVertsBuffer = RcRentedArray.Rent(MAX_VERT_BUFFER_SIZE); + using RcRentedArray randomPolyVertsBuffer = RcRentedArray.Rent(MAX_VERT_BUFFER_SIZE); + using RcRentedArray constrainedVertsBuffer = RcRentedArray.Rent(MAX_VERT_BUFFER_SIZE); + DtPoly randomPoly = null; long randomPolyRef = 0; - float[] randomPolyVerts = null; + Span randomPolyVerts = Span.Empty; while (!m_openList.IsEmpty()) { @@ -282,14 +291,14 @@ public DtStatus FindRandomPointAroundCircle(long startRef, RcVec3f centerPos, fl { // Calc area of the polygon. float polyArea = 0.0f; - float[] polyVerts = new float[bestPoly.vertCount * 3]; + Span polyVerts = polyVertsBuffer.AsSpan().Slice(0, bestPoly.vertCount * 3); for (int j = 0; j < bestPoly.vertCount; ++j) { - RcArrays.Copy(bestTile.data.verts, bestPoly.verts[j] * 3, polyVerts, j * 3, 3); + RcSpans.Copy(bestTile.data.verts, bestPoly.verts[j] * 3, polyVerts, j * 3, 3); } - float[] constrainedVerts = constraint.Apply(polyVerts, centerPos, maxRadius); - if (constrainedVerts != null) + Span constrainedVerts = constraint.Apply(polyVerts, centerPos, maxRadius, constrainedVertsBuffer.AsSpan()); + if (!constrainedVerts.IsEmpty) { int vertCount = constrainedVerts.Length / 3; for (int j = 2; j < vertCount; ++j) @@ -307,7 +316,8 @@ public DtStatus FindRandomPointAroundCircle(long startRef, RcVec3f centerPos, fl { randomPoly = bestPoly; randomPolyRef = bestRef; - randomPolyVerts = constrainedVerts; + randomPolyVerts = randomPolyVertsBuffer.AsSpan().Slice(0, constrainedVerts.Length); + constrainedVerts.CopyTo(randomPolyVerts); } } } @@ -579,7 +589,7 @@ public DtStatus FindNearestPoly(RcVec3f center, RcVec3f halfExtents, IDtQueryFil // Get nearby polygons from proximity grid. DtFindNearestPolyQuery query = new DtFindNearestPolyQuery(this, center); - DtStatus status = QueryPolygons(center, halfExtents, filter, query); + DtStatus status = QueryPolygons(center, halfExtents, filter, ref query); if (status.Failed()) { return status; @@ -593,11 +603,13 @@ public DtStatus FindNearestPoly(RcVec3f center, RcVec3f halfExtents, IDtQueryFil } /// Queries polygons within a tile. - protected void QueryPolygonsInTile(DtMeshTile tile, RcVec3f qmin, RcVec3f qmax, IDtQueryFilter filter, IDtPolyQuery query) + protected void QueryPolygonsInTile(DtMeshTile tile, RcVec3f qmin, RcVec3f qmax, IDtQueryFilter filter, ref TQuery query) + where TQuery : IDtPolyQuery { const int batchSize = 32; Span polyRefs = stackalloc long[batchSize]; - DtPoly[] polys = new DtPoly[batchSize]; + using RcRentedArray polysRent = RcRentedArray.Rent(batchSize); + Span polys = polysRent.AsSpan(); int n = 0; if (tile.data.bvTree != null) @@ -748,7 +760,7 @@ public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; DtCollectPolysQuery collector = new DtCollectPolysQuery(polys, maxPolys); - DtStatus status = QueryPolygons(center, halfExtents, filter, collector); + DtStatus status = QueryPolygons(center, halfExtents, filter, ref collector); if (status.Failed()) return status; @@ -770,7 +782,8 @@ public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] /// @param[in] filter The polygon filter to apply to the query. /// @param[in] query The query. Polygons found will be batched together and passed to this query. - public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, IDtQueryFilter filter, IDtPolyQuery query) + public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, IDtQueryFilter filter, ref TQuery query) + where TQuery : IDtPolyQuery { if (!center.IsFinite() || !halfExtents.IsFinite() || null == filter) { @@ -786,7 +799,8 @@ public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, IDtQueryFilte m_nav.CalcTileLoc(bmax, out var maxx, out var maxy); const int MAX_NEIS = 32; - DtMeshTile[] neis = new DtMeshTile[MAX_NEIS]; + using RcRentedArray neisRent = RcRentedArray.Rent(MAX_NEIS); + Span neis = neisRent.AsSpan(); for (int y = miny; y <= maxy; ++y) { @@ -795,7 +809,7 @@ public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, IDtQueryFilte int nneis = m_nav.GetTilesAt(x, y, neis, MAX_NEIS); for (int j = 0; j < nneis; ++j) { - QueryPolygonsInTile(neis[j], bmin, bmax, filter, query); + QueryPolygonsInTile(neis[j], bmin, bmax, filter, ref query); } } } @@ -877,7 +891,8 @@ public DtStatus FindPath(long startRef, long endRef, RcVec3f startPos, RcVec3f e float lastBestNodeCost = startNode.total; DtRaycastHit rayHit = new DtRaycastHit(); - rayHit.path = new List(); + using var pathRent = RcRentedArray.Rent(MAX_PATH_LENGTH); + rayHit.path = pathRent.AsArray(); while (!m_openList.IsEmpty()) { // Remove node from open list and put it in closed list. @@ -1166,7 +1181,8 @@ public virtual DtStatus UpdateSlicedFindPath(int maxIter, out int doneIters) } var rayHit = new DtRaycastHit(); - rayHit.path = new List(); + using var pathRent = RcRentedArray.Rent(MAX_PATH_LENGTH); + rayHit.path = pathRent.AsArray(); int iter = 0; while (iter < maxIter && !m_openList.IsEmpty()) @@ -1837,6 +1853,7 @@ public virtual DtStatus FindStraightPath(RcVec3f startPos, RcVec3f endPos, return DtStatus.DT_SUCCESS | (straightPathCount >= maxStraightPath ? DtStatus.DT_BUFFER_TOO_SMALL : DtStatus.DT_STATUS_NOTHING); } + private readonly Queue MoveAlongSurface_queue = new Queue(); /// @par /// /// This method is optimized for small delta movement and a small number of @@ -1892,8 +1909,8 @@ public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos startNode.total = 0; startNode.id = startRef; startNode.flags = DtNodeFlags.DT_NODE_CLOSED; - LinkedList stack = new LinkedList(); - stack.AddLast(startNode); + MoveAlongSurface_queue.Clear(); + MoveAlongSurface_queue.Enqueue(startNode); RcVec3f bestPos = new RcVec3f(); float bestDist = float.MaxValue; @@ -1909,11 +1926,10 @@ public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos const int MAX_NEIS = 8; Span neis = stackalloc long[MAX_NEIS]; - while (0 < stack.Count) + while (0 < MoveAlongSurface_queue.Count) { // Pop front. - DtNode curNode = stack.First?.Value; - stack.RemoveFirst(); + DtNode curNode = MoveAlongSurface_queue.Dequeue(); // Get poly and tile. // The API input has been checked already, skip checking internal data. @@ -2012,7 +2028,7 @@ public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos // Mark as the node as visited and push to queue. neighbourNode.pidx = m_tinyNodePool.GetNodeIdx(curNode); neighbourNode.flags |= DtNodeFlags.DT_NODE_CLOSED; - stack.AddLast(neighbourNode); + MoveAlongSurface_queue.Enqueue(neighbourNode); } } } @@ -2258,13 +2274,15 @@ public DtStatus Raycast(long startRef, RcVec3f startPos, RcVec3f endPos, out float t, out RcVec3f hitNormal, ref List path) { DtRaycastHit hit = new DtRaycastHit(); - hit.path = path; + using var pathRent = RcRentedArray.Rent(MAX_PATH_LENGTH); + hit.path = pathRent.AsArray(); DtStatus status = Raycast(startRef, startPos, endPos, filter, 0, ref hit, 0); t = hit.t; hitNormal = hit.hitNormal; - path = hit.path; + path.Clear(); + CollectionExtensions.AddRange(path, new Span(hit.path, 0, hit.pathCount)); return status; } @@ -2330,7 +2348,7 @@ public DtStatus Raycast(long startRef, RcVec3f startPos, RcVec3f endPos, } hit.t = 0; - hit.path.Clear(); + hit.pathCount = 0; hit.pathCost = 0; Span verts = stackalloc RcVec3f[m_nav.GetMaxVertsPerPoly() + 1]; @@ -2383,7 +2401,7 @@ public DtStatus Raycast(long startRef, RcVec3f startPos, RcVec3f endPos, } // Store visited polygons. - hit.path.Add(curRef); + hit.AddPathNode(curRef); // Ray end is completely inside the polygon. if (segMax == -1) @@ -2903,6 +2921,7 @@ public DtStatus FindPolysAroundShape(long startRef, RcVec3f[] verts, IDtQueryFil return DtStatus.DT_SUCCESS; } + private readonly Queue FindLocalNeighbourhood_queue = new Queue(); /// @par /// /// This method is optimized for a small search radius and small number of result @@ -2954,8 +2973,8 @@ public DtStatus FindLocalNeighbourhood(long startRef, RcVec3f centerPos, float r startNode.pidx = 0; startNode.id = startRef; startNode.flags = DtNodeFlags.DT_NODE_CLOSED; - LinkedList stack = new LinkedList(); - stack.AddLast(startNode); + FindLocalNeighbourhood_queue.Clear(); + FindLocalNeighbourhood_queue.Enqueue(startNode); resultRef.Add(startNode.id); resultParent.Add(0L); @@ -2965,11 +2984,11 @@ public DtStatus FindLocalNeighbourhood(long startRef, RcVec3f centerPos, float r Span pa = stackalloc float[m_nav.GetMaxVertsPerPoly() * 3]; Span pb = stackalloc float[m_nav.GetMaxVertsPerPoly() * 3]; - while (0 < stack.Count) + while (0 < FindLocalNeighbourhood_queue.Count) { // Pop front. - DtNode curNode = stack.First?.Value; - stack.RemoveFirst(); + + DtNode curNode = FindLocalNeighbourhood_queue.Dequeue(); // Get poly and tile. // The API input has been checked already, skip checking internal data. @@ -3082,7 +3101,7 @@ public DtStatus FindLocalNeighbourhood(long startRef, RcVec3f centerPos, float r resultRef.Add(neighbourRef); resultParent.Add(curRef); - stack.AddLast(neighbourNode); + FindLocalNeighbourhood_queue.Enqueue(neighbourNode); } } diff --git a/src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs b/src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs index e5d26510..3a67655f 100644 --- a/src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs +++ b/src/DotRecast.Detour/DtNoOpDtPolygonByCircleConstraint.cs @@ -1,3 +1,4 @@ +using System; using DotRecast.Core.Numerics; namespace DotRecast.Detour @@ -10,9 +11,11 @@ private DtNoOpDtPolygonByCircleConstraint() { } - public float[] Apply(float[] polyVerts, RcVec3f circleCenter, float radius) + public Span Apply(Span polyVerts, RcVec3f circleCenter, float radius, Span resultBuffer) { - return polyVerts; + var result = resultBuffer.Slice(0, polyVerts.Length); + polyVerts.CopyTo(result); + return result; } } } \ No newline at end of file diff --git a/src/DotRecast.Detour/DtPathUtils.cs b/src/DotRecast.Detour/DtPathUtils.cs index 24dd8a63..254b4392 100644 --- a/src/DotRecast.Detour/DtPathUtils.cs +++ b/src/DotRecast.Detour/DtPathUtils.cs @@ -20,7 +20,10 @@ 3. This notice may not be removed or altered from any source distribution. using System; using System.Collections.Generic; +using DotRecast.Core.Buffers; +using DotRecast.Core.Collections; using DotRecast.Core.Numerics; +using CollectionExtensions = DotRecast.Core.Collections.CollectionExtensions; namespace DotRecast.Detour { @@ -148,7 +151,7 @@ public static int FixupShortcuts(ref List path, int npath, DtNavMeshQuery return npath; } - public static int MergeCorridorStartMoved(ref List path, int npath, int maxPath, Span visited, int nvisited) + public static int MergeCorridorStartMoved(List path, int npath, int maxPath, Span visited, int nvisited) { int furthestPath = -1; int furthestVisited = -1; @@ -180,22 +183,23 @@ public static int MergeCorridorStartMoved(ref List path, int npath, int ma } // Concatenate paths. - + var endIndex = nvisited - 1; + var length1 = endIndex - furthestVisited; + var length2 = npath - furthestPath; + using var result = RcRentedArray.Rent(length1 + length2); // Adjust beginning of the buffer to include the visited. - List result = new List(); // Store visited - for (int i = nvisited - 1; i > furthestVisited; --i) - { - result.Add(visited[i]); - } - - result.AddRange(path.GetRange(furthestPath, npath - furthestPath)); - - path = result; - return result.Count; + for (int i = 0; i < length1; ++i) + result[i] = visited[endIndex - i]; + + path.CopyTo(furthestPath, result.AsArray(), length1, length2); + + path.Clear(); + CollectionExtensions.AddRange(path, result.AsSpan()); + return result.Length; } - public static int MergeCorridorEndMoved(ref List path, int npath, int maxPath, Span visited, int nvisited) + public static int MergeCorridorEndMoved(List path, int npath, int maxPath, Span visited, int nvisited) { int furthestPath = -1; int furthestVisited = -1; @@ -227,17 +231,18 @@ public static int MergeCorridorEndMoved(ref List path, int npath, int maxP } // Concatenate paths. - List result = path.GetRange(0, furthestPath); - foreach (var v in visited.Slice(furthestVisited, nvisited - furthestVisited)) - { - result.Add(v); - } - - path = result; - return result.Count; + var length1 = furthestPath; + var length2 = nvisited - furthestVisited; + using var result = RcRentedArray.Rent(length1 + length2); + path.CopyTo(0, result.AsArray(), 0, length1); + visited.Slice(furthestVisited, nvisited - furthestVisited).CopyTo(result.AsSpan().Slice(length1, length2)); + + path.Clear(); + CollectionExtensions.AddRange(path, result.AsSpan()); + return path.Count; } - public static int MergeCorridorStartShortcut(ref List path, int npath, int maxPath, List visited, int nvisited) + public static int MergeCorridorStartShortcut(List path, int npath, int maxPath, List visited, int nvisited) { int furthestPath = -1; int furthestVisited = -1; @@ -271,11 +276,15 @@ public static int MergeCorridorStartShortcut(ref List path, int npath, int // Concatenate paths. // Adjust beginning of the buffer to include the visited. - List result = visited.GetRange(0, furthestVisited); - result.AddRange(path.GetRange(furthestPath, npath - furthestPath)); - - path = result; - return result.Count; + var length1 = furthestVisited; + var length2 = npath - furthestPath; + using var result = RcRentedArray.Rent(length1 + length2); + visited.CopyTo(0, result.AsArray(), 0, length1); + path.CopyTo(furthestPath, result.AsArray(), length1, length2); + + path.Clear(); + CollectionExtensions.AddRange(path, result.AsSpan()); + return path.Count; } } } \ No newline at end of file diff --git a/src/DotRecast.Detour/DtRaycastHit.cs b/src/DotRecast.Detour/DtRaycastHit.cs index 58c79ac6..22154e29 100644 --- a/src/DotRecast.Detour/DtRaycastHit.cs +++ b/src/DotRecast.Detour/DtRaycastHit.cs @@ -38,9 +38,16 @@ public struct DtRaycastHit public int hitEdgeIndex; /// Pointer to an array of reference ids of the visited polygons. [opt] - public List path; + public long[] path; + + public int pathCount; /// The cost of the path until hit. public float pathCost; + + public void AddPathNode(long nodeRef) + { + path[pathCount++] = nodeRef; + } } } \ No newline at end of file diff --git a/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs b/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs index 47bf30e7..61cc62ad 100644 --- a/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs +++ b/src/DotRecast.Detour/DtStrictDtPolygonByCircleConstraint.cs @@ -39,8 +39,7 @@ public static void ScaleCircle(Span src, RcVec3f center, float radius, Sp } } - - public float[] Apply(float[] verts, RcVec3f center, float radius) + public Span Apply(Span verts, RcVec3f center, float radius, Span resultBuffer) { float radiusSqr = radius * radius; int outsideVertex = -1; @@ -56,19 +55,30 @@ public float[] Apply(float[] verts, RcVec3f center, float radius) if (outsideVertex == -1) { // polygon inside circle - return verts; + var result = resultBuffer.Slice(0, verts.Length); + verts.CopyTo(result); + return result; } Span qCircle = stackalloc float[UnitCircle.Length]; ScaleCircle(UnitCircle, center, radius, qCircle); - float[] intersection = DtConvexConvexIntersections.Intersect(verts, qCircle); - if (intersection == null && DtUtils.PointInPolygon(center, verts, verts.Length / 3)) + Span intersection = DtConvexConvexIntersections.Intersect(verts, qCircle, resultBuffer); + if (intersection.IsEmpty && DtUtils.PointInPolygon(center, verts, verts.Length / 3)) { // circle inside polygon - return qCircle.ToArray(); + var result = resultBuffer.Slice(0, qCircle.Length); + qCircle.CopyTo(result); + return result; } - return intersection; + if(!intersection.IsEmpty) + { + var result = resultBuffer.Slice(0, intersection.Length); + // No need to copy, data is already in buffer + return result; + } + + return Span.Empty; } } } \ No newline at end of file diff --git a/src/DotRecast.Detour/IDtPolyQuery.cs b/src/DotRecast.Detour/IDtPolyQuery.cs index 890d171b..5a6db49f 100644 --- a/src/DotRecast.Detour/IDtPolyQuery.cs +++ b/src/DotRecast.Detour/IDtPolyQuery.cs @@ -9,6 +9,6 @@ public interface IDtPolyQuery { /// Called for each batch of unique polygons touched by the search area in dtNavMeshQuery::queryPolygons. /// This can be called multiple times for a single query. - void Process(DtMeshTile tile, DtPoly[] poly, Span refs, int count); + void Process(DtMeshTile tile, Span poly, Span refs, int count); } } \ No newline at end of file diff --git a/src/DotRecast.Detour/IDtPolygonByCircleConstraint.cs b/src/DotRecast.Detour/IDtPolygonByCircleConstraint.cs index 9f46ff3a..138e508e 100644 --- a/src/DotRecast.Detour/IDtPolygonByCircleConstraint.cs +++ b/src/DotRecast.Detour/IDtPolygonByCircleConstraint.cs @@ -24,6 +24,6 @@ namespace DotRecast.Detour { public interface IDtPolygonByCircleConstraint { - float[] Apply(float[] polyVerts, RcVec3f circleCenter, float radius); + Span Apply(Span polyVerts, RcVec3f circleCenter, float radius, Span resultBuffer); } } \ No newline at end of file diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs index 4199c74f..256a1dfe 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs @@ -98,7 +98,7 @@ public DtStatus FindFollowPath(DtNavMesh navMesh, DtNavMeshQuery navQuery, long iterPos = result; - pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(ref pathIterPolys, pathIterPolyCount, MAX_POLYS, visited, nvisited); + pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(pathIterPolys, pathIterPolyCount, MAX_POLYS, visited, nvisited); pathIterPolyCount = DtPathUtils.FixupShortcuts(ref pathIterPolys, pathIterPolyCount, navQuery); var status = navQuery.GetPolyHeight(pathIterPolys[0], result, out var h); diff --git a/test/DotRecast.Core.Test/RcRentedArrayTest.cs b/test/DotRecast.Core.Test/RcRentedArrayTest.cs index 6ba6febf..9372d72a 100644 --- a/test/DotRecast.Core.Test/RcRentedArrayTest.cs +++ b/test/DotRecast.Core.Test/RcRentedArrayTest.cs @@ -103,4 +103,19 @@ public void TestDispose() Assert.That(r1.IsDisposed, Is.EqualTo(true)); Assert.That(r1.AsArray(), Is.Null); } + + [Test] + public void TestIdPoolCapacity() + { + var buffer = new RcRentedArray[RcRentedArray.START_RENT_ID_POOL_CAPACITY + 2]; + for (int i = 0; i < buffer.Length; i++) + { + Assert.DoesNotThrow(() => buffer[i] = RcRentedArray.Rent(4)); + } + + for (int i = 0; i < buffer.Length; i++) + { + Assert.DoesNotThrow(() => buffer[i].Dispose()); + } + } } \ No newline at end of file diff --git a/test/DotRecast.Detour.Test/ConvexConvexIntersectionTest.cs b/test/DotRecast.Detour.Test/ConvexConvexIntersectionTest.cs index f78f3312..71f71d7c 100644 --- a/test/DotRecast.Detour.Test/ConvexConvexIntersectionTest.cs +++ b/test/DotRecast.Detour.Test/ConvexConvexIntersectionTest.cs @@ -17,6 +17,7 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ +using System; using NUnit.Framework; namespace DotRecast.Detour.Test; @@ -29,9 +30,10 @@ public void ShouldHandleSamePolygonIntersection() { float[] p = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 }; float[] q = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 }; - float[] intersection = DtConvexConvexIntersections.Intersect(p, q); + float[] buffer = new float[128]; + Span intersection = DtConvexConvexIntersections.Intersect(p, q, buffer); Assert.That(intersection.Length, Is.EqualTo(5 * 3)); - Assert.That(intersection, Is.EqualTo(p)); + Assert.That(intersection.ToArray(), Is.EquivalentTo(p)); } [Test] @@ -39,8 +41,9 @@ public void ShouldHandleIntersection() { float[] p = { -5, 0, -5, -5, 0, 4, 1, 0, 4, 1, 0, -5 }; float[] q = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 }; - float[] intersection = DtConvexConvexIntersections.Intersect(p, q); + float[] buffer = new float[128]; + Span intersection = DtConvexConvexIntersections.Intersect(p, q, buffer); Assert.That(intersection.Length, Is.EqualTo(5 * 3)); - Assert.That(intersection, Is.EqualTo(new[] { 1, 0, 3, 1, 0, -3.4f, -2, 0, -4, -4, 0, 0, -3, 0, 3 })); + Assert.That(intersection.ToArray(), Is.EquivalentTo(new[] { 1, 0, 3, 1, 0, -3.4f, -2, 0, -4, -4, 0, 0, -3, 0, 3 })); } } \ No newline at end of file diff --git a/test/DotRecast.Detour.Test/PolygonByCircleConstraintTest.cs b/test/DotRecast.Detour.Test/PolygonByCircleConstraintTest.cs index 7e7c231e..4329506c 100644 --- a/test/DotRecast.Detour.Test/PolygonByCircleConstraintTest.cs +++ b/test/DotRecast.Detour.Test/PolygonByCircleConstraintTest.cs @@ -17,6 +17,7 @@ misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ +using System; using DotRecast.Core.Numerics; using NUnit.Framework; @@ -32,9 +33,12 @@ public void ShouldHandlePolygonFullyInsideCircle() { float[] polygon = { -2, 0, 2, 2, 0, 2, 2, 0, -2, -2, 0, -2 }; RcVec3f center = new RcVec3f(1, 0, 1); - float[] constrained = _constraint.Apply(polygon, center, 6); + var radius = 6; + + float[] buffer = new float[128]; + Span constrained = _constraint.Apply(polygon, center, radius, buffer); - Assert.That(constrained, Is.EqualTo(polygon)); + Assert.That(constrained.ToArray(), Is.EquivalentTo(polygon)); } [Test] @@ -43,10 +47,13 @@ public void ShouldHandleVerticalSegment() int expectedSize = 21; float[] polygon = { -2, 0, 2, 2, 0, 2, 2, 0, -2, -2, 0, -2 }; RcVec3f center = new RcVec3f(2, 0, 0); + var radius = 3; + + float[] buffer = new float[128]; + Span constrained = _constraint.Apply(polygon, center, radius, buffer); - float[] constrained = _constraint.Apply(polygon, center, 3); Assert.That(constrained.Length, Is.EqualTo(expectedSize)); - Assert.That(constrained, Is.SupersetOf(new[] { 2f, 0f, 2f, 2f, 0f, -2f })); + Assert.That(constrained.ToArray(), Is.SupersetOf(new[] { 2f, 0f, 2f, 2f, 0f, -2f })); } [Test] @@ -55,7 +62,10 @@ public void ShouldHandleCircleFullyInsidePolygon() int expectedSize = 12 * 3; float[] polygon = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 }; RcVec3f center = new RcVec3f(-1, 0, -1); - float[] constrained = _constraint.Apply(polygon, center, 2); + var radius = 2; + + float[] buffer = new float[128]; + Span constrained = _constraint.Apply(polygon, center, radius, buffer); Assert.That(constrained.Length, Is.EqualTo(expectedSize)); @@ -73,10 +83,13 @@ public void ShouldHandleCircleInsidePolygon() int expectedSize = 9 * 3; float[] polygon = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 }; RcVec3f center = new RcVec3f(-2, 0, -1); - float[] constrained = _constraint.Apply(polygon, center, 3); + var radius = 3; + + float[] buffer = new float[128]; + Span constrained = _constraint.Apply(polygon, center, radius, buffer); Assert.That(constrained.Length, Is.EqualTo(expectedSize)); - Assert.That(constrained, Is.SupersetOf(new[] { -2f, 0f, -4f, -4f, 0f, 0f, -3.4641016f, 0.0f, 1.60769534f, -2.0f, 0.0f, 2.0f })); + Assert.That(constrained.ToArray(), Is.SupersetOf(new[] { -2f, 0f, -4f, -4f, 0f, 0f, -3.4641016f, 0.0f, 1.60769534f, -2.0f, 0.0f, 2.0f })); } [Test] @@ -85,9 +98,12 @@ public void ShouldHandleCircleOutsidePolygon() int expectedSize = 7 * 3; float[] polygon = { -4, 0, 0, -3, 0, 3, 2, 0, 3, 3, 0, -3, -2, 0, -4 }; RcVec3f center = new RcVec3f(4, 0, 0); - float[] constrained = _constraint.Apply(polygon, center, 4); + var radius = 4; + + float[] buffer = new float[128]; + Span constrained = _constraint.Apply(polygon, center, radius, buffer); Assert.That(constrained.Length, Is.EqualTo(expectedSize)); - Assert.That(constrained, Is.SupersetOf(new[] { 1.53589869f, 0f, 3f, 2f, 0f, 3f, 3f, 0f, -3f })); + Assert.That(constrained.ToArray(), Is.SupersetOf(new[] { 1.53589869f, 0f, 3f, 2f, 0f, 3f, 3f, 0f, -3f })); } } \ No newline at end of file