巡路算法

public class MapManager
{
    private int mapMaxX = 0;
    private int mapMaxY = 0;
    private int[,] mapInfo;
    //终点坐标
    private int finalX = 0;
    private int finalY = 0;

    //起点坐标
    private int startX = 0;
    private int startY = 0;


    private List<MapInfo> resList = new List<MapInfo>();
    private Dictionary<string, MapInfo> dic_open = new Dictionary<string, MapInfo>();//开放列表
    private Dictionary<string, MapInfo> dic_close = new Dictionary<string, MapInfo>();//关闭列表
    public MapManager(int maxX, int maxY, int startX, int startY, int finalX, int finalY)
    {
        mapMaxX = maxX;
        mapMaxY = maxY;
        mapInfo = new int[mapMaxX, mapMaxY];
        this.startX = startX;
        this.startY = startY;
        this.finalX = finalX;
        this.finalY = finalY;
    }

    public void InitMap()
    {
        for (int i = 0; i < mapMaxX; i++)
        {
            for (int j = 0; j < mapMaxY; j++)
            {
                mapInfo[i, j] = 0;
            }
        }

        mapInfo[startX, startY] = 2;//设置起点
        mapInfo[finalX, finalY] = 3;//设置终点

        //设置障碍物
        for (int i = 0; i < 18; i++)
        {
            mapInfo[6, i] = 1;
        }

        //设置障碍物
        for (int i = 4; i < 20; i++)
        {
            mapInfo[11, i] = 1;
        }

        for (int i = 0; i < 5; i++)
        {
            mapInfo[1 + i, 6] = 1;
        }
        for (int i = 0; i < 4; i++)
        {
            mapInfo[i, 10] = 1;
        }

        for (int i = 0; i < 4; i++)
        {
            mapInfo[3, 11 + i] = 1;
        }

        for (int i = 0; i < 5; i++)
        {
            mapInfo[1 + i, 17] = 1;
        }

        for (int i = 0; i < 4; i++)
        {
            mapInfo[1, 13 + i] = 1;
        }

        for (int i = 0; i < 6; i++)
        {
            mapInfo[12 + i, 4] = 1;
        }

        for (int i = 0; i < 6; i++)
        {
            mapInfo[14 + i, 10] = 1;
        }
    }
/// <summary>
    /// 获取地图显示内容 0空地□、1障碍■、2起点○、3终点●、4路线▲
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    private string GetMapDes(int x, int y)
    {
        if (mapInfo[x, y] == 1)
        {
            return "■";//障碍物
        }

        if (mapInfo[x, y] == 2)
        {
            return "○";//起点
        }

        if (mapInfo[x, y] == 3)
        {
            return "●";//终点
        }


        if (mapInfo[x, y] == 4)
        {
            return "◎";//路线
        }

        if (mapInfo[x, y] == 5)
        {
            return "△";//测试点位
        }

        return "□";//空地
    }

    /// <summary>
    /// 显示地图信息
    /// </summary>
    public void ShowMapDes()
    {
        for (int i = 0; i < mapMaxX; i++)
        {
            string tempDes = GetMapRowDes(i);
            Console.WriteLine(tempDes);
        }
    }

    /// <summary>
    /// 获取某行的地图信息
    /// </summary>
    /// <param name="row"></param>
    /// <returns></returns>
    private string GetMapRowDes(int row)
    {
        string res = string.Empty;
        for (int i = 0; i < mapMaxY; i++)
        {
            res += GetMapDes(row, i);
        }
        return res;
    }


    /// <summary>
    /// 获取路线
    /// </summary>
    public void GetRoad()
    {
        //先把障碍物放进关闭列表
        for (int i = 0; i < mapMaxX; i++)
        {
            for (int j = 0; j < mapMaxY; j++)
            {
                if (mapInfo[i, j] == 1 || mapInfo[i, j] == 2)//障碍物、起点、终点跳过检测
                {
                    MapInfo mapInfo = new MapInfo();
                    mapInfo.x = i;
                    mapInfo.y = j;
                    dic_close.Add(i + "," + j, mapInfo);
                }
            }
        }


        GetPathList(startX, startY, finalX, finalY);

        //Dictionary<string, MapInfo>.Enumerator it = dic_close.GetEnumerator();
        //while (it.MoveNext())
        //{
        //    if (mapInfo[it.Current.Value.x, it.Current.Value.y] == 1
        //        || mapInfo[it.Current.Value.x, it.Current.Value.y] == 2
        //        || mapInfo[it.Current.Value.x, it.Current.Value.y] == 3)
        //    {
        //        continue;
        //    }
        //    mapInfo[it.Current.Value.x, it.Current.Value.y] = 4;
        //}

        //Dictionary<string, MapInfo>.Enumerator it2 = dic_open.GetEnumerator();
        //while (it2.MoveNext())
        //{
        //    if (mapInfo[it2.Current.Value.x, it2.Current.Value.y] == 1 
        //        || mapInfo[it2.Current.Value.x, it2.Current.Value.y] == 2 
        //        || mapInfo[it2.Current.Value.x, it2.Current.Value.y] == 3)
        //    {
        //        continue;
        //    }
        //    mapInfo[it2.Current.Value.x, it2.Current.Value.y] = 4;
        //}

        GetResList(finalX, finalY, ref resList);
        for (int i = 0; i < resList.Count; i++)
        {
            if (resList[i].x == startX && resList[i].y == startY)
            {
                return;
            }
            mapInfo[resList[i].x, resList[i].y] = 4;
        }
    }


