From c9a54d4b4e462031f33fa258ccc5901be518f363 Mon Sep 17 00:00:00 2001 From: ikpil Date: Sun, 19 May 2024 09:16:25 +0900 Subject: [PATCH] Added `DtCollectPolysQuery` and `FindCollectPolyTest` --- CHANGELOG.md | 3 +- .../Jumplink/NavMeshGroundSampler.cs | 2 +- .../DtCallbackPolyQuery.cs} | 6 +- src/DotRecast.Detour/DtCollectPolysQuery.cs | 42 ++++++++++++ src/DotRecast.Detour/DtNavMeshQuery.cs | 65 ++++++++++++++---- .../FindCollectPolyTest.cs | 68 +++++++++++++++++++ .../FindNearestPolyTest.cs | 6 +- 7 files changed, 170 insertions(+), 22 deletions(-) rename src/{DotRecast.Detour.Extras/Jumplink/PolyQueryInvoker.cs => DotRecast.Detour/DtCallbackPolyQuery.cs} (64%) create mode 100644 src/DotRecast.Detour/DtCollectPolysQuery.cs create mode 100644 test/DotRecast.Detour.Test/FindCollectPolyTest.cs diff --git a/CHANGELOG.md b/CHANGELOG.md index 9b7b35d..664b2ab 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,13 +7,14 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] - yyyy-mm-dd ### Added -- Nothing +- Added `DtCollectPolysQuery` and `FindCollectPolyTest` ### Fixed - Nothing ### Changed - Changed `IDtPolyQuery` interface to make `Process()` more versatile +- Changed `PolyQueryInvoker` to `DtActionPolyQuery` ### Removed - Nothing diff --git a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs index ab9364e..93085ca 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs @@ -55,7 +55,7 @@ namespace DotRecast.Detour.Extras.Jumplink RcAtomicBoolean found = new RcAtomicBoolean(); RcAtomicFloat minHeight = new RcAtomicFloat(pt.Y); - navMeshQuery.QueryPolygons(pt, halfExtents, DtQueryNoOpFilter.Shared, new PolyQueryInvoker((tile, poly, refs, count) => + navMeshQuery.QueryPolygons(pt, halfExtents, DtQueryNoOpFilter.Shared, new DtCallbackPolyQuery((tile, poly, refs, count) => { for (int i = 0; i < count; ++i) { diff --git a/src/DotRecast.Detour.Extras/Jumplink/PolyQueryInvoker.cs b/src/DotRecast.Detour/DtCallbackPolyQuery.cs similarity index 64% rename from src/DotRecast.Detour.Extras/Jumplink/PolyQueryInvoker.cs rename to src/DotRecast.Detour/DtCallbackPolyQuery.cs index 245b321..79d9c31 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/PolyQueryInvoker.cs +++ b/src/DotRecast.Detour/DtCallbackPolyQuery.cs @@ -1,12 +1,12 @@ using System; -namespace DotRecast.Detour.Extras.Jumplink +namespace DotRecast.Detour { - public class PolyQueryInvoker : IDtPolyQuery + public class DtCallbackPolyQuery : IDtPolyQuery { private readonly Action _callback; - public PolyQueryInvoker(Action callback) + public DtCallbackPolyQuery(Action callback) { _callback = callback; } diff --git a/src/DotRecast.Detour/DtCollectPolysQuery.cs b/src/DotRecast.Detour/DtCollectPolysQuery.cs new file mode 100644 index 0000000..31acaac --- /dev/null +++ b/src/DotRecast.Detour/DtCollectPolysQuery.cs @@ -0,0 +1,42 @@ +using DotRecast.Core; + +namespace DotRecast.Detour +{ + public class DtCollectPolysQuery : IDtPolyQuery + { + private long[] m_polys; + private int m_maxPolys; + private int m_numCollected; + private bool m_overflow; + + public DtCollectPolysQuery(long[] polys, int maxPolys) + { + m_polys = polys; + m_maxPolys = maxPolys; + } + + public int NumCollected() + { + return m_numCollected; + } + + public bool Overflowed() + { + return m_overflow; + } + + public void Process(DtMeshTile tile, DtPoly[] poly, long[] refs, int count) + { + int numLeft = m_maxPolys - m_numCollected; + int toCopy = count; + if (toCopy > numLeft) + { + m_overflow = true; + toCopy = numLeft; + } + + RcSpans.Copy(refs, 0, m_polys, m_numCollected, toCopy); + m_numCollected += toCopy; + } + } +} \ No newline at end of file diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index f9e089d..e815fd3 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -591,7 +591,7 @@ namespace DotRecast.Detour var tbmin = tile.data.header.bmin; var tbmax = tile.data.header.bmax; float qfac = tile.data.header.bvQuantFactor; - + // Calculate quantized box Span bmin = stackalloc int[3]; Span bmax = stackalloc int[3]; @@ -625,7 +625,7 @@ namespace DotRecast.Detour { polyRefs[n] = refs; polys[n] = tile.data.polys[node.i]; - + if (n == batchSize - 1) { query.Process(tile, polys, polyRefs, batchSize); @@ -706,19 +706,54 @@ namespace DotRecast.Detour } } - /** - * Finds polygons that overlap the search box. - * - * If no polygons are found, the function will return with a polyCount of zero. - * - * @param center - * The center of the search box. [(x, y, z)] - * @param halfExtents - * The search distance along each axis. [(x, y, z)] - * @param filter - * The polygon filter to apply to the query. - * @return The reference ids of the polygons that overlap the query box. - */ + /// @par + /// + /// If no polygons are found, the function will return #DT_SUCCESS with a + /// @p polyCount of zero. + /// + /// If @p polys is too small to hold the entire result set, then the array will + /// be filled to capacity. The method of choosing which polygons from the + /// full set are included in the partial result set is undefined. + /// + /// Finds polygons that overlap the search box. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] + /// @param[in] filter The polygon filter to apply to the query. + /// @param[out] polys The reference ids of the polygons that overlap the query box. + /// @param[out] polyCount The number of polygons in the search result. + /// @param[in] maxPolys The maximum number of polygons the search result can hold. + /// @returns The status flags for the query. + public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, + IDtQueryFilter filter, + long[] polys, out int polyCount, int maxPolys) + { + polyCount = 0; + if (null == polys || maxPolys < 0) + return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; + + DtCollectPolysQuery collector = new DtCollectPolysQuery(polys, maxPolys); + DtStatus status = QueryPolygons(center, halfExtents, filter, collector); + if (status.Failed()) + return status; + + polyCount = collector.NumCollected(); + return collector.Overflowed() + ? DtStatus.DT_SUCCESS | DtStatus.DT_BUFFER_TOO_SMALL + : DtStatus.DT_SUCCESS; + } + + /// @par + /// + /// The query will be invoked with batches of polygons. Polygons passed + /// to the query have bounding boxes that overlap with the center and halfExtents + /// passed to this function. The dtPolyQuery::process function is invoked multiple + /// times until all overlapping polygons have been processed. + /// + /// Finds polygons that overlap the search box. + /// @param[in] center The center of the search box. [(x, y, z)] + /// @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) { if (!center.IsFinite() || !halfExtents.IsFinite() || null == filter) diff --git a/test/DotRecast.Detour.Test/FindCollectPolyTest.cs b/test/DotRecast.Detour.Test/FindCollectPolyTest.cs new file mode 100644 index 0000000..1b7b8a5 --- /dev/null +++ b/test/DotRecast.Detour.Test/FindCollectPolyTest.cs @@ -0,0 +1,68 @@ +using System; +using System.Linq; +using DotRecast.Core.Numerics; +using NUnit.Framework; + +namespace DotRecast.Detour.Test; + +public class FindCollectPolyTest : AbstractDetourTest +{ + private static readonly long[][] POLY_REFS = + { + new long[] + { + 281474976710697L, + 281474976710695L, + 281474976710696L, + 281474976710691L, + }, + new long[] + { + 281474976710769L, + 281474976710773L, + }, + new long[] + { + 281474976710676L, + 281474976710678L, + 281474976710679L, + 281474976710674L, + 281474976710677L, + 281474976710683L, + 281474976710680L, + 281474976710684L, + }, + + new long[] + { + 281474976710748L, + 281474976710753L, + 281474976710752L, + 281474976710750L, + }, + + new long[] + { + 281474976710736L, + 281474976710733L, + 281474976710735L, + } + }; + + [Test] + public void TestFindNearestPoly() + { + IDtQueryFilter filter = new DtQueryDefaultFilter(); + RcVec3f extents = new RcVec3f(2, 4, 2); + var polys = new long[32]; + for (int i = 0; i < startRefs.Length; i++) + { + Array.Fill(polys, 0); + RcVec3f startPos = startPoss[i]; + var status = query.QueryPolygons(startPos, extents, filter, polys, out var polyCount, 32); + Assert.That(status.Succeeded(), Is.True, $"index({i})"); + Assert.That(polyCount, Is.EqualTo(POLY_REFS[i].Length), $"index({i})"); + Assert.That(polys.AsSpan(0, polyCount).ToArray(), Is.EqualTo(POLY_REFS[i]), $"index({i})"); + } + } +} \ No newline at end of file diff --git a/test/DotRecast.Detour.Test/FindNearestPolyTest.cs b/test/DotRecast.Detour.Test/FindNearestPolyTest.cs index 2f662dc..2f6a25a 100644 --- a/test/DotRecast.Detour.Test/FindNearestPolyTest.cs +++ b/test/DotRecast.Detour.Test/FindNearestPolyTest.cs @@ -21,10 +21,12 @@ using NUnit.Framework; namespace DotRecast.Detour.Test; - public class FindNearestPolyTest : AbstractDetourTest { - private static readonly long[] POLY_REFS = { 281474976710696L, 281474976710773L, 281474976710680L, 281474976710753L, 281474976710733L }; + private static readonly long[] POLY_REFS = + { + 281474976710696L, 281474976710773L, 281474976710680L, 281474976710753L, 281474976710733L + }; private static readonly RcVec3f[] POLY_POS = {