Fix route map smoothness

Assets/PerfectWorld/Scripts/Managers/EC_HPWorkMove.cs

@@ -875,11 +875,7 @@ namespace BrewMonster.Scripts
 
                     vCurPos = m_pHost.m_MoveCtrl.GroundMove(m_vCurDir, fSpeed, fDeltaTime, m_pHost.m_fVertSpeed);
                     if (!m_vCurDir.IsZero())
-                    {
-                        //m_pHost.StartModelMove(m_vCurDir, GPDataTypeHelper.g_vAxisY, 100);
-                        //m_pHost.ChangeModelTargetDirAndUp(m_vCurDir, GPDataTypeHelper.g_vAxisY);
-                        UpdateFacingFromDelta(vCurPos);
-                    }
+                        OrientHostHorizontal(m_vCurDir);
 
                     if (m_pHost.m_MoveCtrl.MoveBlocked() >= 3)
                     {

Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/CMoveAgent.cs

@@ -35,6 +35,9 @@ namespace AutoMove
         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)
         {
@@ -119,10 +122,16 @@ namespace AutoMove
         public int GetPathCount() => m_path3D.Count;
 
         /// <summary>
-        /// Matches C++ CMoveAgent::GetOptimizeCatchCount — lookahead window for path following.
-        /// 对应 C++：无 PathOptimizer 时为 0，FindNearest/Farthest 仅在单步索引上工作。
+        /// Matches C++ CMoveAgent::GetOptimizeCatchCount (COptimizePath::m_CatchCount, default 10).
         /// </summary>
-        public int GetOptimizeCatchCount() => 0;
+        public int GetOptimizeCatchCount() => DefaultOptimizeCatchCount;
+
+        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);
+        }
 
         public Vector3 Get3DPathNode(int index)
         {
@@ -130,6 +139,40 @@ namespace AutoMove
             return m_path3D[index];
         }
 
+        /// <summary>Ported from C++ CMoveAgent::Optimize — incremental path smoothing while walking.</summary>
+        public bool Optimize(int moveIndex)
+        {
+            if (moveIndex < 0 || moveIndex >= m_pathMap.Count - 1)
+                return false;
+
+            var layer0 = m_pMoveMap.GetLayer(0);
+            var rmap = layer0?.GetRMap();
+            if (rmap == null)
+                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;
+            return true;
+        }
+
         public System.Collections.Generic.List<Vector3> GetFullPath()
         {
             return new System.Collections.Generic.List<Vector3>(m_path3D);
@@ -293,14 +336,109 @@ namespace AutoMove
             }
 
             rev.Reverse();
-            // Convert to world positions (y will be resolved by host terrain in movement).
-            // 转为世界坐标（y 由移动逻辑/地形解析）。
-            for (int i = 0; i < rev.Count; i++)
+            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++)
             {
-                m_path3D.Add(Map2Wld(rev[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--)
+                {
+                    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]);
+                }
+            }
+
+            return result;
+        }
+
+        /// <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;
+        }
+
         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)
@@ -358,6 +496,8 @@ namespace AutoMove
 
             //m_iStat = PF_STATE_UNKNOWN;
             m_path3D.Clear();
+            m_pathMap.Clear();
+            m_optimizeCurIndex = 0;
         }
 
         /// <summary>

Assets/PerfectWorld/Scripts/Move/CECIntelligentRoute.cs

@@ -376,9 +376,22 @@ namespace BrewMonster.Scripts
 
             // Determine layers (minimal: layer0 or invalid).
             // 确定层（最小实现：层0或无效）。
+            float startDh = 0f;
+            float endDh = 0f;
+            var world = EC_Game.GetGameRun()?.GetWorld();
+            if (world != null)
+            {
+                A3DVECTOR3 n = GPDataTypeHelper.g_vAxisY;
+                if (world.TryGetTerrainHeight(start, ref n, out float startTer))
+                    startDh = start.y - startTer;
+                n = GPDataTypeHelper.g_vAxisY;
+                if (world.TryGetTerrainHeight(end, ref n, out float endTer))
+                    endDh = end.y - endTer;
+            }
+
             float dist;
