diff --git a/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/CMoveAgent.cs b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/CMoveAgent.cs
index e4e37f05c1..20fa085849 100644
--- a/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/CMoveAgent.cs
+++ b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/CMoveAgent.cs
@@ -3,79 +3,59 @@ using System.Collections.Generic;
 using UnityEngine;
 
 // Filename : CMoveAgent.cs
-// Creator  : ported/simplified from C++ (AutoPFImp/AutoMoveImp/MoveAgent.*)
+// Creator  : ported from C++ (AutoPFImp/AutoMoveImp/MoveAgent.*)
 // Date     : 2026/01/09
 
 namespace AutoMove
 {
     /// <summary>
-    /// Minimal MoveAgent: loads movemap and provides A* path on layer0 rmap.
-    /// 最小 MoveAgent：加载 movemap，并在第0层 rmap 上执行 A* 路径搜索。
+    /// MoveAgent: A* on layer0 rmap + COptimizePath (matches C++ CMoveAgent flow).
     /// </summary>
     public class CMoveAgent
     {
-        // Debug switch (enable temporarily while validating routes).
-        // 调试开关（验证寻路时可临时开启）。
-        // NOTE: must not be const, otherwise Unity compiler warns about unreachable code.
-        // 注意：不要用 const，否则 Unity 编译会报“不可达代码”警告。
         private static bool DEBUG_AUTOPF = false;
+
         public abstract class BrushTest
         {
-            // from.y/to.y store DH in original engine.
-            // 原版中 from.y/to.y 存储 DH（相对地形高度差）。
             public abstract bool Collide(Vector3 from, Vector3 to);
         }
 
         private readonly CMoveMap m_pMoveMap = new CMoveMap();
-        private Vector3 m_vOriginOverride;
+        private COptimizePath m_pPathOptimizer;
 
         private Vector2Int m_ptStart;
         private Vector2Int m_ptGoal;
         private int m_iLayerStart;
         private int m_iLayerGoal;
 
-        private readonly List<Vector3> m_path3D = new List<Vector3>(1024);
-        private readonly List<Vector2Int> m_pathMap = new List<Vector2Int>(1024);
-        private int m_optimizeCurIndex;
-        private const int DefaultOptimizeCatchCount = 10;
-
         public bool Load(string basePathNoExt, Func<string, byte[]> resolver, Vector3? originOverride)
         {
-            // basePathNoExt corresponds to "maps\\<map>\\movemap\\r.._..-c.._..-l0" in C++.
-            // basePathNoExt 对应 C++ 的 "maps\\<map>\\movemap\\r.._..-c.._..-l0"。
             string cfgName = basePathNoExt;
             if (!cfgName.EndsWith(".cfg", StringComparison.OrdinalIgnoreCase))
-            {
                 cfgName += ".cfg";
-            }
 
             byte[] cfgBytes = resolver(cfgName);
             if (cfgBytes == null)
-            {
                 return false;
-            }
 
-            // Resolver for referenced files: in cfg, references are baseName + ".prmap" etc.
-            // cfg 内引用：baseName + ".prmap" 等。
             bool ok = m_pMoveMap.Load(cfgBytes, resolver, basePathNoExt);
             if (!ok)
-            {
                 return false;
-            }
 
             if (originOverride.HasValue)
-            {
-                m_vOriginOverride = originOverride.Value;
-                m_pMoveMap.SetOrigin(m_vOriginOverride);
-            }
+                m_pMoveMap.SetOrigin(originOverride.Value);
 
+            CreateOptimizer();
             return true;
         }
 
+        void CreateOptimizer()
+        {
+            m_pPathOptimizer = new COptimizePath();
+        }
+
         public bool IsReady()
         {
-            // Original C++ requires MultiCluGraph; here we only require layer0 RMap.
-            // 原版 C++ 依赖 MultiCluGraph；这里最小实现只要求第0层 RMap。
             var layer0 = m_pMoveMap.GetLayer(0);
             return layer0 != null && layer0.GetRMap() != null;
         }
@@ -101,8 +81,6 @@ namespace AutoMove
 
         public int WhichLayer(Vector3 vWld, float dH, out float layerDist)
         {
-            // Minimal: single layer 0 if passable.
-            // 最小实现：如果可通行则使用单层0。
             layerDist = 0.0f;
             var pt = TransWld2Map(vWld);
             var layer0 = m_pMoveMap.GetLayer(0);
@@ -119,71 +97,72 @@ namespace AutoMove
             return true;
         }
 
-        public int GetPathCount() => m_path3D.Count;
+        public List<PathNode> Get2DPath()
+        {
+            return m_pPathOptimizer?.GetPath();
+        }
 
-        /// <summary>
-        /// Matches C++ CMoveAgent::GetOptimizeCatchCount (COptimizePath::m_CatchCount, default 10).
-        /// </summary>
-        public int GetOptimizeCatchCount() => DefaultOptimizeCatchCount;
+        public int GetPathCount()
+        {
+            if (m_pPathOptimizer != null)
+            {
+                var path = m_pPathOptimizer.GetPath();
+                if (path != null)
+                    return path.Count;
+            }
+            return 0;
+        }
+
+        public int GetOptimizeCatchCount()
+        {
+            return m_pPathOptimizer != null ? m_pPathOptimizer.GetCatchCount() : 0;
+        }
 
         public Vector3 Get2DPathNode(int index)
         {
-            if (index < 0 || index >= m_path3D.Count) return Vector3.zero;
-            Vector3 v = m_path3D[index];
-            return new Vector3(v.x, 0f, v.z);
+            var path2d = Get2DPath();
+            if (path2d == null || index < 0 || index >= path2d.Count)
+                return Vector3.zero;
+            PathNode pathNode = path2d[index];
+            Vector2 ptWld = m_pMoveMap.TransMap2Wld(pathNode.ptMap.x, pathNode.ptMap.y);
+            return new Vector3(ptWld.x, 0f, ptWld.y);
         }
 
         public Vector3 Get3DPathNode(int index)
         {
-            if (index < 0 || index >= m_path3D.Count) return Vector3.zero;
-            return m_path3D[index];
+            var path2d = Get2DPath();
+            if (path2d == null || index < 0 || index >= path2d.Count)
+                return Vector3.zero;
+            PathNode pathNode = path2d[index];
+            Vector2 ptWld = m_pMoveMap.TransMap2Wld(pathNode.ptMap.x, pathNode.ptMap.y);
+            float dh = m_pMoveMap.GetDH(pathNode.layer, (int)pathNode.ptMap.x, (int)pathNode.ptMap.y);
+            return new Vector3(ptWld.x, dh, ptWld.y);
         }
 
-        /// <summary>Ported from C++ CMoveAgent::Optimize — incremental path smoothing while walking.</summary>
+        /// <summary>Ported from C++ CMoveAgent::Optimize.</summary>
         public bool Optimize(int moveIndex)
         {
-            if (moveIndex < 0 || moveIndex >= m_pathMap.Count - 1)
+            if (m_pPathOptimizer == null)
                 return false;
-
-            var layer0 = m_pMoveMap.GetLayer(0);
-            var rmap = layer0?.GetRMap();
-            if (rmap == null)
+            if (!m_pPathOptimizer.NeedOptimize(moveIndex))
                 return false;
-
-            int from = Mathf.Max(moveIndex, m_optimizeCurIndex);
-            if (from >= m_pathMap.Count - 1)
-                return false;
-
-            int best = from + 1;
-            for (int j = m_pathMap.Count - 1; j > from; j--)
-            {
-                if (IsLinePassable(rmap, m_pathMap[from], m_pathMap[j]))
-                {
-                    best = j;
-                    break;
-                }
-            }
-
-            if (best <= from + 1)
-                return false;
-
-            m_pathMap.RemoveRange(from + 1, best - from - 1);
-            m_path3D.RemoveRange(from + 1, best - from - 1);
-            m_optimizeCurIndex = from + 1;
+            m_pPathOptimizer.StepOptimize();
             return true;
         }
 
-        public System.Collections.Generic.List<Vector3> GetFullPath()
+        public List<Vector3> GetFullPath()
         {
-            return new System.Collections.Generic.List<Vector3>(m_path3D);
+            var path = new List<Vector3>();
+            var path2d = Get2DPath();
+            if (path2d == null)
+                return path;
+            for (int i = 0; i < path2d.Count; i++)
+                path.Add(Get2DPathNode(i));
+            return path;
         }
 
         public bool Search(int nMaxExpand = -1)
         {
-            // A* on layer0 rmap
-            // 在第0层 rmap 上执行 A*
-            m_path3D.Clear();
-
             var layer0 = m_pMoveMap.GetLayer(0);
             var rmap = layer0?.GetRMap();
             if (rmap == null) return false;
@@ -192,30 +171,18 @@ namespace AutoMove
             if (w <= 0 || h <= 0) return false;
 
             if (!InBounds(m_ptStart, w, h) || !InBounds(m_ptGoal, w, h))
-            {
                 return false;
-            }
 
-            // If goal or start is not passable, try to find nearest passable (original MoveAgent does this).
-            // 如果起点或终点不可通行，尝试寻找最近可通行点（原版 MoveAgent 会这样做）。
             if (!rmap.GetPixel(m_ptStart.x, m_ptStart.y))
             {
                 if (!TryFindNearestPassable(rmap, m_ptStart, w, h, 64, out var newStart))
-                {
-                    if (DEBUG_AUTOPF) Debug.LogWarning($"[CMoveAgent] start not passable and no nearest passable found. start={m_ptStart}");
                     return false;
-                }
-                if (DEBUG_AUTOPF) Debug.Log($"[CMoveAgent] Adjust start {m_ptStart} -> {newStart}");
                 m_ptStart = newStart;
             }
             if (!rmap.GetPixel(m_ptGoal.x, m_ptGoal.y))
             {
                 if (!TryFindNearestPassable(rmap, m_ptGoal, w, h, 64, out var newGoal))
-                {
-                    if (DEBUG_AUTOPF) Debug.LogWarning($"[CMoveAgent] goal not passable and no nearest passable found. goal={m_ptGoal}");
                     return false;
-                }
-                if (DEBUG_AUTOPF) Debug.Log($"[CMoveAgent] Adjust goal {m_ptGoal} -> {newGoal}");
                 m_ptGoal = newGoal;
             }
 
@@ -227,8 +194,6 @@ namespace AutoMove
             open.Push(m_ptStart, Heuristic(m_ptStart, m_ptGoal));
 
             int expands = 0;
-            //ToDo: need use another method to caculate the value of maxExpand
-            //800000 is a magic number, need to be optimized
             int maxExpand = nMaxExpand > 0 ? nMaxExpand : 800000;
 
             while (open.Count > 0 && expands < maxExpand)
@@ -271,16 +236,12 @@ namespace AutoMove
 
             if (rmap == null) return false;
 
-            // Check origin first
-            // 先检查原点
             if (origin.x >= 0 && origin.x < w && origin.y >= 0 && origin.y < h && rmap.GetPixel(origin.x, origin.y))
             {
                 best = origin;
                 return true;
             }
 
-            // Expand square rings
-            // 按方形“圈”扩展搜索
             for (int r = 1; r <= maxRadius; r++)
             {
                 int minX = Math.Max(0, origin.x - r);
@@ -288,14 +249,11 @@ namespace AutoMove
                 int minY = Math.Max(0, origin.y - r);
                 int maxY = Math.Min(h - 1, origin.y + r);
 
-                // Top/bottom edges
                 for (int x = minX; x <= maxX; x++)
                 {
                     TryConsider(x, minY);
                     TryConsider(x, maxY);
                 }
-
-                // Left/right edges (excluding corners already checked)
                 for (int y = minY + 1; y <= maxY - 1; y++)
                 {
                     TryConsider(minX, y);
@@ -328,123 +286,35 @@ namespace AutoMove
 
         private void ReconstructPath(Dictionary<Vector2Int, Vector2Int> cameFrom, Vector2Int cur)
         {
-            List<Vector2Int> rev = new List<Vector2Int>(1024) { cur };
+            var rev = new List<Vector2Int>(1024) { cur };
             while (cameFrom.TryGetValue(cur, out var prev))
             {
                 cur = prev;
                 rev.Add(cur);
             }
-
             rev.Reverse();
-            var layer0 = m_pMoveMap.GetLayer(0);
-            var rmap = layer0?.GetRMap();
-            List<Vector2Int> smoothed = rmap != null ? SmoothPathMap(rev, rmap) : rev;
 
-            m_pathMap.Clear();
-            m_path3D.Clear();
-            m_optimizeCurIndex = 0;
-            for (int i = 0; i < smoothed.Count; i++)
+            var initPath = new List<PathNode>(rev.Count);
+            for (int i = 0; i < rev.Count; i++)
             {
-                m_pathMap.Add(smoothed[i]);
-                m_path3D.Add(Map2Wld(smoothed[i]));
-            }
-        }
-
-        /// <summary>String-pull on passable grid (same goal as C++ COptimizePath::SetupOptimize).</summary>
-        static List<Vector2Int> SmoothPathMap(List<Vector2Int> raw, CBitImage rmap)
-        {
-            if (raw == null || raw.Count <= 2)
-                return raw;
-
-            var result = new List<Vector2Int>(raw.Count) { raw[0] };
-            int i = 0;
-            while (i < raw.Count - 1)
-            {
-                int furthest = i + 1;
-                for (int j = raw.Count - 1; j > i; j--)
+                initPath.Add(new PathNode
                 {
-                    if (IsLinePassable(rmap, raw[i], raw[j]))
-                    {
-                        furthest = j;
-                        break;
-                    }
-                }
-
-                if (furthest != i)
-                {
-                    if (result[result.Count - 1] != raw[furthest])
-                        result.Add(raw[furthest]);
-                    i = furthest;
-                }
-                else
-                {
-                    i++;
-                    if (result[result.Count - 1] != raw[i])
-                        result.Add(raw[i]);
-                }
+                    ptMap = new Vector2(rev[i].x, rev[i].y),
+                    layer = m_iLayerStart,
+                });
             }
 
-            return result;
-        }
+            if (m_pPathOptimizer == null)
+                CreateOptimizer();
 
-        /// <summary>Matches C++ COptimizePath::_LineTo strict diagonal checks on rmap.</summary>
-        static bool IsLinePassable(CBitImage rmap, Vector2Int from, Vector2Int to)
-        {
-            if (from == to)
-                return true;
-
-            rmap.GetImageSize(out int w, out int h);
-            int x0 = from.x, y0 = from.y;
-            int x1 = to.x, y1 = to.y;
-            int dx = Mathf.Abs(x1 - x0);
-            int dy = Mathf.Abs(y1 - y0);
-            int sx = x0 < x1 ? 1 : -1;
-            int sy = y0 < y1 ? 1 : -1;
-            int err = dx - dy;
-            int lastX = x0, lastY = y0;
-
-            while (true)
-            {
-                if (x0 < 0 || x0 >= w || y0 < 0 || y0 >= h || !rmap.GetPixel(x0, y0))
-                    return false;
-
-                if (x0 == x1 && y0 == y1)
-                    break;
-
-                int e2 = err << 1;
-                int nextX = x0, nextY = y0;
-                if (e2 > -dy)
-                {
-                    err -= dy;
-                    nextX += sx;
-                }
-                if (e2 < dx)
-                {
-                    err += dx;
-                    nextY += sy;
-                }
-
-                if (nextX != lastX && nextY != lastY)
-                {
-                    if (!rmap.GetPixel(lastX, nextY) || !rmap.GetPixel(nextX, lastY))
-                        return false;
-                }
-
-                x0 = nextX;
-                y0 = nextY;
-                lastX = x0;
-                lastY = y0;
-            }
-
-            return true;
+            // C++: m_pPathOptimizer->SetupOptimize(GetMoveMap(), path);
+            m_pPathOptimizer.SetupOptimize(m_pMoveMap, initPath);
         }
 
         private static bool InBounds(Vector2Int p, int w, int h) => p.x >= 0 && p.x < w && p.y >= 0 && p.y < h;
 
         private static int Heuristic(Vector2Int a, Vector2Int b)
         {
-            // Octile distance * 10
-            // 八方向启发式（octile）*10
             int dx = Mathf.Abs(a.x - b.x);
             int dy = Mathf.Abs(a.y - b.y);
             int min = Math.Min(dx, dy);
@@ -454,8 +324,6 @@ namespace AutoMove
 
         private static int Cost(Vector2Int a, Vector2Int b)
         {
-            // Diagonal=14, straight=10
-            // 斜向=14，直向=10
             int dx = Mathf.Abs(a.x - b.x);
             int dy = Mathf.Abs(a.y - b.y);
             return (dx + dy == 2) ? 14 : 10;
@@ -475,35 +343,13 @@ namespace AutoMove
 
         public void ResetSearch()
         {
-            //	Çå³ýµ±Ç°ËÑË÷״̬
-            //if (m_iStat == PF_STATE_UNKNOWN)
-            //{
-            //    return;
-            //}
-            //if (m_pPfAlg)
-            //{
-            //    m_pPfAlg->Reset();
-            //}
-            //if (m_pPathOptimizer)
-            //{
-            //    m_pPathOptimizer.Reset();
-            //}
-            m_ptStart.x = m_ptStart.y = 0;
+            m_ptStart = Vector2Int.zero;
             m_iLayerStart = -1;
-            m_ptGoal.x = m_ptGoal.y = 0;
+            m_ptGoal = Vector2Int.zero;
             m_iLayerGoal = -1;
-            //m_pBrushTest = null;
-
-            //m_iStat = PF_STATE_UNKNOWN;
-            m_path3D.Clear();
-            m_pathMap.Clear();
-            m_optimizeCurIndex = 0;
+            m_pPathOptimizer?.Reset();
         }
 
-        /// <summary>
-        /// Very small min-heap for A*.
-        /// A* 用的小型最小堆。
-        /// </summary>
         private sealed class MinHeap
         {
             private struct Node
@@ -559,5 +405,3 @@ namespace AutoMove
         }
     }
 }
-
-
diff --git a/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/CMoveMap.cs b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/CMoveMap.cs
index 70c4441a47..d4a8aa0139 100644
--- a/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/CMoveMap.cs
+++ b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/CMoveMap.cs
@@ -56,8 +56,13 @@ namespace AutoMove
 
         public Vector2 TransMap2Wld(int x, int y)
         {
-            float wx = m_vOrigin.x + (x + 0.5f) * m_fPixelSize;
-            float wz = m_vOrigin.z + (y + 0.5f) * m_fPixelSize;
+            return TransMap2Wld((float)x, (float)y);
+        }
+
+        public Vector2 TransMap2Wld(float mapX, float mapY)
+        {
+            float wx = m_vOrigin.x + (mapX + 0.5f) * m_fPixelSize;
+            float wz = m_vOrigin.z + (mapY + 0.5f) * m_fPixelSize;
             return new Vector2(wx, wz);
         }
 
diff --git a/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/COptimizePath.cs b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/COptimizePath.cs
new file mode 100644
index 0000000000..e75975f261
--- /dev/null
+++ b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/COptimizePath.cs
@@ -0,0 +1,298 @@
+using System.Collections.Generic;
+using UnityEngine;
+
+// Ported from C++ OptimizePath.h / OptimizePath.cpp (Kui Wu, 2007).
+
+namespace AutoMove
+{
+    /// <summary>Line sampler for map-space path optimization (C++ CLine).</summary>
+    sealed class CLine
+    {
+        Vector2 m_from;
+        Vector2 m_dir;
+        int m_count;
+
+        public void Init(Vector2 from, Vector2 dir)
+        {
+            m_from = from;
+            m_dir = dir;
+            float len = m_dir.magnitude;
+            if (len > 1e-8f)
+            {
+                m_dir.x /= len;
+                m_dir.y /= len;
+            }
+            m_count = 0;
+        }
+
+        public void Init(Vector2 from, int dirX, int dirY)
+        {
+            Init(from, new Vector2(dirX, dirY));
+        }
+
+        public Vector2 Next()
+        {
+            m_count++;
+            return new Vector2(m_from.x + m_dir.x * m_count, m_from.y + m_dir.y * m_count);
+        }
+
+        public int GetCount() => m_count;
+        public Vector2 GetFrom() => m_from;
+        public void Reset() => m_count = 0;
+    }
+
+    /// <summary>Path optimizer — port of C++ COptimizePath.</summary>
+    public sealed class COptimizePath
+    {
+        const int LookAhead = 60;
+        const int LookStep = 3;
+
+        CMoveMap m_pMoveMap;
+        int m_mapWidth;
+        int m_mapHeight;
+        readonly Dictionary<int, short> m_lookUp = new Dictionary<int, short>(4096);
+        readonly List<PathNode> m_path = new List<PathNode>(1024);
+        int m_curIndex = -1;
+        int m_catchCount = 10;
+        int m_curLayer = -1;
+
+        public void Reset()
+        {
+            m_pMoveMap = null;
+            m_mapWidth = 0;
+            m_mapHeight = 0;
+            m_lookUp.Clear();
+            m_path.Clear();
+            m_curIndex = -1;
+            m_curLayer = -1;
+        }
+
+        public int GetCatchCount() => m_catchCount;
+
+        public List<PathNode> GetPath() => m_path;
+
+        public bool NeedOptimize(int moveIndex)
+        {
+            if (m_curIndex < m_path.Count
+                && moveIndex < m_path.Count
+                && m_curIndex - moveIndex < m_catchCount)
+            {
+                if (moveIndex > m_curIndex)
+                {
+                    SetFootprintRange(m_curIndex, moveIndex - 1, 0);
+                    m_curIndex = moveIndex;
+                }
+                return true;
+            }
+            return false;
+        }
+
+        public void SetupOptimize(CMoveMap pMoveMap, List<PathNode> initPath, int catchCount = 10)
+        {
+            m_path.Clear();
+            if (initPath != null && initPath.Count > 0)
+                m_path.AddRange(initPath);
+
+            m_pMoveMap = pMoveMap;
+            m_mapWidth = pMoveMap != null ? pMoveMap.GetMapWidth() : 0;
+            m_mapHeight = pMoveMap != null ? pMoveMap.GetMapLength() : 0;
+            m_curIndex = 0;
+            m_catchCount = catchCount;
+            m_curLayer = -1;
+
+            if (m_path.Count == 0)
+                return;
+
+            CheckLayer();
+        }
+
+        public void StepOptimize()
+        {
+            CheckLayer();
+            int step = m_catchCount * 2;
+            if (step <= 0)
+                return;
+
+            int i = 0;
+            while (i < step && m_curIndex < m_path.Count)
+            {
+                LocalOptimize();
+                i++;
+                m_curIndex++;
+            }
+        }
+
+        void CheckLayer()
+        {
+            if (m_path.Count == 0 || m_curIndex < 0 || m_curIndex >= m_path.Count)
+                return;
+
+            if (m_path[m_curIndex].layer == m_curLayer)
+                return;
+
+            m_curLayer = m_path[m_curIndex].layer;
+            m_lookUp.Clear();
+
+            int i = m_curIndex;
+            while (i < m_path.Count && m_path[i].layer == m_curLayer)
+            {
+                SetFootprint(m_path[i].ptMap, (short)(i + 1));
+                i++;
+            }
+        }
+
+        void LocalOptimize()
+        {
+            int toIndex = Mathf.Min(m_curIndex + LookAhead, m_path.Count - 1);
+            var line = new CLine();
+            Vector2 dir;
+            int newCount = -1;
+
+            while (toIndex - m_curIndex > LookStep)
+            {
+                if (GetFootprint(m_path[toIndex].ptMap) == 0)
+                {
+                    toIndex -= LookStep;
+                    continue;
+                }
+                if (m_path[toIndex].layer != m_curLayer)
+                {
+                    toIndex -= LookStep;
+                    continue;
+                }
+
+                dir = m_path[toIndex].ptMap - m_path[m_curIndex].ptMap;
+                if ((int)dir.x == 0 && (int)dir.y == 0)
+                {
+                    PathIntersect(toIndex);
+                    return;
+                }
+
+                line.Init(m_path[m_curIndex].ptMap, dir);
+                if (LineTo(line, m_path[toIndex].ptMap))
+                {
+                    newCount = line.GetCount();
+                    break;
+                }
+                toIndex -= LookStep;
+            }
+
+            if (newCount > 0)
+            {
+                line.Reset();
+                AddPathPortion(line, toIndex - m_curIndex, newCount);
+            }
+        }
+
+        CBitImage GetRMap(int layer)
+        {
+            if (m_pMoveMap == null)
+                return null;
+            var layerMap = m_pMoveMap.GetLayer(layer);
+            return layerMap?.GetRMap();
+        }
+
+        bool LineTo(CLine line, Vector2 to)
+        {
+            int toX = (int)to.x;
+            int toY = (int)to.y;
+            int curX = (int)line.GetFrom().x;
+            int curY = (int)line.GetFrom().y;
+
+            CBitImage pRMap = GetRMap(m_curLayer);
+            if (pRMap == null)
+                return false;
+
+            int lastX = curX;
+            int lastY = curY;
+
+            while (curX != toX || curY != toY)
+            {
+                Vector2 cur = line.Next();
+                curX = (int)cur.x;
+                curY = (int)cur.y;
+
+                if (!pRMap.GetPixel(curX, curY))
+                    return false;
+
+                bool bNeighbor1 = pRMap.GetPixel(lastX, curY);
+                bool bNeighbor2 = pRMap.GetPixel(curX, lastY);
+
+                if (toX == lastX && toY == curY)
+                    break;
+                if (toX == curX && toY == lastY)
+                    break;
+
+                if (curX != lastX && curY != lastY && (!bNeighbor1 || !bNeighbor2))
+                    return false;
+
+                lastX = curX;
+                lastY = curY;
+            }
+
+            return true;
+        }
+
+        void PathIntersect(int toIndex)
+        {
+            SetFootprintRange(m_curIndex + 1, toIndex, 0);
+            int removeCount = toIndex - m_curIndex;
+            if (removeCount > 0 && m_curIndex + 1 < m_path.Count)
+                m_path.RemoveRange(m_curIndex + 1, removeCount);
+        }
+
+        void AddPathPortion(CLine line, int oldCount, int newCount)
+        {
+            SetFootprintRange(m_curIndex + 1, m_curIndex + oldCount, 0);
+
+            if (oldCount > newCount)
+            {
+                m_path.RemoveRange(m_curIndex + 1, oldCount - newCount);
+            }
+            else if (oldCount < newCount)
+            {
+                int insertCount = newCount - oldCount;
+                for (int k = 0; k < insertCount; k++)
+                    m_path.Insert(m_curIndex + 1, new PathNode());
+            }
+
+            int index = m_curIndex + 1;
+            while (line.GetCount() < newCount && index < m_path.Count)
+            {
+                var node = m_path[index];
+                node.ptMap = line.Next();
+                node.layer = m_curLayer;
+                m_path[index] = node;
+                index++;
+            }
+
+            SetFootprintRange(m_curIndex + 1, m_curIndex + newCount, 1);
+        }
+
+        int GetLookUpKey(int x, int y) => y * m_mapWidth + x;
+
+        int GetFootprint(int x, int y)
+        {
+            return m_lookUp.TryGetValue(GetLookUpKey(x, y), out short v) ? v : 0;
+        }
+
+        int GetFootprint(Vector2 pt) => GetFootprint((int)pt.x, (int)pt.y);
+
+        void SetFootprint(int x, int y, int val)
+        {
+            int key = GetLookUpKey(x, y);
+            if (val == 0)
+                m_lookUp.Remove(key);
+            else
+                m_lookUp[key] = (short)val;
+        }
+
+        void SetFootprint(Vector2 pt, int val) => SetFootprint((int)pt.x, (int)pt.y, val);
+
+        void SetFootprintRange(int fromIndex, int toIndex, int val)
+        {
+            for (int i = fromIndex; i <= toIndex && i < m_path.Count; ++i)
+                SetFootprint(m_path[i].ptMap, val);
+        }
+    }
+}
diff --git a/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/COptimizePath.cs.meta b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/COptimizePath.cs.meta
new file mode 100644
index 0000000000..86d0edecc0
--- /dev/null
+++ b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/COptimizePath.cs.meta
@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 96e45cded56a0bc4188f329f2cf3b676
\ No newline at end of file
diff --git a/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/PathNode.cs b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/PathNode.cs
new file mode 100644
index 0000000000..885435e2d2
--- /dev/null
+++ b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/PathNode.cs
@@ -0,0 +1,22 @@
+using UnityEngine;
+
+namespace AutoMove
+{
+    /// <summary>Matches C++ AutoMove::PathNode (PfConstant.h).</summary>
+    public struct PathNode
+    {
+        public Vector2 ptMap;
+        public int layer;
+
+        public Vector2Int GetPtI()
+        {
+            return new Vector2Int(Mathf.FloorToInt(ptMap.x), Mathf.FloorToInt(ptMap.y));
+        }
+
+        public void SetI(Vector2Int pt, int iLayer)
+        {
+            ptMap = new Vector2(pt.x, pt.y);
+            layer = iLayer;
+        }
+    }
+}
diff --git a/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/PathNode.cs.meta b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/PathNode.cs.meta
new file mode 100644
index 0000000000..0cf163da7d
--- /dev/null
+++ b/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/PathNode.cs.meta
@@ -0,0 +1,2 @@
+fileFormatVersion: 2
+guid: 98c1e0dc325b8e64bbda287a0128aeed
\ No newline at end of file
diff --git a/Assets/PerfectWorld/Scripts/Move/CECIntelligentRoute.cs b/Assets/PerfectWorld/Scripts/Move/CECIntelligentRoute.cs
index a9a0619740..a772cd9a43 100644
--- a/Assets/PerfectWorld/Scripts/Move/CECIntelligentRoute.cs
+++ b/Assets/PerfectWorld/Scripts/Move/CECIntelligentRoute.cs
@@ -19,7 +19,7 @@ namespace BrewMonster.Scripts
     /// </summary>
     public sealed class CECIntelligentRoute
     {
-        private const bool DEBUG_AUTOPF = true;
+        private const bool DEBUG_AUTOPF = false;
         /// <summary>cos(5°) for FindFarthestNode — same as C++ EC_IntelligentRoute.cpp.</summary>
         private static readonly float s_cos5Deg = Mathf.Cos(5f * Mathf.PI / 180f);
         public enum SearchResult
@@ -478,10 +478,8 @@ namespace BrewMonster.Scripts
             }
 
             m_iCurDest = FindNextNode(pos, m_iCurDest + 1);
+            m_dist2CurDest = (GetCurDest() - pos).MagnitudeH();
             agent.Optimize(m_iCurDest);
-            int newPathCount = agent.GetPathCount();
-            if (m_iCurDest >= newPathCount)
-                m_iCurDest = Mathf.Max(0, newPathCount - 1);
             m_dist2CurDest = (GetCurDest() - pos).MagnitudeH();
         }