From 6b11e459b8df683b01ba99613b5e6d9578213ca3 Mon Sep 17 00:00:00 2001
From: ikpil <ikpil@naver.com>
Date: Tue, 22 Aug 2023 00:14:11 +0900
Subject: [PATCH] for unity3d

---
 .../Tools/TestNavmeshSampleTool.cs            | 37 +++---------------
 .../Tools/RcTestNavMeshTool.cs                | 38 ++++++++++++++++++-
 2 files changed, 41 insertions(+), 34 deletions(-)

diff --git a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs
index 4023828..133d22e 100644
--- a/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs
+++ b/src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs
@@ -472,10 +472,8 @@ public class TestNavmeshSampleTool : ISampleTool
                         col = spathCol;
                     }
 
-                    dd.Vertex(straightPathItem.pos.x, straightPathItem.pos.y + 0.4f,
-                        straightPathItem.pos.z, col);
-                    dd.Vertex(straightPathItem2.pos.x, straightPathItem2.pos.y + 0.4f,
-                        straightPathItem2.pos.z, col);
+                    dd.Vertex(straightPathItem.pos.x, straightPathItem.pos.y + 0.4f, straightPathItem.pos.z, col);
+                    dd.Vertex(straightPathItem2.pos.x, straightPathItem2.pos.y + 0.4f, straightPathItem2.pos.z, col);
                 }
 
                 dd.End();
@@ -501,8 +499,7 @@ public class TestNavmeshSampleTool : ISampleTool
                         col = spathCol;
                     }
 
-                    dd.Vertex(straightPathItem.pos.x, straightPathItem.pos.y + 0.4f,
-                        straightPathItem.pos.z, col);
+                    dd.Vertex(straightPathItem.pos.x, straightPathItem.pos.y + 0.4f, straightPathItem.pos.z, col);
                 }
 
                 dd.End();
@@ -793,34 +790,10 @@ public class TestNavmeshSampleTool : ISampleTool
         if (_option.mode == RcTestNavmeshToolMode.PATHFIND_SLICED)
         {
             DtNavMeshQuery navQuery = _sample.GetNavMeshQuery();
+            
             if (m_pathFindStatus.InProgress())
             {
-                m_pathFindStatus = navQuery.UpdateSlicedFindPath(1, out var _);
-            }
-
-            if (m_pathFindStatus.Succeeded())
-            {
-                navQuery.FinalizeSlicedFindPath(ref m_polys);
-                m_straightPath = null;
-                if (m_polys != null)
-                {
-                    // In case of partial path, make sure the end point is clamped to the last polygon.
-                    RcVec3f epos = new RcVec3f();
-                    epos = m_epos;
-                    if (m_polys[m_polys.Count - 1] != m_endRef)
-                    {
-                        var result = navQuery.ClosestPointOnPoly(m_polys[m_polys.Count - 1], m_epos, out var closest, out var _);
-                        if (result.Succeeded())
-                        {
-                            epos = closest;
-                        }
-                    }
-
-                    m_straightPath = new(MAX_POLYS);
-                    navQuery.FindStraightPath(m_spos, epos, m_polys, ref m_straightPath, MAX_POLYS, DtNavMeshQuery.DT_STRAIGHTPATH_ALL_CROSSINGS);
-                }
-
-                m_pathFindStatus = DtStatus.DT_FAILURE;
+                m_pathFindStatus = _tool.UpdateSlicedFindPath(navQuery, 1, m_endRef, m_spos, m_epos, ref m_polys, ref m_straightPath);
             }
         }
     }
diff --git a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
index a44e953..2a845ae 100644
--- a/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
+++ b/src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
@@ -174,14 +174,48 @@ namespace DotRecast.Recast.Toolset.Tools
             return DtStatus.DT_SUCCSESS;
         }
 
-        public DtStatus InitSlicedFindPath(DtNavMeshQuery navQuery, long startRef, long endRef, RcVec3f startPos, RcVec3f m_epos, IDtQueryFilter filter, bool enableRaycast)
+        public DtStatus InitSlicedFindPath(DtNavMeshQuery navQuery, long startRef, long endRef, RcVec3f startPos, RcVec3f endPos, IDtQueryFilter filter, bool enableRaycast)
         {
-            return navQuery.InitSlicedFindPath(startRef, endRef, startPos, m_epos, filter,
+            return navQuery.InitSlicedFindPath(startRef, endRef, startPos, endPos, filter,
                 enableRaycast ? DtNavMeshQuery.DT_FINDPATH_ANY_ANGLE : 0,
                 float.MaxValue
             );
         }
 
+        public DtStatus UpdateSlicedFindPath(DtNavMeshQuery navQuery, int maxIter, long endRef, RcVec3f startPos,  RcVec3f endPos,
+            ref List<long> path, ref List<StraightPathItem> straightPath)
+        {
+            var status = navQuery.UpdateSlicedFindPath(maxIter, out _);
+
+            if (!status.Succeeded())
+            {
+                return status;
+            }
+
+            navQuery.FinalizeSlicedFindPath(ref path);
+
+            straightPath?.Clear();
+            if (path != null)
+            {
+                // In case of partial path, make sure the end point is clamped to the last polygon.
+                RcVec3f epos = endPos;
+                if (path[path.Count - 1] != endRef)
+                {
+                    var result = navQuery.ClosestPointOnPoly(path[path.Count - 1], endPos, out var closest, out var _);
+                    if (result.Succeeded())
+                    {
+                        epos = closest;
+                    }
+                }
+
+                straightPath = new List<StraightPathItem>(MAX_POLYS);
+                navQuery.FindStraightPath(startPos, epos, path, ref straightPath, MAX_POLYS, DtNavMeshQuery.DT_STRAIGHTPATH_ALL_CROSSINGS);
+            }
+
+            return DtStatus.DT_SUCCSESS;
+        }
+
+
         public DtStatus Raycast(DtNavMeshQuery navQuery, long startRef, RcVec3f startPos, RcVec3f endPos, IDtQueryFilter filter,
             ref List<long> polys, ref List<StraightPathItem> straightPath, out RcVec3f hitPos, out RcVec3f hitNormal, out bool hitResult)
         {