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

Changed `IDtPolyQuery` interface to make `Process()` more versatile

This commit is contained in:
ikpil 2024-05-18 14:49:18 +09:00 committed by Ikpil
parent 0ce8ffba32
commit 3808c13876
7 changed files with 100 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CHANGELOG.md
View File

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

16
src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs
View File

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

8
src/DotRecast.Detour.Extras/Jumplink/PolyQueryInvoker.cs
View File

@ -4,16 +4,16 @@ namespace DotRecast.Detour.Extras.Jumplink
{
public class PolyQueryInvoker : IDtPolyQuery
{
public readonly Action<DtMeshTile, DtPoly, long> _callback;
private readonly Action<DtMeshTile, DtPoly[], long[], int> _callback;
public PolyQueryInvoker(Action<DtMeshTile, DtPoly, long> callback)
public PolyQueryInvoker(Action<DtMeshTile, DtPoly[], long[], int> 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);
}
}
}

63
src/DotRecast.Detour/DtFindNearestPolyQuery.cs
View File

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

44
src/DotRecast.Detour/DtNavMeshQuery.cs
View File

@ -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<int> bmin = stackalloc int[3];
Span<int> 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);
}
}
/**

7
src/DotRecast.Detour/IDtPolyQuery.cs
View File

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

10
test/DotRecast.Detour.Test/FindNearestPolyTest.cs
View File

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