    /// <summary>
    /// 获取路径点位
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <param name="finalX"></param>
    /// <param name="finalY"></param>
    /// <param name="resList"></param>
    private void GetPathList(int x, int y, int finalX, int finalY)
    {

        if (x < 0 || y < 0)
        {
            return;
        }
        Dictionary<string, MapInfo> dic_roundPoints = GetRoundPoints(x, y);

        if (dic_roundPoints.Count <= 0)
        {
            return;
        }

        //判断是否有点位在关闭列表，如果有，就删除,这一步是为了剔除障碍物和已经用过的路径点
        IsExistsCloseListAndDelete(ref dic_roundPoints);


        if (dic_roundPoints.Count > 0)
        {
            AddToOpen(dic_roundPoints);//将周围点位添加进开放列表
        }
        if (dic_open.ContainsKey(x + "," + y))
        {
            dic_close.Add(x + "," + y, dic_open[x + "," + y]);
            dic_open.Remove(x + "," + y);
        }



        if (dic_open.ContainsKey(finalX + "," + finalY))
        {
            return;//说明到达了终点
        }

        //找出最小的点
        dic_open = dic_open.OrderBy(p => p.Value.f).ToDictionary(p => p.Key, o => o.Value);

        MapInfo tempParent = dic_open.First().Value;


        if (dic_open.Count <= 0)
        {
            return;
        }

        GetPathList(tempParent.x, tempParent.y, finalX, finalY);
    }


    /// <summary>
    /// 将源集合添加进开放列表
    /// </summary>
    /// <param name="oriDic"></param>
    private void AddToOpen(Dictionary<string, MapInfo> oriDic)
    {
        Dictionary<string, MapInfo>.Enumerator it = oriDic.GetEnumerator();
        while (it.MoveNext())
        {
            if (!dic_open.ContainsKey(it.Current.Key))
            {
                dic_open.Add(it.Current.Key, it.Current.Value);
            }
            else
            {
                //需要判断，如果新值比原来的小就更新
                if (it.Current.Value.f <= dic_open[it.Current.Key].f)
                {
                    dic_open[it.Current.Key] = it.Current.Value;
                }
            }
        }
    }


    /// <summary>
    /// 获取结果列表
    /// </summary>
    private void GetResList(int x, int y, ref List<MapInfo> resList)
    {
        int parentX = -1;
        int parentY = -1;
        if (dic_open.ContainsKey(x + "," + y))
        {
            parentX = dic_open[x + "," + y].parent.x;
            parentY = dic_open[x + "," + y].parent.y;
        }

        if (dic_close.ContainsKey(x + "," + y))
        {
            parentX = dic_close[x + "," + y].parent.x;
            parentY = dic_close[x + "," + y].parent.y;
        }


        if (parentX == -1 || parentY == -1)
        {
            return;
        }

        MapInfo resP = new MapInfo();
        resP.x = parentX;
        resP.y = parentY;
        resList.Add(resP);
        if (resP.x != startX || resP.y != startY)//如果父节点不是起点，则继续往前推进
        {
            GetResList(resP.x, resP.y, ref resList);
        }
    }




    /// <summary>
    /// 源list中的点位是否存在关闭列表，存在就删除
    /// </summary>
    /// <param name="dic_ori"></param>
    private void IsExistsCloseListAndDelete(ref Dictionary<string, MapInfo> dic_ori)
    {
        List<MapInfo> temp = new List<MapInfo>();
        Dictionary<string, MapInfo>.Enumerator it = dic_ori.GetEnumerator();
        while (it.MoveNext())
        {
            if (dic_close.ContainsKey(it.Current.Key))
            {
                temp.Add(it.Current.Value);
            }
        }

        for (int i = 0; i < temp.Count; i++)
        {
            dic_ori.Remove(temp[i].x + "," + temp[i].y);
        }
    }


