From 8a5614eca40cbcf5d960e03c1c123c937544a29c Mon Sep 17 00:00:00 2001
From: ikpil <ikpil0@gmail.com>
Date: Sun, 18 Jun 2023 11:43:01 +0900
Subject: [PATCH] change ClosestPointOnPolyBoundary

---
 src/DotRecast.Detour.Crowd/DtPathCorridor.cs | 18 ++++----
 src/DotRecast.Detour/DtNavMeshQuery.cs       | 48 ++++++++------------
 2 files changed, 30 insertions(+), 36 deletions(-)

diff --git a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs
index 493d9d1..83edb8f 100644
--- a/src/DotRecast.Detour.Crowd/DtPathCorridor.cs
+++ b/src/DotRecast.Detour.Crowd/DtPathCorridor.cs
@@ -515,7 +515,7 @@ namespace DotRecast.Detour.Crowd
             }
         }
 
-        public void TrimInvalidPath(long safeRef, float[] safePos, DtNavMeshQuery navquery, IDtQueryFilter filter)
+        public bool TrimInvalidPath(long safeRef, float[] safePos, DtNavMeshQuery navquery, IDtQueryFilter filter)
         {
             // Keep valid path as far as possible.
             int n = 0;
@@ -524,7 +524,12 @@ namespace DotRecast.Detour.Crowd
                 n++;
             }
 
-            if (n == 0)
+            if (m_path.Count == n)
+            {
+                // All valid, no need to fix.
+                return true;
+            }
+            else if (n == 0)
             {
                 // The first polyref is bad, use current safe values.
                 m_pos.Set(safePos);
@@ -533,16 +538,13 @@ namespace DotRecast.Detour.Crowd
             }
             else if (n < m_path.Count)
             {
-                m_path = m_path.GetRange(0, n);
                 // The path is partially usable.
+                m_path = m_path.GetRange(0, n);
             }
 
             // Clamp target pos to last poly
-            var result = navquery.ClosestPointOnPolyBoundary(m_path[m_path.Count - 1], m_target);
-            if (result.Succeeded())
-            {
-                m_target = result.result;
-            }
+            navquery.ClosestPointOnPolyBoundary(m_path[m_path.Count - 1], m_target, out m_target);
+            return true;
         }
 
         /**
diff --git a/src/DotRecast.Detour/DtNavMeshQuery.cs b/src/DotRecast.Detour/DtNavMeshQuery.cs
index 7acfb92..fa0dd06 100644
--- a/src/DotRecast.Detour/DtNavMeshQuery.cs
+++ b/src/DotRecast.Detour/DtNavMeshQuery.cs
@@ -448,35 +448,31 @@ namespace DotRecast.Detour
         ///
         /// Much faster than ClosestPointOnPoly().
         ///
-        /// If the provided position lies within the polygon's xz-bounds (above or below),
+        /// If the provided position lies within the polygon's xz-bounds (above or below), 
         /// then @p pos and @p closest will be equal.
         ///
-        /// The height of @p closest will be the polygon boundary. The height detail is not used.
-        ///
+        /// The height of @p closest will be the polygon boundary.  The height detail is not used.
+        /// 
         /// @p pos does not have to be within the bounds of the polybon or the navigation mesh.
-        ///
-        /// Returns a point on the boundary closest to the source point if the source point is outside the
+        /// 
+        /// Returns a point on the boundary closest to the source point if the source point is outside the 
         /// polygon's xz-bounds.
-        /// @param[in] ref The reference id to the polygon.
-        /// @param[in] pos The position to check. [(x, y, z)]
-        /// @param[out] closest The closest point. [(x, y, z)]
+        ///  @param[in]		ref			The reference id to the polygon.
+        ///  @param[in]		pos			The position to check. [(x, y, z)]
+        ///  @param[out]	closest		The closest point. [(x, y, z)]
         /// @returns The status flags for the query.
-        public Result<RcVec3f> ClosestPointOnPolyBoundary(long refs, RcVec3f pos)
+        public DtStatus ClosestPointOnPolyBoundary(long refs, RcVec3f pos, out RcVec3f closest)
         {
+            closest = pos;
             var status = m_nav.GetTileAndPolyByRef(refs, out var tile, out var poly);
             if (status.Failed())
             {
-                return Results.Of<RcVec3f>(status, "");
+                return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
             }
 
-            if (tile == null)
+            if (tile == null || !RcVec3f.IsFinite(pos))
             {
-                return Results.InvalidParam<RcVec3f>("Invalid tile");
-            }
-
-            if (!RcVec3f.IsFinite(pos))
-            {
-                return Results.InvalidParam<RcVec3f>();
+                return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
             }
 
             // Collect vertices.
@@ -489,7 +485,6 @@ namespace DotRecast.Detour
                 Array.Copy(tile.data.verts, poly.verts[i] * 3, verts, i * 3, 3);
             }
 
-            RcVec3f closest;
             if (DetourCommon.DistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
             {
                 closest = pos;
@@ -513,7 +508,7 @@ namespace DotRecast.Detour
                 closest = RcVec3f.Lerp(verts, va, vb, edget[imin]);
             }
 
-            return Results.Success(closest);
+            return DtStatus.DT_SUCCSESS;
         }
 
         /// @par
@@ -1545,20 +1540,18 @@ namespace DotRecast.Detour
             }
 
             // TODO: Should this be callers responsibility?
-            Result<RcVec3f> closestStartPosRes = ClosestPointOnPolyBoundary(path[0], startPos);
+            var closestStartPosRes = ClosestPointOnPolyBoundary(path[0], startPos, out var closestStartPos);
             if (closestStartPosRes.Failed())
             {
                 return Results.InvalidParam<List<StraightPathItem>>("Cannot find start position");
             }
 
-            var closestStartPos = closestStartPosRes.result;
-            var closestEndPosRes = ClosestPointOnPolyBoundary(path[path.Count - 1], endPos);
+            var closestEndPosRes = ClosestPointOnPolyBoundary(path[path.Count - 1], endPos, out var closestEndPos);
             if (closestEndPosRes.Failed())
             {
                 return Results.InvalidParam<List<StraightPathItem>>("Cannot find end position");
             }
 
-            var closestEndPos = closestEndPosRes.result;
             // Add start point.
             DtStatus stat = AppendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], straightPath, maxStraightPath);
             if (!stat.InProgress())
@@ -1592,18 +1585,17 @@ namespace DotRecast.Detour
                         int fromType; // // fromType is ignored.
                         
                         // Next portal.
-                        var portalPoints = GetPortalPoints(path[i], path[i + 1], out left, out right, out fromType, out toType);
-                        if (portalPoints.Failed())
+                        var ppStatus = GetPortalPoints(path[i], path[i + 1], out left, out right, out fromType, out toType);
+                        if (ppStatus.Failed())
                         {
                             // Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
                             // Clamp the end point to path[i], and return the path so far.
-                            closestEndPosRes = ClosestPointOnPolyBoundary(path[i], endPos);
-                            if (closestEndPosRes.Failed())
+                            var cpStatus = ClosestPointOnPolyBoundary(path[i], endPos, out closestEndPos);
+                            if (cpStatus.Failed())
                             {
                                 return Results.InvalidParam<List<StraightPathItem>>();
                             }
 
-                            closestEndPos = closestEndPosRes.result;
                             // Append portals along the current straight path segment.
                             if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
                             {