diff --git a/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs index 9b80d30..bc1d249 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs @@ -7,11 +7,14 @@ namespace DotRecast.Detour.Extras.Jumplink { public abstract class AbstractGroundSampler : IGroundSampler { - protected void SampleGround(JumpLinkBuilderConfig acfg, EdgeSampler es, Func> heightFunc) + public delegate bool ComputeNavMeshHeight(RcVec3f pt, float cellSize, out float height); + + protected void SampleGround(JumpLinkBuilderConfig acfg, EdgeSampler es, ComputeNavMeshHeight heightFunc) { float cs = acfg.cellSize; float dist = (float)Math.Sqrt(RcVec3f.Dist2DSqr(es.start.p, es.start.q)); int ngsamples = Math.Max(2, (int)Math.Ceiling(dist / cs)); + SampleGroundSegment(heightFunc, es.start, ngsamples); foreach (GroundSegment end in es.end) { @@ -21,7 +24,7 @@ namespace DotRecast.Detour.Extras.Jumplink public abstract void Sample(JumpLinkBuilderConfig acfg, RecastBuilderResult result, EdgeSampler es); - protected void SampleGroundSegment(Func> heightFunc, GroundSegment seg, int nsamples) + protected void SampleGroundSegment(ComputeNavMeshHeight heightFunc, GroundSegment seg, int nsamples) { seg.gsamples = new GroundSample[nsamples]; @@ -32,12 +35,12 @@ namespace DotRecast.Detour.Extras.Jumplink GroundSample s = new GroundSample(); seg.gsamples[i] = s; RcVec3f pt = RcVec3f.Lerp(seg.p, seg.q, u); - Tuple height = heightFunc.Invoke(pt, seg.height); + bool success = heightFunc.Invoke(pt, seg.height, out var height); s.p.x = pt.x; - s.p.y = height.Item2; + s.p.y = height; s.p.z = pt.z; - if (!height.Item1) + if (!success) { continue; } diff --git a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs index fdaaa31..4f16dc8 100644 --- a/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs +++ b/src/DotRecast.Detour.Extras/Jumplink/NavMeshGroundSampler.cs @@ -12,7 +12,7 @@ namespace DotRecast.Detour.Extras.Jumplink public override void Sample(JumpLinkBuilderConfig acfg, RecastBuilderResult result, EdgeSampler es) { DtNavMeshQuery navMeshQuery = CreateNavMesh(result, acfg.agentRadius, acfg.agentHeight, acfg.agentClimb); - SampleGround(acfg, es, (pt, h) => GetNavMeshHeight(navMeshQuery, pt, acfg.cellSize, h)); + SampleGround(acfg, es, (RcVec3f pt, float heightRange, out float height) => GetNavMeshHeight(navMeshQuery, pt, acfg.cellSize, heightRange, out height)); } private DtNavMeshQuery CreateNavMesh(RecastBuilderResult r, float agentRadius, float agentHeight, float agentClimb) @@ -42,12 +42,15 @@ namespace DotRecast.Detour.Extras.Jumplink } - private Tuple GetNavMeshHeight(DtNavMeshQuery navMeshQuery, RcVec3f pt, float cs, float heightRange) + private bool GetNavMeshHeight(DtNavMeshQuery navMeshQuery, RcVec3f pt, float cs, float heightRange, out float height) { + height = default; + 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); + navMeshQuery.QueryPolygons(pt, halfExtents, filter, new PolyQueryInvoker((tile, poly, refs) => { var status = navMeshQuery.GetPolyHeight(refs, pt, out var h); @@ -60,12 +63,15 @@ namespace DotRecast.Detour.Extras.Jumplink } } })); + if (found.Get()) { - return Tuple.Create(true, minHeight.Get()); + height = minHeight.Get(); + return true; } - return Tuple.Create(false, pt.y); + height = pt.y; + return false; } } } \ No newline at end of file