    /// <summary>
    /// 获取上方点位
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    public MapInfo GetUpPoint(int x, int y)
    {
        if (x - 1 < 0)
        {
            return null;
        }
        MapInfo mapInfo = new MapInfo();
        mapInfo.x = x - 1;
        mapInfo.y = y;
        mapInfo.g = GetG(mapInfo.x, mapInfo.y, x, y);
        mapInfo.h = GetH(mapInfo.x, mapInfo.y, finalX, finalY);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.x = x;
        mapInfo.parent.y = y;
        return mapInfo;
    }

    /// <summary>
    /// 獲取下方的點位
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    public MapInfo GetDownPoint(int x, int y)
    {
        if (x + 1 >= mapMaxX)
        {
            return null;
        }

        MapInfo mapInfo = new MapInfo();
        mapInfo.x = x + 1;
        mapInfo.y = y;
        mapInfo.g = GetG(mapInfo.x, mapInfo.y, x, y);
        mapInfo.h = GetH(mapInfo.x, mapInfo.y, finalX, finalY);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.x = x;
        mapInfo.parent.y = y;
        return mapInfo;
    }

    /// <summary>
    /// 获取左边点位
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    public MapInfo GetLeftPoint(int x, int y)
    {
        if (y - 1 < 0)
        {
            return null;
        }
        MapInfo mapInfo = new MapInfo();
        mapInfo.x = x;
        mapInfo.y = y - 1;
        mapInfo.g = GetG(mapInfo.x, mapInfo.y, x, y);
        mapInfo.h = GetH(mapInfo.x, mapInfo.y, finalX, finalY);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.x = x;
        mapInfo.parent.y = y;
        return mapInfo;
    }

    /// <summary>
    /// 获取右边点位
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    public MapInfo GetRightPoint(int x, int y)
    {
        if (y + 1 >= mapMaxY)
        {
            return null;
        }

        MapInfo mapInfo = new MapInfo();
        mapInfo.x = x;
        mapInfo.y = y + 1;
        mapInfo.g = GetG(mapInfo.x, mapInfo.y, x, y);
        mapInfo.h = GetH(mapInfo.x, mapInfo.y, finalX, finalY);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.x = x;
        mapInfo.parent.y = y;
        return mapInfo;
    }

    /// <summary>
    /// 获取左上点位
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    public MapInfo GetLeftUpPoint(int x, int y)
    {
        if (x - 1 < 0 || y - 1 < 0)
        {
            return null;
        }

        MapInfo mapInfo = new MapInfo();
        mapInfo.x = x - 1;
        mapInfo.y = y - 1;
        mapInfo.g = GetG(mapInfo.x, mapInfo.y, x, y);
        mapInfo.h = GetH(mapInfo.x, mapInfo.y, finalX, finalY);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.x = x;
        mapInfo.parent.y = y;
        return mapInfo;
    }


    /// <summary>
    /// 获取右上点位
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    public MapInfo GetRightUpPoint(int x, int y)
    {
        if (x - 1 < 0 || y + 1 >= mapMaxY)
        {
            return null;
        }

        MapInfo mapInfo = new MapInfo();
        mapInfo.x = x - 1;
        mapInfo.y = y + 1;
        mapInfo.g = GetG(mapInfo.x, mapInfo.y, x, y);
        mapInfo.h = GetH(mapInfo.x, mapInfo.y, finalX, finalY);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.x = x;
        mapInfo.parent.y = y;
        return mapInfo;
    }

    /// <summary>
    /// 获取左下点位
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    public MapInfo GetLeftDownPoint(int x, int y)
    {
        if (x + 1 >= mapMaxX || y - 1 < 0)
        {
            return null;
        }

        MapInfo mapInfo = new MapInfo();
        mapInfo.x = x + 1;
        mapInfo.y = y - 1;
        mapInfo.g = GetG(mapInfo.x, mapInfo.y, x, y);
        mapInfo.h = GetH(mapInfo.x, mapInfo.y, finalX, finalY);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.x = x;
        mapInfo.parent.y = y;
        return mapInfo;
    }

