diff --git a/CHANGELOG.md b/CHANGELOG.md index eb6df2f..9b7b35d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,7 +13,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Nothing ### Changed -- Nothing +- Changed `IDtPolyQuery` interface to make `Process()` more versatile ### Removed - Nothing diff --git a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs index 7c9f3d9..ab9364e 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs @@ -55,15 +55,19 @@ 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) => + navMeshQuery.QueryPolygons(pt, halfExtents, DtQueryNoOpFilter.Shared, new PolyQueryInvoker((tile, poly, refs, count) => { - var status = navMeshQuery.GetPolyHeight(refs, pt, out var h); - if (status.Succeeded()) + for (int i = 0; i < count; ++i) { - if (h > minHeight.Get() && h < maxHeight) + var status = navMeshQuery.GetPolyHeight(refs[i], pt, out var h); + if (status.Succeeded()) { - minHeight.Exchange(h); - found.Set(true); + if (h > minHeight.Get() && h < maxHeight) + { + minHeight.Exchange(h); + found.Set(true); + return; + } } } })); diff --git a/src/DotRecast.Detour.Extras/Jumplink/PolyQueryInvoker.cs b/src/DotRecast.Detour.Extras/Jumplink/PolyQueryInvoker.cs index 3ea5b9b..245b321 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/PolyQueryInvoker.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/PolyQueryInvoker.cs @@ -4,16 +4,16 @@ namespace DotRecast.Detour.Extras.Jumplink { public class PolyQueryInvoker : IDtPolyQuery { - public readonly Action _callback; + private readonly Action _callback; - public PolyQueryInvoker(Action callback) + public PolyQueryInvoker(Action callback) { _callback = callback; } - public void Process(DtMeshTile tile, DtPoly poly, long refs) + public void Process(DtMeshTile tile, DtPoly[] poly, long[] refs, int count) { - _callback?.Invoke(tile, poly, refs); + _callback?.Invoke(tile, poly, refs, count); } } } \ No newline at end of file diff --git a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs index 3aed2aa..ec3d62c 100644 --- a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs +++ b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs @@ -7,44 +7,49 @@ namespace DotRecast.Detour { private readonly DtNavMeshQuery _query; private readonly RcVec3f _center; - private long _nearestRef; - private RcVec3f _nearestPt; - private bool _overPoly; private float _nearestDistanceSqr; + private long _nearestRef; + private RcVec3f _nearestPoint; + private bool _overPoly; public DtFindNearestPolyQuery(DtNavMeshQuery query, RcVec3f center) { - this._query = query; - this._center = center; + _query = query; + _center = center; _nearestDistanceSqr = float.MaxValue; - _nearestPt = center; + _nearestPoint = center; } - public void Process(DtMeshTile tile, DtPoly poly, long refs) + public void Process(DtMeshTile tile, DtPoly[] poly, long[] refs, int count) { - // Find nearest polygon amongst the nearby polygons. - _query.ClosestPointOnPoly(refs, _center, out var closestPtPoly, out var posOverPoly); + for (int i = 0; i < count; ++i) + { + long polyRef = refs[i]; + float d; + + // Find nearest polygon amongst the nearby polygons. + _query.ClosestPointOnPoly(polyRef, _center, out var closestPtPoly, out var posOverPoly); - // If a point is directly over a polygon and closer than - // climb height, favor that instead of straight line nearest point. - float d = 0; - RcVec3f diff = RcVec3f.Subtract(_center, closestPtPoly); - if (posOverPoly) - { - d = MathF.Abs(diff.Y) - tile.data.header.walkableClimb; - d = d > 0 ? d * d : 0; - } - else - { - d = diff.LengthSquared(); - } + // If a point is directly over a polygon and closer than + // climb height, favor that instead of straight line nearest point. + RcVec3f diff = RcVec3f.Subtract(_center, closestPtPoly); + if (posOverPoly) + { + d = MathF.Abs(diff.Y) - tile.data.header.walkableClimb; + d = d > 0 ? d * d : 0; + } + else + { + d = diff.LengthSquared(); + } - if (d < _nearestDistanceSqr) - { - _nearestPt = closestPtPoly; - _nearestDistanceSqr = d; - _nearestRef = refs; - _overPoly = posOverPoly; + if (d < _nearestDistanceSqr) + { + _nearestPoint = closestPtPoly; + _nearestDistanceSqr = d; + _nearestRef = polyRef; + _overPoly = posOverPoly; + } } } @@ -55,7 +60,7 @@ namespace DotRecast.Detour public RcVec3f NearestPt() { - return _nearestPt; + return _nearestPoint; } public bool OverPoly() diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 2bfa491..f9e089d 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -20,6 +20,7 @@ freely, subject to the following restrictions: using System; using System.Collections.Generic; +using System.Reflection; using DotRecast.Core; using DotRecast.Core.Collections; using DotRecast.Core.Numerics; @@ -575,15 +576,22 @@ namespace DotRecast.Detour return DtStatus.DT_SUCCESS; } - // FIXME: (PP) duplicate? + /// Queries polygons within a tile. protected void QueryPolygonsInTile(DtMeshTile tile, RcVec3f qmin, RcVec3f qmax, IDtQueryFilter filter, IDtPolyQuery query) { + const int batchSize = 32; + long[] polyRefs = new long[batchSize]; + DtPoly[] polys = new DtPoly[batchSize]; + int n = 0; + if (tile.data.bvTree != null) { int nodeIndex = 0; + int end = tile.data.header.bvNodeCount; 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]; @@ -604,7 +612,6 @@ namespace DotRecast.Detour // Traverse tree long @base = m_nav.GetPolyRefBase(tile); - int end = tile.data.header.bvNodeCount; while (nodeIndex < end) { DtBVNode node = tile.data.bvTree[nodeIndex]; @@ -616,7 +623,18 @@ namespace DotRecast.Detour long refs = @base | (long)node.i; if (filter.PassFilter(refs, tile, tile.data.polys[node.i])) { - query.Process(tile, tile.data.polys[node.i], refs); + polyRefs[n] = refs; + polys[n] = tile.data.polys[node.i]; + + if (n == batchSize - 1) + { + query.Process(tile, polys, polyRefs, batchSize); + n = 0; + } + else + { + n++; + } } } @@ -645,6 +663,7 @@ namespace DotRecast.Detour continue; } + // Must pass filter long refs = @base | (long)i; if (!filter.PassFilter(refs, tile, p)) { @@ -664,10 +683,27 @@ namespace DotRecast.Detour if (DtUtils.OverlapBounds(qmin, qmax, bmin, bmax)) { - query.Process(tile, p, refs); + polyRefs[n] = refs; + polys[n] = p; + + if (n == batchSize - 1) + { + query.Process(tile, polys, polyRefs, batchSize); + n = 0; + } + else + { + n++; + } } } } + + // Process the last polygons that didn't make a full batch. + if (n > 0) + { + query.Process(tile, polys, polyRefs, n); + } } /** diff --git a/src/DotRecast.Detour/IDtPolyQuery.cs b/src/DotRecast.Detour/IDtPolyQuery.cs index 56fb244..0baf9c5 100644 --- a/src/DotRecast.Detour/IDtPolyQuery.cs +++ b/src/DotRecast.Detour/IDtPolyQuery.cs @@ -1,7 +1,12 @@ namespace DotRecast.Detour { + /// Provides custom polygon query behavior. + /// Used by dtNavMeshQuery::queryPolygons. + /// @ingroup detour public interface IDtPolyQuery { - void Process(DtMeshTile tile, DtPoly poly, long refs); + /// 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, long[] refs, int count); } } \ No newline at end of file diff --git a/test/DotRecast.Detour.Test/FindNearestPolyTest.cs b/test/DotRecast.Detour.Test/FindNearestPolyTest.cs index bf47916..2f662dc 100644 --- a/test/DotRecast.Detour.Test/FindNearestPolyTest.cs +++ b/test/DotRecast.Detour.Test/FindNearestPolyTest.cs @@ -44,11 +44,11 @@ public class FindNearestPolyTest : AbstractDetourTest { RcVec3f startPos = startPoss[i]; var status = query.FindNearestPoly(startPos, extents, filter, out var nearestRef, out var nearestPt, out var _); - Assert.That(status.Succeeded(), Is.True); - Assert.That(nearestRef, Is.EqualTo(POLY_REFS[i])); - Assert.That(nearestPt.X, Is.EqualTo(POLY_POS[i].X).Within(0.001f)); - Assert.That(nearestPt.Y, Is.EqualTo(POLY_POS[i].Y).Within(0.001f)); - Assert.That(nearestPt.Z, Is.EqualTo(POLY_POS[i].Z).Within(0.001f)); + Assert.That(status.Succeeded(), Is.True, $"index({i})"); + Assert.That(nearestRef, Is.EqualTo(POLY_REFS[i]), $"index({i})"); + Assert.That(nearestPt.X, Is.EqualTo(POLY_POS[i].X).Within(0.001f), $"index({i})"); + Assert.That(nearestPt.Y, Is.EqualTo(POLY_POS[i].Y).Within(0.001f), $"index({i})"); + Assert.That(nearestPt.Z, Is.EqualTo(POLY_POS[i].Z).Within(0.001f), $"index({i})"); } }