test/Assets/PerfectWorld/Scripts/Move/AutoPFImp/AutoMove/COptimizePath.cs

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);
        }
    }
}