    /// <summary>
    /// 获取右下点位
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <returns></returns>
    public MapInfo GetRightDownPoint(int x, int y)
    {
        if (x + 1 >= mapMaxX || y + 1 >= mapMaxY)
        {
            return null;
        }

        MapInfo mapInfo = new MapInfo();
        mapInfo.x = x + 1;
        mapInfo.y = y + 1;
        mapInfo.g = GetG(mapInfo.x, mapInfo.y, x, y);
        mapInfo.h = GetH(mapInfo.x, mapInfo.y, finalX, finalY);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.x = x;
        mapInfo.parent.y = y;
        return mapInfo;
    }


    /// <summary>
    /// 获取H估值 该点位到目标点位的（非欧几里得距离，也非麦哈顿距离,实际距离，横移一格算10，斜方向移动算14）
    /// 公式结果：
    /// H = abs(abs(fy - y) -  abs(fx - x))*10 + abs(fx - x)*14
    /// </summary>
    /// <returns></returns>
    private int GetH(int x, int y, int finalx, int finaly)
    {
        int res = Math.Abs(finaly - y - (finalx - x)) * 10 + Math.Abs(finalx - x) * 14;
        return res;
    }



    /// <summary>
    /// 获取g估值 上一个节点+当前的xy坐标的
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <param name="parentX"></param>
    /// <param name="parentY"></param>
    /// <returns></returns>
    private int GetG(int x, int y, int parentX, int parentY)
    {
        int res = 0;
        if (dic_open.ContainsKey(parentX + "," + parentY))
        {
            res = dic_open[parentX + "," + parentY].g;//父节点的g值
        }
        else
        {
            res = dic_close[parentX + "," + parentY].g;
        }


        if (x == parentX || y == parentY)//说明是同一列或者行
        {
            res += 10;
        }
        else
        {
            //说明是斜方向的
            res += 14;
        }
        return res;
    }


    /// <summary>
    /// 获取周围的节点
    /// </summary>
    public Dictionary<string, MapInfo> GetRoundPoints(int x, int y)
    {
        Dictionary<string, MapInfo> list_roundPoint = new Dictionary<string, MapInfo>();

        //正常来说是8个方向的点位
        //左边
        MapInfo lp = GetLeftPoint(x, y);
        if (lp != null)
        {
            list_roundPoint.Add(lp.x + "," + lp.y, lp);
        }

        //右边
        MapInfo rp = GetRightPoint(x, y);
        if (rp != null)
        {
            list_roundPoint.Add(rp.x + "," + rp.y, rp);
        }

        //上面
        MapInfo up = GetUpPoint(x, y);
        if (up != null)
        {
            list_roundPoint.Add(up.x + "," + up.y, up);
        }

        //下面
        MapInfo dp = GetDownPoint(x, y);
        if (dp != null)
        {
            list_roundPoint.Add(dp.x + "," + dp.y, dp);
        }

        //左上
        MapInfo lup = GetLeftUpPoint(x, y);
        if (lup != null)
        {
            list_roundPoint.Add(lup.x + "," + lup.y, lup);
        }

        //右上
        MapInfo rup = GetRightUpPoint(x, y);
        if (rup != null)
        {
            list_roundPoint.Add(rup.x + "," + rup.y, rup);
        }


        //左下
        MapInfo ldp = GetLeftDownPoint(x, y);
        if (ldp != null)
        {
            list_roundPoint.Add(ldp.x + "," + ldp.y, ldp);
        }

        //右下
        MapInfo rdp = GetRightDownPoint(x, y);
        if (rdp != null)
        {
            list_roundPoint.Add(rdp.x + "," + rdp.y, rdp);
        }

        return list_roundPoint;
    }


    /// <summary>
    /// 地图节点信息
    /// </summary>
    public class MapInfo
    {
        public int x = 0;
        public int y = 0;

        /// <summary>
        /// 点位类型
        /// </summary>
        public int type = 0;
        /// <summary>
        /// 跑到此格子的成本
        /// </summary>
        public int g = -1;

        /// <summary>
        /// 跑到终点的最短距离（忽略障碍物）
        /// </summary>
        public int h = -1;

        /// <summary>
        /// 跑到终点的最终成本
        /// </summary>
        public int f = -1;

        public ParentInfo parent = new ParentInfo();
    }

    public class ParentInfo
    {
        public int x = -1;
        public int y = -1;
    }

}

static void Main(string[] args)
{
    MapManager mapManager = new MapManager(20, 20, 2, 3, 17, 17);
    mapManager.InitMap();
    mapManager.ShowMapDes();

    Console.WriteLine("-------------------------开始绘制路线---------------------------------");

    mapManager.GetRoad();

    mapManager.ShowMapDes();

    Console.ReadLine();
}