WSHCommon/Common/CommonAI/RTS/Manhattan/RTSManhattan.cs

using System;
using System.Collections.Generic;
using System.Text;

using CommonAI.RTS;
using CommonLang.Vector;
using CommonLang;
using CommonLang.Concurrent;
using CommonAI.ZoneServer.JSGModule;

namespace CommonAI.RTS.Manhattan
{
    /// <summary>
    /// 实现了曼哈顿算法的A*寻路算法
    /// </summary>
    public partial class AstarManhattan : RTSAstar<AstarManhattan.MMapNode>
    {
        #region _Fields_
        private MTerrain terrain_map;
        private IManhattanMap manhattan_map;
        private MSpaceAStar space_div;
        private readonly float space_sqrt;

        private readonly float cellw;
        private readonly float cellh;
        private readonly float cell_sqrt;

        private readonly float totalw;
        private readonly float totalh;
        private int area_count = 0;

        private readonly int div_size;
        private int space_optmize_limit_distance = 4;
        private int space_div_enable_distance = 4;

        private DirtyNodes<MMapNode> dirty_nodes = new DirtyNodes<MMapNode>();


        #endregion

        //---------------------------------------------------------------------------------------------------------------------
        /// <summary>
        /// 创建一个以曼哈顿算法的A*寻路算法
		/// </summary>
		/// <param name="map_data">地图适配器</param>
		/// <param name="inclined">是否要检测斜边可以移动</param>
		/// <param name="space_size">空间切割尺寸</param>
		public AstarManhattan(int uniqueID, IManhattanMap map_data, bool inclined = true, int space_size = 0)
        {
			//21.3, 7.3, 5.7, 3
            this.manhattan_map = map_data;
            this.cellw = map_data.CellW;
            this.cellh = map_data.CellH;
            this.totalw = cellw * map_data.XCount;
            this.totalh = cellh * map_data.YCount;
            if (totalw < ushort.MinValue || totalw > ushort.MaxValue || totalh < ushort.MinValue || totalh > ushort.MaxValue)
            {
                throw new Exception(string.Format("Terrain Size must in ({0}~{1}", ushort.MinValue, ushort.MaxValue));
            }
            this.div_size = space_size;
            this.cell_sqrt = (float)Math.Sqrt((cellw * cellw) + (cellh * cellh));
            this.space_sqrt = (float)Math.Sqrt((div_size * div_size) + (div_size * div_size));
            this.terrain_map = new MTerrain(this, map_data, inclined);
            base.Init(terrain_map);
            this.ResetArea();
			if (space_size > 0)
            {
				//this.space_div = MSpaceNodeCache.GetCacheAStart(uniqueID, this, space_size); 
				this.space_div = new MSpaceAStar(uniqueID, this, space_size);
			}
        }

		protected override void Disposing()
        {
            base.Disposing();
            if (space_div != null)
            {
                space_div.Dispose();
                space_div = null;
            }
            this.dirty_nodes.Clear();
            this.manhattan_map = null;
            this.terrain_map = null;
        }
        
        //---------------------------------------------------------------------------------------------------------------------
        #region _Properties_

        public IManhattanMap MMap { get { return manhattan_map; } }

        /// <summary>
        /// 超过多少格子（空间分割格子）不进行路径优化
        /// </summary>
        public int SpaceOptimizePathLimitDistance
        {
            get { return space_optmize_limit_distance; }
            set { if (value > 1) { space_optmize_limit_distance = value; } }
        }
        /// <summary>
        /// 超过多少格子（空间分割格子）启用空间分割
        /// </summary>
        public int SpaceDivEnableDistance
        {
            get { return space_div_enable_distance; }
            set { if (value > 1) { space_div_enable_distance = value; } }
        }

        /// <summary>
        /// 空间分割尺度
        /// </summary>
        public int SpaceDivSize
        {
            get { return div_size; }
        }
        /// <summary>
        /// 空间分割寻路
        /// </summary>
        public MSpaceAStar SpaceAstar
        {
            get { return space_div; }
        }
        /// <summary>
        /// 是否支持8方向
        /// </summary>
        public bool IsInclined
        {
            get { return terrain_map.is_inclined; }
        }
        /// <summary>
        /// 地图总共多少可行走区域
        /// </summary>
        public int AreaCount
        {
            get { return area_count; }
        }

        #endregion
        //---------------------------------------------------------------------------------------------------------------------
        #region _ForEachTerrain_

