diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index 7467c06..482b9b4 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -245,10 +245,13 @@ namespace DotRecast.Detour.Crowd UpdateAgentParameters(ag, option); // Find nearest position on navmesh and place the agent there. - Result nearestPoly = _navQuery.FindNearestPoly(pos, _ext, _filters[ag.option.queryFilterType]); + var status = _navQuery.FindNearestPoly(pos, _ext, _filters[ag.option.queryFilterType], out var refs, out var nearest, out var _); + if (status.Failed()) + { + nearest = pos; + refs = 0; + } - var nearest = nearestPoly.Succeeded() ? nearestPoly.result.GetNearestPos() : pos; - long refs = nearestPoly.Succeeded() ? nearestPoly.result.GetNearestRef() : 0L; ag.corridor.Reset(refs, nearest); ag.boundary.Reset(); ag.partial = false; @@ -462,13 +465,8 @@ namespace DotRecast.Detour.Crowd { // Current location is not valid, try to reposition. // TODO: this can snap agents, how to handle that? - Result nearestPoly = _navQuery.FindNearestPoly(ag.npos, _ext, - _filters[ag.option.queryFilterType]); - agentRef = nearestPoly.Succeeded() ? nearestPoly.result.GetNearestRef() : 0L; - if (nearestPoly.Succeeded()) - { - agentPos = nearestPoly.result.GetNearestPos(); - } + _navQuery.FindNearestPoly(ag.npos, _ext, _filters[ag.option.queryFilterType], out agentRef, out var nearest, out var _); + agentPos = nearest; if (agentRef == 0) { @@ -507,14 +505,8 @@ namespace DotRecast.Detour.Crowd if (!_navQuery.IsValidPolyRef(ag.targetRef, _filters[ag.option.queryFilterType])) { // Current target is not valid, try to reposition. - Result fnp = _navQuery.FindNearestPoly(ag.targetPos, _ext, - _filters[ag.option.queryFilterType]); - ag.targetRef = fnp.Succeeded() ? fnp.result.GetNearestRef() : 0L; - if (fnp.Succeeded()) - { - ag.targetPos = fnp.result.GetNearestPos(); - } - + _navQuery.FindNearestPoly(ag.targetPos, _ext, _filters[ag.option.queryFilterType], out ag.targetRef, out var nearest, out var _); + ag.targetPos = nearest; replan = true; } diff --git a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs index 14c6f2f..06adcd2 100644 --- a/src/DotRecast.Detour/DtFindNearestPolyQuery.cs +++ b/src/DotRecast.Detour/DtFindNearestPolyQuery.cs @@ -6,30 +6,30 @@ namespace DotRecast.Detour { public class DtFindNearestPolyQuery : IDtPolyQuery { - private readonly DtNavMeshQuery query; - private readonly RcVec3f center; - private long nearestRef; - private RcVec3f nearestPt; - private bool overPoly; - private float nearestDistanceSqr; + private readonly DtNavMeshQuery _query; + private readonly RcVec3f _center; + private long _nearestRef; + private RcVec3f _nearestPt; + private bool _overPoly; + private float _nearestDistanceSqr; public DtFindNearestPolyQuery(DtNavMeshQuery query, RcVec3f center) { - this.query = query; - this.center = center; - nearestDistanceSqr = float.MaxValue; - nearestPt = center; + this._query = query; + this._center = center; + _nearestDistanceSqr = float.MaxValue; + _nearestPt = center; } public void Process(DtMeshTile tile, DtPoly poly, long refs) { // Find nearest polygon amongst the nearby polygons. - query.ClosestPointOnPoly(refs, center, out var closestPtPoly, out var posOverPoly); + _query.ClosestPointOnPoly(refs, _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 = center.Subtract(closestPtPoly); + RcVec3f diff = _center.Subtract(closestPtPoly); if (posOverPoly) { d = Math.Abs(diff.y) - tile.data.header.walkableClimb; @@ -40,18 +40,28 @@ namespace DotRecast.Detour d = RcVec3f.LenSqr(diff); } - if (d < nearestDistanceSqr) + if (d < _nearestDistanceSqr) { - nearestPt = closestPtPoly; - nearestDistanceSqr = d; - nearestRef = refs; - overPoly = posOverPoly; + _nearestPt = closestPtPoly; + _nearestDistanceSqr = d; + _nearestRef = refs; + _overPoly = posOverPoly; } } - public FindNearestPolyResult Result() + public long NearestRef() { - return new FindNearestPolyResult(nearestRef, nearestPt, overPoly); + return _nearestRef; + } + + public RcVec3f NearestPt() + { + return _nearestPt; + } + + public bool OverPoly() + { + return _overPoly; } } } \ No newline at end of file diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index e2774a2..2c2117e 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -559,29 +559,36 @@ namespace DotRecast.Detour return null != height ? Results.Success(height.Value) : Results.InvalidParam(); } - /** - * Finds the polygon nearest to the specified center point. If center and nearestPt point to an equal position, - * isOverPoly will be true; however there's also a special case of climb height inside the polygon - * - * @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 FindNearestPolyResult containing nearestRef, nearestPt and overPoly - */ - public Result FindNearestPoly(RcVec3f center, RcVec3f halfExtents, IDtQueryFilter filter) + /// Finds the polygon nearest to the specified center point. + /// [opt] means the specified parameter can be a null pointer, in that case the output parameter will not be set. + /// + /// @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] nearestRef The reference id of the nearest polygon. Will be set to 0 if no polygon is found. + /// @param[out] nearestPt The nearest point on the polygon. Unchanged if no polygon is found. [opt] [(x, y, z)] + /// @param[out] isOverPoly Set to true if the point's X/Z coordinate lies inside the polygon, false otherwise. Unchanged if no polygon is found. [opt] + /// @returns The status flags for the query. + public DtStatus FindNearestPoly(RcVec3f center, RcVec3f halfExtents, IDtQueryFilter filter, + out long nearestRef, out RcVec3f nearestPt, out bool isOverPoly) { + nearestRef = 0; + nearestPt = center; + isOverPoly = false; + // Get nearby polygons from proximity grid. DtFindNearestPolyQuery query = new DtFindNearestPolyQuery(this, center); DtStatus status = QueryPolygons(center, halfExtents, filter, query); if (status.Failed()) { - return Results.Of(status, ""); + return status; } - return Results.Success(query.Result()); + nearestRef = query.NearestRef(); + nearestPt = query.NearestPt(); + isOverPoly = query.OverPoly(); + + return DtStatus.DT_SUCCSESS; } // FIXME: (PP) duplicate? @@ -3318,7 +3325,7 @@ namespace DotRecast.Detour { return false; } - + // If cannot pass filter, assume flags has changed and boundary is invalid. if (!filter.PassFilter(refs, tile, poly)) { diff --git a/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs b/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs index d86c038..5e57f9f 100644 --- a/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs @@ -279,10 +279,10 @@ public class CrowdProfilingTool private void MoveMob(DtNavMeshQuery navquery, IDtQueryFilter filter, DtCrowdAgent ag, CrowdAgentData crowAgentData) { // Move somewhere - Result nearestPoly = navquery.FindNearestPoly(ag.npos, crowd.GetQueryExtents(), filter); - if (nearestPoly.Succeeded()) + var status = navquery.FindNearestPoly(ag.npos, crowd.GetQueryExtents(), filter, out var nearestRef, out var nearest, out var _); + if (status.Succeeded()) { - var status = navquery.FindRandomPointAroundCircle(nearestPoly.result.GetNearestRef(), crowAgentData.home, zoneRadius * 2f, filter, rnd, + status = navquery.FindRandomPointAroundCircle(nearestRef, crowAgentData.home, zoneRadius * 2f, filter, rnd, out var randomRef, out var randomPt); if (status.Succeeded()) { @@ -294,10 +294,10 @@ public class CrowdProfilingTool private void MoveVillager(DtNavMeshQuery navquery, IDtQueryFilter filter, DtCrowdAgent ag, CrowdAgentData crowAgentData) { // Move somewhere close - Result nearestPoly = navquery.FindNearestPoly(ag.npos, crowd.GetQueryExtents(), filter); - if (nearestPoly.Succeeded()) + var status = navquery.FindNearestPoly(ag.npos, crowd.GetQueryExtents(), filter, out var nearestRef, out var nearest, out var _); + if (status.Succeeded()) { - var status = navquery.FindRandomPointAroundCircle(nearestPoly.result.GetNearestRef(), crowAgentData.home, zoneRadius * 0.2f, filter, rnd, + status = navquery.FindRandomPointAroundCircle(nearestRef, crowAgentData.home, zoneRadius * 0.2f, filter, rnd, out var randomRef, out var randomPt); if (status.Succeeded()) { diff --git a/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs b/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs index c67b63f..1dfdf71 100644 --- a/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/CrowdTool.cs @@ -158,8 +158,7 @@ public class CrowdTool : ITool { IDtQueryFilter filter = new DtQueryDefaultFilter(); RcVec3f halfExtents = crowd.GetQueryExtents(); - Result result = navquery.FindNearestPoly(p, halfExtents, filter); - long refs = result.result.GetNearestRef(); + navquery.FindNearestPoly(p, halfExtents, filter, out var refs, out var nearest, out var _); if (refs != 0) { Result flags = nav.GetPolyFlags(refs); @@ -288,9 +287,7 @@ public class CrowdTool : ITool } else { - Result result = navquery.FindNearestPoly(p, halfExtents, filter); - m_targetRef = result.result.GetNearestRef(); - m_targetPos = result.result.GetNearestPos(); + navquery.FindNearestPoly(p, halfExtents, filter, out m_targetRef, out m_targetPos, out var _); if (m_agentDebug.agent != null) { crowd.RequestMoveTarget(m_agentDebug.agent, m_targetRef, m_targetPos); @@ -798,10 +795,8 @@ public class CrowdTool : ITool { return "Crowd"; } - + public void HandleClickRay(RcVec3f start, RcVec3f direction, bool shift) { - } - } \ No newline at end of file diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index 4a66888..432f077 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -168,7 +168,7 @@ public class TestNavmeshTool : ITool DtNavMeshQuery m_navQuery = m_sample.GetNavMeshQuery(); if (m_sposSet) { - m_startRef = m_navQuery.FindNearestPoly(m_spos, m_polyPickExt, m_filter).result?.GetNearestRef() ?? 0; + m_navQuery.FindNearestPoly(m_spos, m_polyPickExt, m_filter, out m_startRef, out var _, out var _); } else { @@ -177,7 +177,7 @@ public class TestNavmeshTool : ITool if (m_eposSet) { - m_endRef = m_navQuery.FindNearestPoly(m_epos, m_polyPickExt, m_filter).result?.GetNearestRef() ?? 0; + m_navQuery.FindNearestPoly(m_epos, m_polyPickExt, m_filter, out m_endRef, out var _, out var _); } else { diff --git a/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs b/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs index fc1870a..456cb24 100644 --- a/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs +++ b/test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs @@ -143,10 +143,10 @@ public class AbstractCrowdTest } else { - Result nearest = query.FindNearestPoly(pos, ext, filter); + query.FindNearestPoly(pos, ext, filter, out var refs, out var nearest, out var _); foreach (DtCrowdAgent ag in crowd.GetActiveAgents()) { - crowd.RequestMoveTarget(ag, nearest.result.GetNearestRef(), nearest.result.GetNearestPos()); + crowd.RequestMoveTarget(ag, refs, nearest); } } } diff --git a/test/DotRecast.Detour.Dynamic.Test/DynamicNavMeshTest.cs b/test/DotRecast.Detour.Dynamic.Test/DynamicNavMeshTest.cs index de47d10..164e081 100644 --- a/test/DotRecast.Detour.Dynamic.Test/DynamicNavMeshTest.cs +++ b/test/DotRecast.Detour.Dynamic.Test/DynamicNavMeshTest.cs @@ -34,30 +34,35 @@ public class DynamicNavMeshTest Task future = mesh.Build(Task.Factory); // wait for build to complete bool _ = future.Result; + // create new query DtNavMeshQuery query = new DtNavMeshQuery(mesh.NavMesh()); IDtQueryFilter filter = new DtQueryDefaultFilter(); + // find path - FindNearestPolyResult start = query.FindNearestPoly(START_POS, EXTENT, filter).result; - FindNearestPolyResult end = query.FindNearestPoly(END_POS, EXTENT, filter).result; - List path = query.FindPath(start.GetNearestRef(), end.GetNearestRef(), start.GetNearestPos(), - end.GetNearestPos(), filter, DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result; + query.FindNearestPoly(START_POS, EXTENT, filter, out var startRef, out var startPt, out var _); + query.FindNearestPoly(END_POS, EXTENT, filter, out var endRef, out var endPt, out var _); + + List path = query.FindPath(startRef, endRef, startPt, endPt, filter, DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result; // check path length without any obstacles Assert.That(path.Count, Is.EqualTo(16)); + // place obstacle ICollider colldier = new SphereCollider(SPHERE_POS, 20, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_GROUND, 0.1f); long colliderId = mesh.AddCollider(colldier); + // update navmesh asynchronously future = mesh.Update(Task.Factory); // wait for update to complete _ = future.Result; // create new query query = new DtNavMeshQuery(mesh.NavMesh()); + // find path again - start = query.FindNearestPoly(START_POS, EXTENT, filter).result; - end = query.FindNearestPoly(END_POS, EXTENT, filter).result; - path = query.FindPath(start.GetNearestRef(), end.GetNearestRef(), start.GetNearestPos(), end.GetNearestPos(), filter, - DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result; + query.FindNearestPoly(START_POS, EXTENT, filter, out startRef, out startPt, out var _); + query.FindNearestPoly(END_POS, EXTENT, filter, out endRef, out endPt, out var _); + path = query.FindPath(startRef, endRef, startPt, endPt, filter, DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result; + // check path length with obstacles Assert.That(path.Count, Is.EqualTo(19)); // remove obstacle @@ -68,11 +73,12 @@ public class DynamicNavMeshTest _ = future.Result; // create new query query = new DtNavMeshQuery(mesh.NavMesh()); + // find path one more time - start = query.FindNearestPoly(START_POS, EXTENT, filter).result; - end = query.FindNearestPoly(END_POS, EXTENT, filter).result; - path = query.FindPath(start.GetNearestRef(), end.GetNearestRef(), start.GetNearestPos(), end.GetNearestPos(), filter, - DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result; + query.FindNearestPoly(START_POS, EXTENT, filter, out startRef, out startPt, out var _); + query.FindNearestPoly(END_POS, EXTENT, filter, out endRef, out endPt, out var _); + path = query.FindPath(startRef, endRef, startPt, endPt, filter, DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE, float.MaxValue).result; + // path length should be back to the initial value Assert.That(path.Count, Is.EqualTo(16)); } diff --git a/test/DotRecast.Detour.Extras.Test/Unity/Astar/UnityAStarPathfindingImporterTest.cs b/test/DotRecast.Detour.Extras.Test/Unity/Astar/UnityAStarPathfindingImporterTest.cs index 510b9d3..06c9027 100644 --- a/test/DotRecast.Detour.Extras.Test/Unity/Astar/UnityAStarPathfindingImporterTest.cs +++ b/test/DotRecast.Detour.Extras.Test/Unity/Astar/UnityAStarPathfindingImporterTest.cs @@ -66,15 +66,15 @@ public class UnityAStarPathfindingImporterTest DtMeshData data = tile.data; DtBVNode[] bvNodes = data.bvTree; data.bvTree = null; // set BV-Tree empty to get 'clear' search poly without BV - FindNearestPolyResult clearResult = GetNearestPolys(mesh, position)[0]; // check poly to exists + var clearResult = GetNearestPolys(mesh, position)[0]; // check poly to exists // restore BV-Tree and try search again // important aspect in that test: BV result must equals result without BV // if poly not found or found other poly - tile bounds is wrong! data.bvTree = bvNodes; - FindNearestPolyResult bvResult = GetNearestPolys(mesh, position)[0]; + var bvResult = GetNearestPolys(mesh, position)[0]; - Assert.That(bvResult.GetNearestRef(), Is.EqualTo(clearResult.GetNearestRef())); + Assert.That(bvResult.refs, Is.EqualTo(clearResult.refs)); } private DtNavMesh LoadNavMesh(string filename) @@ -95,24 +95,25 @@ public class UnityAStarPathfindingImporterTest DtNavMeshQuery query = new DtNavMeshQuery(mesh); IDtQueryFilter filter = new DtQueryDefaultFilter(); - FindNearestPolyResult[] polys = GetNearestPolys(mesh, startPos, endPos); - return query.FindPath(polys[0].GetNearestRef(), polys[1].GetNearestRef(), startPos, endPos, filter); + var polys = GetNearestPolys(mesh, startPos, endPos); + return query.FindPath(polys[0].refs, polys[1].refs, startPos, endPos, filter); } - private FindNearestPolyResult[] GetNearestPolys(DtNavMesh mesh, params RcVec3f[] positions) + private DtPolyPoint[] GetNearestPolys(DtNavMesh mesh, params RcVec3f[] positions) { DtNavMeshQuery query = new DtNavMeshQuery(mesh); IDtQueryFilter filter = new DtQueryDefaultFilter(); RcVec3f extents = RcVec3f.Of(0.1f, 0.1f, 0.1f); - FindNearestPolyResult[] results = new FindNearestPolyResult[positions.Length]; + var results = new DtPolyPoint[positions.Length]; for (int i = 0; i < results.Length; i++) { RcVec3f position = positions[i]; - Result result = query.FindNearestPoly(position, extents, filter); - Assert.That(result.Succeeded(), Is.True); - Assert.That(result.result.GetNearestPos(), Is.Not.EqualTo(RcVec3f.Zero), "Nearest start position is null!"); - results[i] = result.result; + var status = query.FindNearestPoly(position, extents, filter, out var nearestRef, out var nearest, out var _); + Assert.That(status.Succeeded(), Is.True); + Assert.That(nearest, Is.Not.EqualTo(RcVec3f.Zero), "Nearest start position is null!"); + + results[i] = new DtPolyPoint(nearestRef, nearest); } return results; diff --git a/test/DotRecast.Detour.Test/FindNearestPolyTest.cs b/test/DotRecast.Detour.Test/FindNearestPolyTest.cs index 2f5d3f7..4625d82 100644 --- a/test/DotRecast.Detour.Test/FindNearestPolyTest.cs +++ b/test/DotRecast.Detour.Test/FindNearestPolyTest.cs @@ -42,12 +42,12 @@ public class FindNearestPolyTest : AbstractDetourTest for (int i = 0; i < startRefs.Length; i++) { RcVec3f startPos = startPoss[i]; - Result poly = query.FindNearestPoly(startPos, extents, filter); - Assert.That(poly.Succeeded(), Is.True); - Assert.That(poly.result.GetNearestRef(), Is.EqualTo(POLY_REFS[i])); + var status = query.FindNearestPoly(startPos, extents, filter, out var nearestRef, out var nearest, out var _); + Assert.That(status.Succeeded(), Is.True); + Assert.That(nearestRef, Is.EqualTo(POLY_REFS[i])); for (int v = 0; v < POLY_POS[i].Length; v++) { - Assert.That(poly.result.GetNearestPos()[v], Is.EqualTo(POLY_POS[i][v]).Within(0.001f)); + Assert.That(nearest[v], Is.EqualTo(POLY_POS[i][v]).Within(0.001f)); } } } @@ -61,12 +61,12 @@ public class FindNearestPolyTest : AbstractDetourTest for (int i = 0; i < startRefs.Length; i++) { RcVec3f startPos = startPoss[i]; - Result poly = query.FindNearestPoly(startPos, extents, filter); - Assert.That(poly.Succeeded(), Is.True); - Assert.That(poly.result.GetNearestRef(), Is.EqualTo(0L)); + var status = query.FindNearestPoly(startPos, extents, filter, out var nearestRef, out var nearest, out var _); + Assert.That(status.Succeeded(), Is.True); + Assert.That(nearestRef, Is.EqualTo(0L)); for (int v = 0; v < POLY_POS[i].Length; v++) { - Assert.That(poly.result.GetNearestPos()[v], Is.EqualTo(startPos[v]).Within(0.001f)); + Assert.That(nearest[v], Is.EqualTo(startPos[v]).Within(0.001f)); } } } diff --git a/test/DotRecast.Detour.TileCache.Test/TileCacheFindPathTest.cs b/test/DotRecast.Detour.TileCache.Test/TileCacheFindPathTest.cs index f3c224e..c5c5626 100644 --- a/test/DotRecast.Detour.TileCache.Test/TileCacheFindPathTest.cs +++ b/test/DotRecast.Detour.TileCache.Test/TileCacheFindPathTest.cs @@ -49,12 +49,9 @@ public class TileCacheFindPathTest : AbstractTileCacheTest { IDtQueryFilter filter = new DtQueryDefaultFilter(); RcVec3f extents = RcVec3f.Of(2f, 4f, 2f); - Result findPolyStart = query.FindNearestPoly(start, extents, filter); - Result findPolyEnd = query.FindNearestPoly(end, extents, filter); - long startRef = findPolyStart.result.GetNearestRef(); - long endRef = findPolyEnd.result.GetNearestRef(); - RcVec3f startPos = findPolyStart.result.GetNearestPos(); - RcVec3f endPos = findPolyEnd.result.GetNearestPos(); + query.FindNearestPoly(start, extents, filter, out var startRef, out var startPos, out var _); + query.FindNearestPoly(end, extents, filter, out var endRef, out var endPos, out var _); + Result> path = query.FindPath(startRef, endRef, startPos, endPos, filter); int maxStraightPath = 256; int options = 0;