From 9c42e58e86ce146f4d12315901b4f6967e673493 Mon Sep 17 00:00:00 2001 From: ikpil Date: Tue, 20 Jun 2023 01:36:03 +0900 Subject: [PATCH] change FindCorners --- src/DotRecast.Detour.Crowd/DtCrowd.cs | 2 +- src/DotRecast.Detour.Crowd/DtPathCorridor.cs | 18 ++-- src/DotRecast.Detour/DtNavMeshQuery.cs | 89 ++++++++++--------- src/DotRecast.Detour/DtStatus.cs | 3 +- src/DotRecast.Detour/PathUtils.cs | 4 +- src/DotRecast.Detour/StraightPathItem.cs | 2 +- .../Tools/TestNavmeshTool.cs | 18 ++-- .../PathCorridorTest.cs | 50 +++++++---- test/DotRecast.Detour.Test/FindPathTest.cs | 5 +- .../TileCacheFindPathTest.cs | 6 +- 10 files changed, 102 insertions(+), 95 deletions(-) diff --git a/src/DotRecast.Detour.Crowd/DtCrowd.cs b/src/DotRecast.Detour.Crowd/DtCrowd.cs index 0c7f4be..3b0ff83 100644 --- a/src/DotRecast.Detour.Crowd/DtCrowd.cs +++ b/src/DotRecast.Detour.Crowd/DtCrowd.cs @@ -942,7 +942,7 @@ namespace DotRecast.Detour.Crowd } // Find corners for steering - ag.corners = ag.corridor.FindCorners(DT_CROWDAGENT_MAX_CORNERS, _navQuery, _filters[ag.option.queryFilterType]); + ag.corridor.FindCorners(ref ag.corners, DT_CROWDAGENT_MAX_CORNERS, _navQuery, _filters[ag.option.queryFilterType]); // Check to see if the corner after the next corner is directly visible, // and short cut to there. diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs index 83edb8f..0be8de8 100644 --- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs +++ b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs @@ -234,16 +234,14 @@ namespace DotRecast.Detour.Crowd * @param[in] navquery The query object used to build the corridor. * @return Corners */ - public List FindCorners(int maxCorners, DtNavMeshQuery navquery, IDtQueryFilter filter) + public int FindCorners(ref List corners, int maxCorners, DtNavMeshQuery navquery, IDtQueryFilter filter) { - List path = new List(); - Result> result = navquery.FindStraightPath(m_pos, m_target, m_path, maxCorners, 0); + var result = navquery.FindStraightPath(m_pos, m_target, m_path, ref corners, maxCorners, 0); if (result.Succeeded()) { - path = result.result; // Prune points in the beginning of the path which are too close. int start = 0; - foreach (StraightPathItem spi in path) + foreach (StraightPathItem spi in corners) { if ((spi.GetFlags() & DtNavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 || RcVec3f.Dist2DSqr(spi.GetPos(), m_pos) > MIN_TARGET_DIST) @@ -254,11 +252,11 @@ namespace DotRecast.Detour.Crowd start++; } - int end = path.Count; + int end = corners.Count; // Prune points after an off-mesh connection. - for (int i = start; i < path.Count; i++) + for (int i = start; i < corners.Count; i++) { - StraightPathItem spi = path[i]; + StraightPathItem spi = corners[i]; if ((spi.GetFlags() & DtNavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0) { end = i + 1; @@ -266,10 +264,10 @@ namespace DotRecast.Detour.Crowd } } - path = path.GetRange(start, end - start); + corners = corners.GetRange(start, end - start); } - return path; + return corners.Count; } /** diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs index 0cbda03..9cc15cc 100644 --- a/src/DotRecast.Detour/DtNavMeshQuery.cs +++ b/src/DotRecast.Detour/DtNavMeshQuery.cs @@ -1422,7 +1422,7 @@ namespace DotRecast.Detour return Results.Of(status, path); } - protected DtStatus AppendVertex(RcVec3f pos, int flags, long refs, List straightPath, + protected DtStatus AppendVertex(RcVec3f pos, int flags, long refs, ref List straightPath, int maxStraightPath) { if (straightPath.Count > 0 && DetourCommon.VEqual(straightPath[straightPath.Count - 1].pos, pos)) @@ -1450,7 +1450,7 @@ namespace DotRecast.Detour } protected DtStatus AppendPortals(int startIdx, int endIdx, RcVec3f endPos, List path, - List straightPath, int maxStraightPath, int options) + ref List straightPath, int maxStraightPath, int options) { var startPos = straightPath[straightPath.Count - 1].pos; // Append or update last vertex @@ -1491,7 +1491,7 @@ namespace DotRecast.Detour if (DetourCommon.IntersectSegSeg2D(startPos, endPos, left, right, out var _, out var t)) { var pt = RcVec3f.Lerp(left, right, t); - stat = AppendVertex(pt, 0, path[i + 1], straightPath, maxStraightPath); + stat = AppendVertex(pt, 0, path[i + 1], ref straightPath, maxStraightPath); if (!stat.InProgress()) { return stat; @@ -1503,58 +1503,61 @@ namespace DotRecast.Detour } /// @par - /// Finds the straight path from the start to the end position within the polygon corridor. - /// + /// /// This method peforms what is often called 'string pulling'. /// - /// The start position is clamped to the first polygon in the path, and the - /// end position is clamped to the last. So the start and end positions should + /// The start position is clamped to the first polygon in the path, and the + /// end position is clamped to the last. So the start and end positions should /// normally be within or very near the first and last polygons respectively. /// - /// The returned polygon references represent the reference id of the polygon - /// that is entered at the associated path position. The reference id associated - /// with the end point will always be zero. This allows, for example, matching + /// The returned polygon references represent the reference id of the polygon + /// that is entered at the associated path position. The reference id associated + /// with the end point will always be zero. This allows, for example, matching /// off-mesh link points to their representative polygons. /// - /// If the provided result buffers are too small for the entire result set, - /// they will be filled as far as possible from the start toward the end + /// If the provided result buffers are too small for the entire result set, + /// they will be filled as far as possible from the start toward the end /// position. /// - /// @param[in] startPos Path start position. [(x, y, z)] - /// @param[in] endPos Path end position. [(x, y, z)] - /// @param[in] path An array of polygon references that represent the path corridor. - /// @param[out] straightPath Points describing the straight path. [(x, y, z) * @p straightPathCount]. - /// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0] - /// @param[in] options Query options. (see: #dtStraightPathOptions) + /// Finds the straight path from the start to the end position within the polygon corridor. + /// @param[in] startPos Path start position. [(x, y, z)] + /// @param[in] endPos Path end position. [(x, y, z)] + /// @param[in] path An array of polygon references that represent the path corridor. + /// @param[in] pathSize The number of polygons in the @p path array. + /// @param[out] straightPath Points describing the straight path. [(x, y, z) * @p straightPathCount]. + /// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0] + /// @param[in] options Query options. (see: #dtStraightPathOptions) /// @returns The status flags for the query. - public virtual Result> FindStraightPath(RcVec3f startPos, RcVec3f endPos, List path, + public virtual DtStatus FindStraightPath(RcVec3f startPos, RcVec3f endPos, List path, + ref List straightPath, int maxStraightPath, int options) { - List straightPath = new List(); + straightPath.Clear(); + if (!RcVec3f.IsFinite(startPos) || !RcVec3f.IsFinite(endPos) || null == path || 0 == path.Count || path[0] == 0 || maxStraightPath <= 0) { - return Results.InvalidParam>(); + return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } // TODO: Should this be callers responsibility? var closestStartPosRes = ClosestPointOnPolyBoundary(path[0], startPos, out var closestStartPos); if (closestStartPosRes.Failed()) { - return Results.InvalidParam>("Cannot find start position"); + return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } var closestEndPosRes = ClosestPointOnPolyBoundary(path[path.Count - 1], endPos, out var closestEndPos); if (closestEndPosRes.Failed()) { - return Results.InvalidParam>("Cannot find end position"); + return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } // Add start point. - DtStatus stat = AppendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], straightPath, maxStraightPath); + DtStatus stat = AppendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], ref straightPath, maxStraightPath); if (!stat.InProgress()) { - return Results.Success(straightPath); + return stat; } if (path.Count > 1) @@ -1591,19 +1594,20 @@ namespace DotRecast.Detour var cpStatus = ClosestPointOnPolyBoundary(path[i], endPos, out closestEndPos); if (cpStatus.Failed()) { - return Results.InvalidParam>(); + return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM; } // Append portals along the current straight path segment. if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0) { // Ignore status return value as we're just about to return anyway. - AppendPortals(apexIndex, i, closestEndPos, path, straightPath, maxStraightPath, options); + AppendPortals(apexIndex, i, closestEndPos, path, ref straightPath, maxStraightPath, options); } // Ignore status return value as we're just about to return anyway. - AppendVertex(closestEndPos, 0, path[i], straightPath, maxStraightPath); - return Results.Success(straightPath); + AppendVertex(closestEndPos, 0, path[i], ref straightPath, maxStraightPath); + + return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM | (straightPath.Count >= maxStraightPath ? DtStatus.DT_BUFFER_TOO_SMALL : DtStatus.DT_STATUS_NOTHING); } // If starting really close the portal, advance. @@ -1639,11 +1643,10 @@ namespace DotRecast.Detour // Append portals along the current straight path segment. if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0) { - stat = AppendPortals(apexIndex, leftIndex, portalLeft, path, straightPath, maxStraightPath, - options); + stat = AppendPortals(apexIndex, leftIndex, portalLeft, path, ref straightPath, maxStraightPath, options); if (!stat.InProgress()) { - return Results.Success(straightPath); + return stat; } } @@ -1663,10 +1666,10 @@ namespace DotRecast.Detour long refs = leftPolyRef; // Append or update vertex - stat = AppendVertex(portalApex, flags, refs, straightPath, maxStraightPath); + stat = AppendVertex(portalApex, flags, refs, ref straightPath, maxStraightPath); if (!stat.InProgress()) { - return Results.Success(straightPath); + return stat; } portalLeft = portalApex; @@ -1696,11 +1699,10 @@ namespace DotRecast.Detour // Append portals along the current straight path segment. if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0) { - stat = AppendPortals(apexIndex, rightIndex, portalRight, path, straightPath, - maxStraightPath, options); + stat = AppendPortals(apexIndex, rightIndex, portalRight, path, ref straightPath, maxStraightPath, options); if (!stat.InProgress()) { - return Results.Success(straightPath); + return stat; } } @@ -1720,10 +1722,10 @@ namespace DotRecast.Detour long refs = rightPolyRef; // Append or update vertex - stat = AppendVertex(portalApex, flags, refs, straightPath, maxStraightPath); + stat = AppendVertex(portalApex, flags, refs, ref straightPath, maxStraightPath); if (!stat.InProgress()) { - return Results.Success(straightPath); + return stat; } portalLeft = portalApex; @@ -1742,18 +1744,17 @@ namespace DotRecast.Detour // Append portals along the current straight path segment. if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0) { - stat = AppendPortals(apexIndex, path.Count - 1, closestEndPos, path, straightPath, maxStraightPath, - options); + stat = AppendPortals(apexIndex, path.Count - 1, closestEndPos, path, ref straightPath, maxStraightPath, options); if (!stat.InProgress()) { - return Results.Success(straightPath); + return stat; } } } // Ignore status return value as we're just about to return anyway. - AppendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, straightPath, maxStraightPath); - return Results.Success(straightPath); + AppendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, ref straightPath, maxStraightPath); + return DtStatus.DT_SUCCSESS | (straightPath.Count >= maxStraightPath ? DtStatus.DT_BUFFER_TOO_SMALL : DtStatus.DT_STATUS_NOTHING); } /// @par diff --git a/src/DotRecast.Detour/DtStatus.cs b/src/DotRecast.Detour/DtStatus.cs index ae294bc..1081d92 100644 --- a/src/DotRecast.Detour/DtStatus.cs +++ b/src/DotRecast.Detour/DtStatus.cs @@ -31,6 +31,7 @@ namespace DotRecast.Detour // Detail information for status. public static readonly DtStatus DT_STATUS_DETAIL_MASK = new DtStatus(0x0ffffff); + public static readonly DtStatus DT_STATUS_NOTHING = new DtStatus(0); // nothing public static readonly DtStatus DT_WRONG_MAGIC = new DtStatus(1 << 0); // Input data is not recognized. public static readonly DtStatus DT_WRONG_VERSION = new DtStatus(1 << 1); // Input data is in wrong version. public static readonly DtStatus DT_OUT_OF_MEMORY = new DtStatus(1 << 2); // Operation ran out of memory. @@ -82,7 +83,7 @@ namespace DotRecast.Detour { return new DtStatus(left.Value | right.Value); } - + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static DtStatus operator &(DtStatus left, DtStatus right) { diff --git a/src/DotRecast.Detour/PathUtils.cs b/src/DotRecast.Detour/PathUtils.cs index 2229a08..43d8cff 100644 --- a/src/DotRecast.Detour/PathUtils.cs +++ b/src/DotRecast.Detour/PathUtils.cs @@ -34,13 +34,13 @@ namespace DotRecast.Detour float minTargetDist, List path) { // Find steer target. - Result> result = navQuery.FindStraightPath(startPos, endPos, path, MAX_STEER_POINTS, 0); + var straightPath = new List(); + var result = navQuery.FindStraightPath(startPos, endPos, path, ref straightPath, MAX_STEER_POINTS, 0); if (result.Failed()) { return null; } - List straightPath = result.result; float[] steerPoints = new float[straightPath.Count * 3]; for (int i = 0; i < straightPath.Count; i++) { diff --git a/src/DotRecast.Detour/StraightPathItem.cs b/src/DotRecast.Detour/StraightPathItem.cs index d719d93..4818b3d 100644 --- a/src/DotRecast.Detour/StraightPathItem.cs +++ b/src/DotRecast.Detour/StraightPathItem.cs @@ -22,7 +22,7 @@ using DotRecast.Core; namespace DotRecast.Detour { -//TODO: (PP) Add comments + //TODO: (PP) Add comments public class StraightPathItem { public RcVec3f pos; diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs index 24e7e3f..1f1aa2a 100644 --- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs @@ -63,7 +63,7 @@ public class TestNavmeshTool : IRcTool { return _impl; } - + public void OnSampleChanged() { // .. @@ -277,7 +277,7 @@ public class TestNavmeshTool : IRcTool // Reached off-mesh connection. RcVec3f startPos = RcVec3f.Zero; RcVec3f endPos = RcVec3f.Zero; - + // Advance the path up to and over the off-mesh connection. long prevRef = 0; long polyRef = polys[0]; @@ -345,8 +345,7 @@ public class TestNavmeshTool : IRcTool } } - m_straightPath = m_navQuery.FindStraightPath(m_spos, epos, m_polys, MAX_POLYS, - m_straightPathOptions).result; + m_navQuery.FindStraightPath(m_spos, epos, m_polys, ref m_straightPath, MAX_POLYS, m_straightPathOptions); } } else @@ -839,7 +838,7 @@ public class TestNavmeshTool : IRcTool var result = _impl.GetSample() .GetNavMeshQuery() .GetPolyWallSegments(m_polys[i], false, m_filter, ref segmentVerts, ref segmentRefs); - + if (result.Succeeded()) { dd.Begin(LINES, 2.0f); @@ -999,14 +998,7 @@ public class TestNavmeshTool : IRcTool } } - { - Result> result = m_navQuery.FindStraightPath(m_spos, epos, m_polys, - MAX_POLYS, DtNavMeshQuery.DT_STRAIGHTPATH_ALL_CROSSINGS); - if (result.Succeeded()) - { - m_straightPath = result.result; - } - } + m_navQuery.FindStraightPath(m_spos, epos, m_polys, ref m_straightPath, MAX_POLYS, DtNavMeshQuery.DT_STRAIGHTPATH_ALL_CROSSINGS); } m_pathFindStatus = DtStatus.DT_FAILURE; diff --git a/test/DotRecast.Detour.Crowd.Test/PathCorridorTest.cs b/test/DotRecast.Detour.Crowd.Test/PathCorridorTest.cs index 17dcedc..fef140a 100644 --- a/test/DotRecast.Detour.Crowd.Test/PathCorridorTest.cs +++ b/test/DotRecast.Detour.Crowd.Test/PathCorridorTest.cs @@ -44,16 +44,24 @@ public class PathCorridorTest straightPath.Add(new StraightPathItem(RcVec3f.Of(12, 20, 30.00002f), 0, 0)); straightPath.Add(new StraightPathItem(RcVec3f.Of(11f, 21, 32f), 0, 0)); straightPath.Add(new StraightPathItem(RcVec3f.Of(11f, 21, 32f), 0, 0)); - Result> result = Results.Success(straightPath); var mockQuery = new Mock(It.IsAny()); mockQuery.Setup(q => q.FindStraightPath( - It.IsAny(), - It.IsAny(), - It.IsAny>(), - It.IsAny(), - It.IsAny()) - ).Returns(result); - List path = corridor.FindCorners(int.MaxValue, mockQuery.Object, filter); + It.IsAny(), + It.IsAny(), + It.IsAny>(), + ref It.Ref>.IsAny, + It.IsAny(), + It.IsAny()) + ) + .Callback((RcVec3f startPos, RcVec3f endPos, List path, + ref List refStraightPath, int maxStraightPath, int options) => + { + refStraightPath = straightPath; + }) + .Returns(() => DtStatus.DT_SUCCSESS); + + var path = new List(); + corridor.FindCorners(ref path, int.MaxValue, mockQuery.Object, filter); Assert.That(path.Count, Is.EqualTo(4)); Assert.That(path, Is.EqualTo(straightPath)); } @@ -67,19 +75,25 @@ public class PathCorridorTest straightPath.Add(new StraightPathItem(RcVec3f.Of(11f, 21, 32f), 0, 0)); straightPath.Add(new StraightPathItem(RcVec3f.Of(12f, 22, 33f), DtNavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION, 0)); // offmesh straightPath.Add(new StraightPathItem(RcVec3f.Of(11f, 21, 32f), DtNavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION, 0)); // offmesh - Result> result = Results.Success(straightPath); var mockQuery = new Mock(It.IsAny()); - var s = mockQuery.Setup(q => q.FindStraightPath( - It.IsAny(), - It.IsAny(), - It.IsAny>(), - It.IsAny(), - It.IsAny()) - ).Returns(result); + mockQuery.Setup(q => q.FindStraightPath( + It.IsAny(), + It.IsAny(), + It.IsAny>(), + ref It.Ref>.IsAny, + It.IsAny(), + It.IsAny()) + ).Callback((RcVec3f startPos, RcVec3f endPos, List path, + ref List refStraightPath, int maxStraightPath, int options) => + { + refStraightPath = straightPath; + }) + .Returns(() => DtStatus.DT_SUCCSESS); - List path = corridor.FindCorners(int.MaxValue, mockQuery.Object, filter); + var path = new List(); + corridor.FindCorners(ref path, int.MaxValue, mockQuery.Object, filter); Assert.That(path.Count, Is.EqualTo(2)); Assert.That(path, Is.EqualTo(new List { straightPath[2], straightPath[3] })); } -} +} \ No newline at end of file diff --git a/test/DotRecast.Detour.Test/FindPathTest.cs b/test/DotRecast.Detour.Test/FindPathTest.cs index f79ac6b..7aca937 100644 --- a/test/DotRecast.Detour.Test/FindPathTest.cs +++ b/test/DotRecast.Detour.Test/FindPathTest.cs @@ -188,9 +188,8 @@ public class FindPathTest : AbstractDetourTest var startPos = startPoss[i]; var endPos = endPoss[i]; Result> path = query.FindPath(startRef, endRef, startPos, endPos, filter); - Result> result = query.FindStraightPath(startPos, endPos, path.result, - int.MaxValue, 0); - List straightPath = result.result; + var straightPath = new List(); + query.FindStraightPath(startPos, endPos, path.result, ref straightPath, int.MaxValue, 0); Assert.That(straightPath.Count, Is.EqualTo(STRAIGHT_PATHS[i].Length)); for (int j = 0; j < STRAIGHT_PATHS[i].Length; j++) { diff --git a/test/DotRecast.Detour.TileCache.Test/TileCacheFindPathTest.cs b/test/DotRecast.Detour.TileCache.Test/TileCacheFindPathTest.cs index c5c5626..4601d8b 100644 --- a/test/DotRecast.Detour.TileCache.Test/TileCacheFindPathTest.cs +++ b/test/DotRecast.Detour.TileCache.Test/TileCacheFindPathTest.cs @@ -55,7 +55,9 @@ public class TileCacheFindPathTest : AbstractTileCacheTest Result> path = query.FindPath(startRef, endRef, startPos, endPos, filter); int maxStraightPath = 256; int options = 0; - Result> pathStr = query.FindStraightPath(startPos, endPos, path.result, maxStraightPath, options); - Assert.That(pathStr.result.Count, Is.EqualTo(8)); + + var pathStr = new List(); + query.FindStraightPath(startPos, endPos, path.result, ref pathStr, maxStraightPath, options); + Assert.That(pathStr.Count, Is.EqualTo(8)); } } \ No newline at end of file