        public static void NormalizeTerrainRegionByBlock(IManhattanMap tmap, ref int min_x, ref int min_y, ref int max_x, ref int max_y)
        {
            min_x = Math.Max(min_x, 0);
            min_y = Math.Max(min_y, 0);
            max_x = Math.Min(max_x, tmap.XCount - 1);
            max_y = Math.Min(max_y, tmap.YCount - 1);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="bx"></param>
        /// <param name="by"></param>
        public delegate void ForEachTerrainAction(int bx, int by);

        public static void ForEachTerrainPoint(IManhattanMap tmap, float sx, float sy, ForEachTerrainAction action)
        {
            int cx1 = (int)((sx) / tmap.CellW);
            int cy1 = (int)((sy) / tmap.CellH);
            if (cx1 >= 0 && cx1 < tmap.XCount && cy1 >= 0 && cy1 < tmap.YCount)
            {
                action(cx1, cy1);
            }
        }
        public static void ForEachTerrainRect(IManhattanMap tmap, float sx, float sy, float w, float h, ForEachTerrainAction action)
        {
            int cx1 = (int)((sx) / tmap.CellW);
            int cy1 = (int)((sy) / tmap.CellH);
            int cx2 = (int)((sx + w) / tmap.CellW);
            int cy2 = (int)((sy + h) / tmap.CellH);
            cx1 = Math.Max(cx1, 0);
            cy1 = Math.Max(cy1, 0);
            cx2 = Math.Min(cx2, tmap.XCount - 1);
            cy2 = Math.Min(cy2, tmap.YCount - 1);
            for (int cx = cx1; cx <= cx2; ++cx)
            {
                for (int cy = cy1; cy <= cy2; ++cy)
                {
                    action(cx, cy);
                }
            }
        }
        public static void ForEachTerrainRound(IManhattanMap tmap, float sx, float sy, float r, ForEachTerrainAction action)
        {
            int cx1 = (int)((sx - r) / tmap.CellW);
            int cy1 = (int)((sy - r) / tmap.CellH);
            int cx2 = (int)((sx + r) / tmap.CellW);
            int cy2 = (int)((sy + r) / tmap.CellH);
            cx1 = Math.Max(cx1, 0);
            cy1 = Math.Max(cy1, 0);
            cx2 = Math.Min(cx2, tmap.XCount - 1);
            cy2 = Math.Min(cy2, tmap.YCount - 1);
            for (int cx = cx1; cx <= cx2; ++cx)
            {
                for (int cy = cy1; cy <= cy2; ++cy)
                {
                    if (CMath.intersectRectRound(
                        cx * tmap.CellW,
                        cy * tmap.CellH,
                        cx * tmap.CellW + tmap.CellW,
                        cy * tmap.CellH + tmap.CellH,
                        sx, sy, r))
                    {
                        action(cx, cy);
                    }
                }
            }
        }
        public static void ForEachTerrainEllipse(IManhattanMap tmap, float sx, float sy, float w, float h, ForEachTerrainAction action)
        {
            float scrw = w / 2;
            float scrh = h / 2;
            float scx = sx + w / 2;
            float scy = sy + h / 2;
            int cx1 = (int)((sx) / tmap.CellW);
            int cy1 = (int)((sy) / tmap.CellH);
            int cx2 = (int)((sx + w) / tmap.CellW);
            int cy2 = (int)((sy + h) / tmap.CellH);
            cx1 = Math.Max(cx1, 0);
            cy1 = Math.Max(cy1, 0);
            cx2 = Math.Min(cx2, tmap.XCount - 1);
            cy2 = Math.Min(cy2, tmap.YCount - 1);
            float cx0;
            float cy0;
            for (int cx = cx1; cx <= cx2; ++cx)
            {
                for (int cy = cy1; cy <= cy2; ++cy)
                {
                    cx0 = cx * tmap.CellW;
                    cy0 = cy * tmap.CellH;
                    if (CMath.intersectRectEllipse(cx0, cy0, cx0 + tmap.CellW, cy0 + tmap.CellH, scx, scy, scrw, scrh))
                    {
                        action(cx, cy);
                    }
                }
            }
        }
        public static void ForEachTerrainLine(IManhattanMap tmap, float x0, float y0, float x1, float y1, ForEachTerrainAction action)
        {
            int bx0 = (int)(x0 / tmap.CellW);
            int by0 = (int)(y0 / tmap.CellH);
            int bx1 = (int)(x1 / tmap.CellW);
            int by1 = (int)(y1 / tmap.CellH);
            if (bx0 > bx1) CUtils.Swap(ref bx0, ref bx1);
            if (by0 > by1) CUtils.Swap(ref by0, ref by1);
            if (bx0 < 0) bx0 = 0;
            if (by0 < 0) by0 = 0;
            if (bx1 >= tmap.XCount) bx1 = tmap.XCount - 1;
            if (by1 >= tmap.YCount) by1 = tmap.YCount - 1;
            float cx0;
            float cy0;
            for (int by = by0; by <= by1; by++)
            {
                for (int bx = bx0; bx <= bx1; bx++)
                {
                    cx0 = bx * tmap.CellW;
                    cy0 = by * tmap.CellH;
                    if (CMath.intersectRectLine(cx0, cy0, cx0 + tmap.CellW, cy0 + tmap.CellH, x0, y0, x1, y1))
                    {
                        action(bx, by);
                    }
                }
            }
        }
        public static void ForEachTerrainStripWidth(IManhattanMap tmap, float x0, float y0, float x1, float y1, float line_r, ForEachTerrainAction action)
        {
            CommonLang.Geometry.Vector2[] points = CMath.toStripWidthPolygon(x0, y0, x1, y1, line_r);
            CommonLang.Geometry.Vector2 min, max;
            CMath.toBoundingBox(points, out min, out max);
            int bx0 = (int)(min.X / tmap.CellW);
            int by0 = (int)(min.Y / tmap.CellH);
            int bx1 = (int)(max.X / tmap.CellW);
            int by1 = (int)(max.Y / tmap.CellH);
            if (bx0 < 0) bx0 = 0;
            if (by0 < 0) by0 = 0;
            if (bx1 >= tmap.XCount) bx1 = tmap.XCount - 1;
            if (by1 >= tmap.YCount) by1 = tmap.YCount - 1;
            float cx0;
            float cy0;
            for (int by = by0; by <= by1; by++)
            {
                for (int bx = bx0; bx <= bx1; bx++)
                {
                    cx0 = bx * tmap.CellW;
                    cy0 = by * tmap.CellH;
                    if (CMath.intersectRectPolygon(cx0, cy0, cx0 + tmap.CellW, cy0 + tmap.CellH, points))
                    {
                        action(bx, by);
                    }
                }
            }
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="bx"></param>
        /// <param name="by"></param>
        /// <returns>True can break</returns>
        public delegate bool ForEachTerrainPredicate(int bx, int by);

        public static bool ForEachTerrainPoint(IManhattanMap tmap, float sx, float sy, ForEachTerrainPredicate action)
        {
            int cx1 = (int)((sx) / tmap.CellW);
            int cy1 = (int)((sy) / tmap.CellH);
            if (cx1 >= 0 && cx1 < tmap.XCount && cy1 >= 0 && cy1 < tmap.YCount)
            {
                if (action(cx1, cy1))
                {
                    return true;
                }
            }
            return false;
        }
        public static bool ForEachTerrainRect(IManhattanMap tmap, float sx, float sy, float w, float h, ForEachTerrainPredicate action)
        {
            int cx1 = (int)((sx) / tmap.CellW);
            int cy1 = (int)((sy) / tmap.CellH);
            int cx2 = (int)((sx + w) / tmap.CellW);
            int cy2 = (int)((sy + h) / tmap.CellH);
            cx1 = Math.Max(cx1, 0);
            cy1 = Math.Max(cy1, 0);
            cx2 = Math.Min(cx2, tmap.XCount - 1);
            cy2 = Math.Min(cy2, tmap.YCount - 1);
            for (int cx = cx1; cx <= cx2; ++cx)
            {
                for (int cy = cy1; cy <= cy2; ++cy)
                {
                    if (action(cx, cy))
                    {
                        return true;
                    }
                }
            }
            return false;
        }
        public static bool ForEachTerrainRound(IManhattanMap tmap, float sx, float sy, float r, ForEachTerrainPredicate action)
        {
            int cx1 = (int)((sx - r) / tmap.CellW);
            int cy1 = (int)((sy - r) / tmap.CellH);
            int cx2 = (int)((sx + r) / tmap.CellW);
            int cy2 = (int)((sy + r) / tmap.CellH);
            cx1 = Math.Max(cx1, 0);
            cy1 = Math.Max(cy1, 0);
            cx2 = Math.Min(cx2, tmap.XCount - 1);
            cy2 = Math.Min(cy2, tmap.YCount - 1);
            for (int cx = cx1; cx <= cx2; ++cx)
            {
                for (int cy = cy1; cy <= cy2; ++cy)
                {
                    if (CMath.intersectRectRound(
                        cx * tmap.CellW,
                        cy * tmap.CellH,
                        cx * tmap.CellW + tmap.CellW,
                        cy * tmap.CellH + tmap.CellH,
                        sx, sy, r))
                    {
                        if (action(cx, cy))
                        {
                            return true;
                        }
                    }
                }
            }
            return false;
        }
        public static bool ForEachTerrainEllipse(IManhattanMap tmap, float sx, float sy, float w, float h, ForEachTerrainPredicate action)
        {
            float scrw = w / 2;
            float scrh = h / 2;
            float scx = sx + w / 2;
            float scy = sy + h / 2;
            int cx1 = (int)((sx) / tmap.CellW);
            int cy1 = (int)((sy) / tmap.CellH);
            int cx2 = (int)((sx + w) / tmap.CellW);
            int cy2 = (int)((sy + h) / tmap.CellH);
            cx1 = Math.Max(cx1, 0);
            cy1 = Math.Max(cy1, 0);
            cx2 = Math.Min(cx2, tmap.XCount - 1);
            cy2 = Math.Min(cy2, tmap.YCount - 1);
            float cx0;
            float cy0;
            for (int cx = cx1; cx <= cx2; ++cx)
            {
                for (int cy = cy1; cy <= cy2; ++cy)
                {
                    cx0 = cx * tmap.CellW;
                    cy0 = cy * tmap.CellH;
                    if (CMath.intersectRectEllipse(cx0, cy0, cx0 + tmap.CellW, cy0 + tmap.CellH, scx, scy, scrw, scrh))
                    {
                        if (action(cx, cy))
                        {
                            return true;
                        }
                    }
                }
            }
            return false;
        }
        public static bool ForEachTerrainLine(IManhattanMap tmap, float x0, float y0, float x1, float y1, ForEachTerrainPredicate action)
        {
            int bx0 = (int)(x0 / tmap.CellW);
            int by0 = (int)(y0 / tmap.CellH);
            int bx1 = (int)(x1 / tmap.CellW);
            int by1 = (int)(y1 / tmap.CellH);
            if (bx0 > bx1) CUtils.Swap(ref bx0, ref bx1);
            if (by0 > by1) CUtils.Swap(ref by0, ref by1);
            if (bx0 < 0) bx0 = 0;
            if (by0 < 0) by0 = 0;
            if (bx1 >= tmap.XCount) bx1 = tmap.XCount - 1;
            if (by1 >= tmap.YCount) by1 = tmap.YCount - 1;
            float cx0;
            float cy0;
            for (int by = by0; by <= by1; by++)
            {
                for (int bx = bx0; bx <= bx1; bx++)
                {
                    cx0 = bx * tmap.CellW;
                    cy0 = by * tmap.CellH;
                    if (CMath.intersectRectLine(cx0, cy0, cx0 + tmap.CellW, cy0 + tmap.CellH, x0, y0, x1, y1))
                    {
                        if (action(bx, by))
                        {
                            return true;
                        }
                    }
                }
            }
            return false;
        }
        public static bool ForEachTerrainStripWidth(IManhattanMap tmap, float x0, float y0, float x1, float y1, float line_r, ForEachTerrainPredicate action)
        {
            CommonLang.Geometry.Vector2[] points = CMath.toStripWidthPolygon(x0, y0, x1, y1, line_r);
            CommonLang.Geometry.Vector2 min, max;
            CMath.toBoundingBox(points, out min, out max);
            int bx0 = (int)(min.X / tmap.CellW);
            int by0 = (int)(min.Y / tmap.CellH);
            int bx1 = (int)(max.X / tmap.CellW);
            int by1 = (int)(max.Y / tmap.CellH);
            if (bx0 < 0) bx0 = 0;
            if (by0 < 0) by0 = 0;
            if (bx1 >= tmap.XCount) bx1 = tmap.XCount - 1;
            if (by1 >= tmap.YCount) by1 = tmap.YCount - 1;
            float cx0;
            float cy0;
            for (int by = by0; by <= by1; by++)
            {
                for (int bx = bx0; bx <= bx1; bx++)
                {
                    cx0 = bx * tmap.CellW;
                    cy0 = by * tmap.CellH;
                    if (CMath.intersectRectPolygon(cx0, cy0, cx0 + tmap.CellW, cy0 + tmap.CellH, points))
                    {
                        if (action(bx, by))
                        {
                            return true;
                        }
                    }
                }
            }
            return false;
        }

        #endregion
        //---------------------------------------------------------------------------------------------------------------------
        #region _FillTerrain_

        public bool TryFillTerrain(int bx, int by, int value)
        {
            if (manhattan_map.SetValue(bx, by, value))
            {
                var mnode = terrain_map.getMapNode(bx, by);
                if (mnode != null && mnode.resetValue(this.terrain_map))
                {
                    dirty_nodes.AddDirty(mnode);
                    return true;
                }
            }
            return false;
        }

        //---------------------------------------------------------------------------------------------------------------------

        public void FillTerrain(float sx, float sy, int value)
        {
            ForEachTerrainPoint(manhattan_map, sx, sy, (bx, by) =>
            {
                TryFillTerrain(bx, by, value);
            });
            this.BeginFindPath();
        }
        public void FillTerrain(float sx, float sy, float w, float h, int value)
        {
            ForEachTerrainRect(manhattan_map, sx, sy, w, h, (bx, by) =>
            {
                TryFillTerrain(bx, by, value);
            });
            this.BeginFindPath();
        }
        public void FillTerrain(float sx, float sy, float w, float h, int[,] matrix)
        {
            ForEachTerrainRect(manhattan_map, sx, sy, w, h, (bx, by) =>
            {
                TryFillTerrain(bx, by, matrix[bx, by]);
            });
            this.BeginFindPath();
        }

        //---------------------------------------------------------------------------------------------------------------------

        public void FillTerrainByBlock(int bx, int by, int value)
        {
            if (bx >= 0 && bx < manhattan_map.XCount && by >= 0 && by < manhattan_map.YCount)
            {
                TryFillTerrain(bx, by, value);
            }
            this.BeginFindPath();
        }
        public void FillTerrainByBlock(int bx, int by, int bw, int bh, int value)
        {
            int cx1 = Math.Max(bx, 0);
            int cy1 = Math.Max(by, 0);
            int cx2 = Math.Min(bx + bw, manhattan_map.XCount);
            int cy2 = Math.Min(by + bh, manhattan_map.YCount);
            for (int cx = cx1; cx < cx2; ++cx)
            {
                for (int cy = cy1; cy < cy2; ++cy)
                {
                    TryFillTerrain(cx, cy, value);
                }
            }
            this.BeginFindPath();
        }
        public void FillTerrainByBlock(int bx, int by, int bw, int bh, int[,] matrix)
        {
            int cx1 = Math.Max(bx, 0);
            int cy1 = Math.Max(by, 0);
            int cx2 = Math.Min(bx + bw, manhattan_map.XCount);
            int cy2 = Math.Min(by + bh, manhattan_map.YCount);
            for (int cx = cx1; cx < cx2; ++cx)
            {
                for (int cy = cy1; cy < cy2; ++cy)
                {
                    TryFillTerrain(cx, cy, matrix[cx, cy]);
                }
            }
            this.BeginFindPath();
        }

        //---------------------------------------------------------------------------------------------------------------------

        public void BeginFindPath()
        {
            if (dirty_nodes.Count > 0)
            {
                using (var list = ListObjectPool<MMapNode>.AllocAutoRelease(dirty_nodes.Values))
                {
                    var near_table = GetNearTables(IsInclined);
                    foreach (var dn in list)
                    {
                        foreach (var offset in near_table)
                        {
                            var sn = GetMapNode(dn.BX + offset[0], dn.BY + offset[1]);
                            if (sn != null)
                            {
                                dirty_nodes.AddDirty(sn);
                            }
                        }
                    }
                }
                using (var list = ListObjectPool<MMapNode>.AllocAutoRelease(dirty_nodes.Values))
                {
                    foreach (var mn in list)
                    {
                        mn.resetNextNodes(this.terrain_map);
                    }
                }
                dirty_nodes.Clear();
                this.ResetArea();
            }
            if (space_div != null)
            {
                this.space_div.beginFindPath();
            }
        }

        /// <summary>
        /// 计算连续区域
        /// </summary>
        private void ResetArea()
        {
            byte area_index = 0;
            HashMap<MMapNode, bool> dirty_map = new HashMap<MMapNode, bool>(terrain_map.TotalNodeCount);
            for (int by = 0; by < terrain_map.ycount; by++)
            {
                for (int bx = 0; bx < terrain_map.xcount; bx++)
                {
                    var mnode = terrain_map.getMapNode(bx, by);
                    if (mnode != null)
                    {
                        mnode.area = 0;
                        if (!mnode.Blocked)
                        {
                            dirty_map.Add(mnode, true);
                        }
                    }
                }
            }
            Stack<MMapNode> stack = new Stack<MMapNode>(dirty_map.Count);
            while (dirty_map.Count > 0)
            {
                area_index++;
                var exist = dirty_map.GetEnumerator();
                stack.Clear();
                if (exist.MoveNext())
                {
                    var current = exist.Current.Key;
                    dirty_map.Remove(current);
                    stack.Push(current);
                }
                while (stack.Count > 0)
                {
                    MMapNode cur = stack.Pop();
                    cur.area = area_index;
                    for (int i = cur.nextnodes.Length - 1; i >= 0; --i)
                    {
                        MMapNode next = cur.nextnodes[i];
                        if (dirty_map.Remove(next))
                        {
                            stack.Push(next);
                        }
                    }
                }
            }
            area_count = area_index;
        }

        #endregion
        //---------------------------------------------------------------------------------------------------------------------
        #region _FindPath_

        public override WayPoint GenWayPoint(MapNode node)
        {
            return new MWayPoint(node as MMapNode);
        }

        public enum FindPathResult : byte
        {
            /// <summary>
            /// 可通过
            /// </summary>
            Cross = 0,
            /// <summary>
            /// 没有路径
            /// </summary>
            NoWay = 1,
            /// <summary>
            /// 原地
            /// </summary>
            Destination = 2,
            /// <summary>
            /// 寻路范围超出地图范围
            /// </summary>
            OutOfMap = 3,
        }

        public virtual FindPathResult findPath(float sx, float sy, float dx, float dy, out MWayPoint ret, bool optimize = true)
        {
            ret = null;
            int sbx = (int)(sx / cellw);
            int sby = (int)(sy / cellh);
            int dbx = (int)(dx / cellw);
            int dby = (int)(dy / cellh);
            if (sbx >= 0 && sbx < terrain_map.xcount &&
                sby >= 0 && sby < terrain_map.ycount &&
                dbx >= 0 && dbx < terrain_map.xcount &&
                dby >= 0 && dby < terrain_map.ycount)
            {
                if (sbx == dbx && sby == dby)
                {
                    var dn = terrain_map.getMapNode(sbx, sby);
                    if (dn != null)
                    {
                        ret = new MWayPoint(dn);
                        ret.SetPos(dx, dy);
                        return FindPathResult.Destination;
                    }
                    else
                    {
                        return FindPathResult.NoWay;
                    }
                }
                var snode = terrain_map.getMapNode(sbx, sby);
                var dnode = terrain_map.getMapNode(dbx, dby);
                if (snode == null || dnode == null)
                {
                    return FindPathResult.NoWay;
                }
                if (snode.Blocked || dnode.Blocked)
                {
                    return FindPathResult.NoWay;
                }
                if (snode.AreaValue != dnode.AreaValue)
                {
                    return FindPathResult.NoWay;
                }
                BeginFindPath();
                #region space_div
                if (space_div != null)
                {
                    var sn = space_div.getSpaceNodeByBlock(sbx, sby);
                    var dn = space_div.getSpaceNodeByBlock(dbx, dby);
                    if (sn != null && dn != null && sn != dn)
                    {
                        if ((Math.Abs(sn.CX - dn.CX) >= space_div_enable_distance) ||
                            (Math.Abs(sn.CY - dn.CY) >= space_div_enable_distance))
                        {
                            if (sn.AnchorNode != null && dn.AnchorNode != null)
                            {
                                var space_wp = space_div.findPath(sn, dn) as MSpaceAStar.MSpaceWayPoint;
                                if (space_wp == null || space_wp.next == null)
                                {
                                    return FindPathResult.NoWay;
                                }

                                space_wp = space_wp.Next;
                                space_wp.tail.prev.LinkNext(null);

                                var node_head = space_wp.SpaceNode.AnchorNode;
                                var node_tail = space_wp.Tail.SpaceNode.AnchorNode;

                                var path_head = findPathInternal(sx, sy, node_head.PosX, node_head.PosY, snode, node_head, optimize) as MWayPoint;
                                if (path_head == null)
                                {
                                    return FindPathResult.NoWay;
                                }
                                var path_tail = findPathInternal(node_tail.PosX, node_tail.PosY, dx, dy, node_tail, dnode, optimize) as MWayPoint;
                                if (path_tail == null)
                                {
                                    return FindPathResult.NoWay;
                                }
                                var path_body = space_wp.ToLinkWayPoint();
                                if (path_body != null)
                                {
                                    path_head.tail.LinkNext(path_body);
                                    path_body.tail.LinkNext(path_tail);
                                    if (optimize)
                                    {
                                        if (Math.Abs(sn.CX - dn.CX) <= space_optmize_limit_distance &&
                                            Math.Abs(sn.CY - dn.CY) <= space_optmize_limit_distance)
                                        {
                                            optimizePathInner(path_head);
                                        }
                                    }
                                }
                                else
                                {
                                    path_head.tail.LinkNext(path_tail);
                                    if (optimize)
                                    {
                                        optimizePathInner(path_head);
                                    }
                                }
                                ret = path_head;
                                ret = ret.Next;
                                return FindPathResult.Cross;
                            }
                        }
                    }
                }
                #endregion
                ret = findPathInternal(sx, sy, dx, dy, snode, dnode, optimize);
				if (ret == null || ret.Next == null)
				{
                    return FindPathResult.NoWay;
                }
                else
                {
                    ret = ret.Next;
                    return FindPathResult.Cross;
                }
            }
            else
            {
                return FindPathResult.OutOfMap;
            }
        }

        protected virtual MWayPoint findPathInternal(float sx, float sy, float dx, float dy, MMapNode snode, MMapNode dnode, bool optimize)
        {
            MWayPoint ret = base.findPath(snode.tempNode, dnode.tempNode) as MWayPoint;
            if (ret.next == null)
            {
                return ret.next as MWayPoint;
            }
            else
            {
                MWayPoint root = ret;
                MWayPoint tail = root.Tail;
                if (!terrain_map.is_inclined)
                {
                    root.SetPos(sx, sy);
                    tail.SetPos(dx, dy);
                }
                else
                {
                    var nroot = GenWayPoint(snode);
                    nroot.LinkNext(root);
                    root = nroot as MWayPoint;
                    root.SetPos(sx, sy);

                    var ntail = GenWayPoint(dnode);
                    tail.LinkNext(ntail);
                    tail = ntail as MWayPoint;
                    tail.SetPos(dx, dy);
                }
                if (optimize)
                {
                    optimizePathInner(root);
                }
                ret = root;
            }
            return ret;
        }
        protected MWayPoint findPathInternal(MMapNode snode, MMapNode dnode)
        {
            MWayPoint ret = base.findPath(snode.tempNode, dnode.tempNode) as MWayPoint;
            if (ret.next == null)
            {
                return ret.next as MWayPoint;
            }
            else
            {
                optimizePathInner(ret);
            }
            return ret;
        }

        protected virtual void optimizePathInner(MWayPoint root)
        {
            bool finded = false;
            do
            {
                finded = false;
                MWayPoint current = root;
                while (current != null && current.next != null)
                {
                    MWayPoint next = current.Next;
                    while (true)
                    {
                        if (next == null)
                        {
                            current = next;
                            break;
                        }
                        if ((current.PosX == next.PosX) && (current.PosY == next.PosY))
                        {
                            current.LinkNext(next.Next);
                            next = next.Next;
                            finded = true;
                            continue;
                        }
                        if (next.Next == null)
                        {
                            current = next;
                            break;
                        }
                        if (current.IsCenter && next.IsCenter && next.Next.IsCenter)
                        {
                            //三点一线//
                            if (current.BX == next.BX && current.BX == next.Next.BX)
                            {
                                current.LinkNext(next.Next);
                                next = next.Next;
                                finded = true;
                                break;
                            }
                            if (current.BY == next.BY && current.BY == next.Next.BY)
                            {
                                current.LinkNext(next.Next);
                                next = next.Next;
                                finded = true;
                                break;
                            }
                            if (IsInclined)
                            {
                                int cnnx = current.BX - next.Next.BX;
                                int cnny = current.BY - next.Next.BY;
                                if (Math.Abs(cnnx) == Math.Abs(cnny))
                                {
                                    int cnx = current.BX - next.BX;
                                    int cny = current.BY - next.BY;
                                    int nnx = next.BX - next.Next.BX;
                                    int nny = next.BY - next.Next.BY;
                                    if ((cnnx == cnx + nnx) && (cnny == cny + nny))
                                    {
                                        current.LinkNext(next.Next);
                                        next = next.Next;
                                        finded = true;
                                        break;
                                    }
                                }
                            }
                        }
                        if (TouchMapLine(current.PosX, current.PosY, next.Next.PosX, next.Next.PosY))
                        {
                            current = next;
                            break;
                        }
                        else
                        {
                            current.LinkNext(next.Next);
                            next = next.Next;
                            finded = true;
                            continue;
                        }
                    }
                }
            }
            while (finded);
        }

        #endregion
        //---------------------------------------------------------------------------------------------------------------------
        #region _MapNodes_

        public MMapNode GetMapNode(int bx, int by)
        {
            if (bx >= 0 && bx < terrain_map.xcount && by >= 0 && by < terrain_map.ycount)
            {
                return this.terrain_map.getMapNode(bx, by);
            }
            return null;
        }
        public MMapNode GetMapNodeByPos(float x, float y)
        {
            int bx = (int)(x / cellw);
            int by = (int)(y / cellh);
            return GetMapNode(bx, by);
        }

        public MSpaceAStar.MSpaceNode GetSpaceMapNode(int cx, int cy)
        {
            if (space_div != null)
            {
                return space_div.SpaceMap.getSpaceNode(cx, cy);
            }
            return null;
        }

        public void PosToBlock(float x, float y, out int bx, out int by)
        {
            bx = (int)(x / cellw);
            by = (int)(y / cellh);
        }

        public bool InRangeByBlock(int bx, int by)
        {
            if (bx >= 0 && bx < manhattan_map.XCount && by >= 0 && by < manhattan_map.YCount)
            {
                return true;
            }
            return false;
        }

        public bool TouchMapBlock(int bx, int by)
        {
            var node = GetMapNode(bx, by);
            if (node == null)
                return true;
            return node.Blocked;
        }

        public bool TouchMapRect(float x0, float y0, float x1, float y1)
        {
            if (x0 > x1) CUtils.Swap(ref x0, ref x1);
            if (y0 > y1) CUtils.Swap(ref y0, ref y1);
            int bx0 = (int)(x0 / cellw);
            int by0 = (int)(y0 / cellh);
            int bx1 = (int)(x1 / cellw) + 1;
            int by1 = (int)(y1 / cellh) + 1;
            if (bx0 < 0) bx0 = 0;
            if (by0 < 0) by0 = 0;
            if (bx1 >= terrain_map.xcount) bx1 = terrain_map.xcount - 1;
            if (by1 >= terrain_map.ycount) by1 = terrain_map.ycount - 1;
            for (int by = by0; by <= by1; by++)
            {
                for (int bx = bx0; bx <= bx1; bx++)
                {
                    MMapNode node = terrain_map.getMapNode(bx, by);
                    if (node == null || node.Blocked)
                    {
                        return true;
                    }
                }
            }
            return false;
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="x0"></param>
        /// <param name="y0"></param>
        /// <param name="x1"></param>
        /// <param name="y1"></param>
        /// <returns></returns>
        public bool TouchMapLine(float x0, float y0, float x1, float y1)
        {
            int bx0 = (int)(x0 / cellw);
            int by0 = (int)(y0 / cellh);
            int bx1 = (int)(x1 / cellw);
            int by1 = (int)(y1 / cellh);
            if (bx0 > bx1) CUtils.Swap(ref bx0, ref bx1);
            if (by0 > by1) CUtils.Swap(ref by0, ref by1);
            if (bx0 < 0) bx0 = 0;
            if (by0 < 0) by0 = 0;
            if (bx1 >= terrain_map.xcount) bx1 = terrain_map.xcount - 1;
            if (by1 >= terrain_map.ycount) by1 = terrain_map.ycount - 1;
            for (int by = by0; by <= by1; by++)
            {
                for (int bx = bx0; bx <= bx1; bx++)
                {
                    MMapNode node = terrain_map.getMapNode(bx, by);
                    if (node == null)
                    {
                        return true;
                    }
                    else if (node.TouchLine(x0, y0, x1, y1))
                    {
                        return true;
                    }
                }
            }
            return false;
        }
        /// <summary>
        /// 获得线段和地图碰撞最近的焦点
        /// </summary>
        /// <param name="x0">起始点X</param>
        /// <param name="y0">起始点Y</param>
        /// <param name="x1">结束点X</param>
        /// <param name="y1">结束点Y</param>
        /// <param name="touch_x">接触点</param>
        /// <param name="touch_y">接触点</param>
        /// <param name="touch_distance">接触距离</param>
        /// <returns></returns>
        public bool GetLineTouchPoint(float x0, float y0, float x1, float y1, out float touch_x, out float touch_y, out float touch_distance)
        {
            int bx0 = (int)(x0 / cellw);
            int by0 = (int)(y0 / cellh);
            int bx1 = (int)(x1 / cellw);
            int by1 = (int)(y1 / cellh);
            if (bx0 > bx1) CUtils.Swap(ref bx0, ref bx1);
            if (by0 > by1) CUtils.Swap(ref by0, ref by1);
            if (bx0 < 0) bx0 = 0;
            if (by0 < 0) by0 = 0;
            if (bx1 >= terrain_map.xcount) bx1 = terrain_map.xcount - 1;
            if (by1 >= terrain_map.ycount) by1 = terrain_map.ycount - 1;

            bool ret = false;
            float t_d = 0;
            float t_x = 0;
            float t_y = 0;
            touch_x = 0;
            touch_y = 0;
            touch_distance = float.MaxValue;

            for (int by = by0; by <= by1; by++)
            {
                for (int bx = bx0; bx <= bx1; bx++)
                {
                    MMapNode node = terrain_map.getMapNode(bx, by);
                    if (node != null && node.GetLineTouchPoint(x0, y0, x1, y1, out t_x, out t_y, out t_d))
                    {
                        if (t_d < touch_distance)
                        {
                            touch_distance = t_d;
                            touch_x = t_x;
                            touch_y = t_y;
                        }
                        ret = true;
                    }
                }
            }
            return ret;
        }

        public enum TryMoveToMapBorderResult : byte
        {
            ARRIVE = 0,
            TOUCH = 1,
            BLOCK = 2,
        }

        /// <summary>
        /// 尝试移动坐标，如果碰撞则贴在碰撞边缘
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="dx"></param>
        /// <param name="dy"></param>
        /// <returns></returns>
        public TryMoveToMapBorderResult TryMoveToMapBorder(ref float x, ref float y, float dx, float dy)
        {
            TryMoveToMapBorderResult result = TryMoveToMapBorderResult.ARRIVE;
            float oldx = x;
            float oldy = y;
            if (TouchMapBlock((int)((x + dx) / cellw), (int)((y) / cellh)))
            {
                result = TryMoveToMapBorderResult.TOUCH;
            }
            else
            {
                x += dx;
            }
            if (TouchMapBlock((int)((x) / cellw), (int)((y + dy) / cellh)))
            {
                result = TryMoveToMapBorderResult.TOUCH;
            }
            else
            {
                y += dy;
            }
            if (x == oldx && y == oldy)
            {
                result = TryMoveToMapBorderResult.BLOCK;
            }
            return result;
        }

        /// <summary>
        /// 找到附近可过去的随机一点
        /// </summary>
        /// <param name="random"></param>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="distance"></param>
        /// <param name="recommend">优先四周无其他阻挡</param>
        /// <returns></returns>
        public MMapNode FindNearRandomMoveableNode(System.Random random, float x, float y, float distance, bool recommend = false)
        {
            int cx1 = (int)((x - distance) / this.cellw) - 1;
            int cy1 = (int)((y - distance) / this.cellh) - 1;
            int cx2 = (int)((x + distance) / this.cellw) + 1;
            int cy2 = (int)((y + distance) / this.cellh) + 1;
            cx1 = Math.Max(cx1, 0);
            cy1 = Math.Max(cy1, 0);
            cx2 = Math.Min(cx2, this.manhattan_map.XCount - 1);
            cy2 = Math.Min(cy2, this.manhattan_map.YCount - 1);
            int x_len = cx2 - cx1 + 1;
            int y_len = cy2 - cy1 + 1;
            if (x_len > 0 && y_len > 0)
            {
                int bx = random.Next(x_len);
                int by = random.Next(y_len);
                for (int iy = 0; iy < y_len; iy++)
                {
                    for (int ix = 0; ix < x_len; ix++)
                    {
                        var ret = terrain_map.getMapNode(
                            cx1 + CMath.cycNum(bx, ix, x_len),
                            cy1 + CMath.cycNum(by, iy, y_len));
                        if (ret != null && !ret.Blocked)
                        {
                            if (recommend)
                            {
                                return ToRecommend(ret, cx1, cy1, cx2, cy2);
                            }
                            else
                            {
                                return ret;
                            }
                        }
                    }
                }
            }
            return null;
        }


		/// <summary>
		/// 找到附近可过去的随机一点
		/// </summary>
		/// <param name="random"></param>
		/// <param name="x"></param>
		/// <param name="y"></param>
		/// <param name="distance"></param>
		/// <param name="recommend">优先四周无其他阻挡</param>
		/// <returns></returns>
		public MMapNode FindNearRandomMoveableNode(System.Random random, float x, float y, float minDis, float maxDis, bool recommend = false)
		{
			int cx1 = (int)((x - maxDis) / this.cellw) - 1;
			int cy1 = (int)((y - maxDis) / this.cellh) - 1;
			int cx2 = (int)((x + maxDis) / this.cellw) + 1;
			int cy2 = (int)((y + maxDis) / this.cellh) + 1;
			cx1 = Math.Max(cx1, 0);
			cy1 = Math.Max(cy1, 0);
			cx2 = Math.Min(cx2, this.manhattan_map.XCount - 1);
			cy2 = Math.Min(cy2, this.manhattan_map.YCount - 1);
			int x_len = cx2 - cx1 + 1;
			int y_len = cy2 - cy1 + 1;

			int mx1 = (int)((x - minDis) / this.cellw) - 1;
			int my1 = (int)((y - minDis) / this.cellh) - 1;
			int mx2 = (int)((x + minDis) / this.cellw) + 1;
			int my2 = (int)((y + minDis) / this.cellh) + 1;
			mx1 = Math.Max(mx1, 0);
			my1 = Math.Max(my1, 0);
			mx2 = Math.Min(mx2, this.manhattan_map.XCount - 1);
			my2 = Math.Min(my2, this.manhattan_map.YCount - 1);
			int mx_len = mx2 - mx1 + 1;
			int my_len = my2 - my1 + 1;

			if (x_len > 0 && y_len > 0)
			{
				int bx = random.Next(x_len);
				int by = random.Next(y_len);
				int tempX = 0, tempY = 0;
				for (int iy = 0; iy < y_len; iy++)
				{
					for (int ix = 0; ix < x_len; ix++)
					{
						tempX = cx1 + CMath.cycNum(bx, ix, x_len);
						tempY = cy1 + CMath.cycNum(by, iy, y_len);
						if (mx1 < tempX && tempX < mx2 && my1 < tempY && tempY < my2)
							continue;

						var ret = terrain_map.getMapNode(tempX, tempY);
						if (ret != null && !ret.Blocked)
						{
							if (recommend)
							{
								ret = ToRecommend(ret, cx1, cy1, cx2, cy2);
							}
							//if(CMath.includeRectPoint(cx1, cy1, cx2, cy2, ret.PosX, ret.PosY) 
							//	&& !CMath.includeRectPoint(mx1, my1, mx2, my2, ret.PosX, ret.PosY))
							//{
								
							//}
							//else
							//{
							//	System.Console.WriteLine("验证失败");
							//}
							return ret;
						}
					}
				}
			}
			return null;
		}


		/// <summary>
		/// 指定范围可过去的一点.
		/// </summary>
		/// <param name="x"></param>
		/// <param name="y"></param>
		/// <param name="distance"></param>
		/// <param name="recommend"></param>
		/// <returns></returns>
		public MMapNode FindNearMoveableNode(float x, float y, float distance, bool recommend = false)
        {
            int cx1 = (int)((x - distance) / this.cellw) - 1;
            int cy1 = (int)((y - distance) / this.cellh) - 1;
            int cx2 = (int)((x + distance) / this.cellw) + 1;
            int cy2 = (int)((y + distance) / this.cellh) + 1;
            cx1 = Math.Max(cx1, 0);
            cy1 = Math.Max(cy1, 0);
            cx2 = Math.Min(cx2, this.manhattan_map.XCount - 1);
            cy2 = Math.Min(cy2, this.manhattan_map.YCount - 1);
            int x_len = cx2 - cx1 + 1;
            int y_len = cy2 - cy1 + 1;
            if (x_len > 0 && y_len > 0)
            {
                int bx = (x_len);
                int by = (y_len);
                for (int iy = 0; iy < y_len; iy++)
                {
                    for (int ix = 0; ix < x_len; ix++)
                    {
                        var ret = terrain_map.getMapNode(
                            cx1 + CMath.cycNum(bx, ix, x_len),
                            cy1 + CMath.cycNum(by, iy, y_len));
                        if (ret != null && !ret.Blocked)
                        {
                            if (recommend)
                            {
                                return ToRecommend(ret, cx1, cy1, cx2, cy2);
                            }
                            else
                            {
                                return ret;
                            }
                        }
                    }
                }
            }
            return null;
        }

        /// <summary>
        /// 找到离中心最近的可移动点
        /// </summary>
        /// <param name="sx"></param>
        /// <param name="sy"></param>
        /// <param name="sw"></param>
        /// <param name="sh"></param>
        /// <param name="recommend">优先四周无其他阻挡</param>
        /// <returns></returns>
        public MMapNode FindNearMoveableNodeByBlock(int sx, int sy, int sw, int sh, bool recommend = true)
        {
            int count = Math.Max(sw / 2 + 1, sh / 2 + 1);
            int dx = sx + sw - 1;
            int dy = sy + sh - 1;
            int cx = sx + sw / 2;
            int cy = sy + sh / 2;
            for (int r = 0; r <= count; r++)
            {
                int cx1 = cx - r;
                int cy1 = cy - r;
                int cx2 = cx + r;
                int cy2 = cy + r;
                for (int ix = cx1; ix <= cx2; ix++)
                {
                    if ((ix >= sx) && (ix <= dx))
                    {
                        for (int iy = cy1; iy < cy2; iy++)
                        {
                            if ((iy >= sy) && (iy <= dy))
                            {
                                var cn = GetMapNode(ix, iy);
                                if (cn != null && !cn.Blocked)
                                {
                                    if (recommend)
                                    {
                                        return ToRecommend(cn, sx, sy, dx, dy);
                                    }
                                    else
                                    {
                                        return cn;
                                    }
                                }
                            }
                        }
                    }
                }
            }
            return null;
        }

        private MMapNode ToRecommend(MMapNode src, int min_x, int min_y, int max_x, int max_y)
        {
            if (src.IsAllCross) return src;
            foreach (int[] offset in near_table_inclined)
            {
                int ix = src.BX + offset[0];
                int iy = src.BY + offset[1];
                if (ix >= min_x && iy >= min_y && ix <= max_x && iy <= max_y)
                {
                    var cn = GetMapNode(ix, iy);
                    if (cn != null && cn.IsAllCross)
                    {
                        return cn;
                    }
                }
            }
            return src;
        }

        /// <summary>
        /// 检测范围内有无期望的地块
        /// </summary>
        /// <param name="sx"></param>
        /// <param name="sy"></param>
        /// <param name="sw"></param>
        /// <param name="sh"></param>
        /// <param name="expect_block"></param>
        /// <returns></returns>
        private bool CheckNearExpectByBlock(int sx, int sy, int sw, int sh, bool expect_block)
        {
            int cx1 = Math.Max(sx, 0);
            int cy1 = Math.Max(sy, 0);
            int cx2 = Math.Min(sx + sw, this.manhattan_map.XCount - 1);
            int cy2 = Math.Min(sy + sh, this.manhattan_map.YCount - 1);
            for (int cx = cx1; cx <= cx2; ++cx)
            {
                for (int cy = cy1; cy <= cy2; ++cy)
                {
                    var cn = terrain_map.getMapNode(cx, cy);
                    if (cn == null)
                    {
                        if (expect_block == false) return false;
                    }
                    else if (cn.Blocked != expect_block)
                    {
                        return false;
                    }
                }
            }
            return true;
        }
        /// <summary>
        /// 检测范围内希望的地块数量
        /// </summary>
        /// <param name="sx"></param>
        /// <param name="sy"></param>
        /// <param name="sw"></param>
        /// <param name="sh"></param>
        /// <param name="expect_block"></param>
        /// <returns></returns>
        private int GetNearExpectCountByBlock(int sx, int sy, int sw, int sh, bool expect_block)
        {
            int ret = 0;
            int cx1 = Math.Max(sx, 0);
            int cy1 = Math.Max(sy, 0);
            int cx2 = Math.Min(sx + sw, this.manhattan_map.XCount);
            int cy2 = Math.Min(sy + sh, this.manhattan_map.YCount);
            for (int cx = cx1; cx < cx2; ++cx)
            {
                for (int cy = cy1; cy < cy2; ++cy)
                {
                    var cn = terrain_map.getMapNode(cx, cy);
                    if (cn == null)
                    {
                        if (expect_block) ret++;
                    }
                    else if (cn.Blocked == expect_block)
                    {
                        ret++;
                    }
                }
            }
            return ret;
        }

        #endregion
        //---------------------------------------------------------------------------------------------------------------------
        #region _常量_

        private static int[][] near_table_inclined = new int[][]
        {
            new int[]{ 0,-1}, // top
            new int[]{-1, 0}, // left
            new int[]{ 1, 0}, // right
            new int[]{ 0, 1}, // bottom

            new int[]{-1,-1}, // top left
            new int[]{ 1,-1}, // top right
            new int[]{-1, 1}, // bottom left
            new int[]{ 1, 1}, // bottom right
        };
        private static int[][] near_table_cross = new int[][]
        {
            new int[]{ 0,-1}, // top
            new int[]{-1, 0}, // left
            new int[]{ 1, 0}, // right
            new int[]{ 0, 1}, // bottom
        };

        public static int[][] GetNearTables(bool inclined)
        {
            int[][] near_table = inclined ? near_table_inclined : near_table_cross;
            return near_table;
        }

        class DirtyNodes<T>
        {
            private HashMap<T, T> map = new HashMap<T, T>();
            public void AddDirty(T obj) { map.Put(obj, obj); }
            public void Clear() { map.Clear(); }
            public int Count { get { return map.Count; } }
            public ICollection<T> Values { get { return map.Values; } }
        }

        #endregion
        //------------------------------------------------------------------------------------------------------------------
    }





}