-            int startLayer = agent.WhichLayer(new Vector3(start.x, start.y, start.z), 0.0f, out dist);
-            int endLayer = agent.WhichLayer(new Vector3(end.x, end.y, end.z), 0.0f, out dist);
+            int startLayer = agent.WhichLayer(new Vector3(start.x, start.y, start.z), startDh, out dist);
+            int endLayer = agent.WhichLayer(new Vector3(end.x, end.y, end.z), endDh, out dist);
             if (startLayer < 0) startLayer = 0;
             if (endLayer < 0) endLayer = 0;
 
@@ -465,8 +478,11 @@ namespace BrewMonster.Scripts
             }
 
             m_iCurDest = FindNextNode(pos, m_iCurDest + 1);
+            agent.Optimize(m_iCurDest);
+            int newPathCount = agent.GetPathCount();
+            if (m_iCurDest >= newPathCount)
+                m_iCurDest = Mathf.Max(0, newPathCount - 1);
             m_dist2CurDest = (GetCurDest() - pos).MagnitudeH();
-            // C++ calls agent->Optimize(m_iCurDest) here; Unity CMoveAgent has no path optimizer yet.
         }
 
         A3DVECTOR3 GetNodePos(int iNode)
@@ -489,17 +505,26 @@ namespace BrewMonster.Scripts
             if (agent == null)
                 return new A3DVECTOR3(0f, 0f, 0f);
             Vector3 v = agent.Get3DPathNode(iNode);
-            // C++ adds GetTerrainHeight to Y; Unity path nodes use movemap/world height from loader.
-            return new A3DVECTOR3(v.x, v.y, v.z);
+            var pos = new A3DVECTOR3(v.x, v.y, v.z);
+            var world = EC_Game.GetGameRun()?.GetWorld();
+            if (world != null)
+            {
+                A3DVECTOR3 n = GPDataTypeHelper.g_vAxisY;
+                if (world.TryGetTerrainHeight(pos, ref n, out float terY))
+                    pos.y = terY;
+            }
+            return pos;
         }
 
         A3DVECTOR3 GetNodePosXZ(int iNode)
         {
             if (IsIdle())
                 return new A3DVECTOR3(0f, 0f, 0f);
-            A3DVECTOR3 p = GetNodePosNoCheck(iNode);
-            p.y = 0f;
-            return p;
+            RangedMoveAgent? pCurAgent = GetCurAgent();
+            if (pCurAgent == null || pCurAgent.Value.agent == null)
+                return new A3DVECTOR3(0f, 0f, 0f);
+            Vector3 v = pCurAgent.Value.agent.Get2DPathNode(iNode);
+            return new A3DVECTOR3(v.x, v.y, v.z);
         }
 
         int FindNearestNode(A3DVECTOR3 curPos, int iNodeFrom)

Assets/PerfectWorld/Scripts/World/CECWorld.cs

@@ -32,9 +32,25 @@ namespace BrewMonster.Scripts.World
             return m_pOnmtMan;
         }
         public float GetTerrainHeight(A3DVECTOR3 vPos, ref A3DVECTOR3 pvNormal /* NULL */)
+        {
+            A3DVECTOR3 normal = pvNormal;
+            if (!TryGetTerrainHeight(vPos, ref normal, out float height))
+                return vPos.y;
+            pvNormal = normal;
+            return height;
+        }
+
+        /// <summary>False when terrain is not loaded yet (e.g. during early AutoPF Search).</summary>
+        public bool TryGetTerrainHeight(A3DVECTOR3 vPos, ref A3DVECTOR3 pvNormal, out float height)
         {
             A3DTerrain2 pTerrain = GetTerrain();
-            return pTerrain.GetPosHeight(vPos, ref pvNormal);
+            if (pTerrain == null)
+            {
+                height = vPos.y;
+                return false;
+            }
+            height = pTerrain.GetPosHeight(vPos, ref pvNormal);
+            return true;
         }
 
         public float GetWaterHeight(A3DVECTOR3 vPos)