巡路算法

比较闲，看了网上的思路后写了一个玩玩

public class MapManager
{
    private int mapMaxRow = 0;
    private int mapMaxCol = 0;
    private int[,] mapInfo;
    //终点坐标
    private int finalRowIndex = 0;
    private int finalColIndex = 0;

    //起点坐标
    private int startRowIndex = 0;
    private int startColIndex = 0;

    /// <summary>
    /// 结果列表
    /// </summary>
    public 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 mapMaxRow, int mapMaxCol, int startRowIndex, int startColIndex, int finalRowIndex, int finalColIndex)
    {
        this.mapMaxRow = mapMaxRow;
        this.mapMaxCol = mapMaxCol;
        mapInfo = new int[this.mapMaxRow, this.mapMaxCol];
        this.startRowIndex = startRowIndex;
        this.startColIndex = startColIndex;
        this.finalRowIndex = finalRowIndex;
        this.finalColIndex = finalColIndex;
    }

    /// <summary>
    /// 添加障碍物
    /// </summary>
    /// <param name="rowIndex"></param>
    /// <param name="colIndex"></param>
    public void AddObstacle(int rowIndex, int colIndex)
    {
        mapInfo[rowIndex, colIndex] = (int)PointType.OBSTACLE;
    }

    /// <summary>
    /// 初始化地图
    /// </summary>
    public void InitMap()
    {
        for (int i = 0; i < mapMaxRow; i++)
        {
            for (int j = 0; j < mapMaxCol; j++)
            {
                mapInfo[i, j] = (int)PointType.EMPTY;
            }
        }

        mapInfo[startRowIndex, startColIndex] = (int)PointType.START;//设置起点
        mapInfo[finalRowIndex, finalColIndex] = (int)PointType.END;//设置终点
    }


    /// <summary>
    /// 获取地图显示内容 0空地□、1障碍■、2起点○、3终点●、4路线▲
    /// </summary>
    /// <param name="rowIndex"></param>
    /// <param name="colIndex"></param>
    /// <returns></returns>
    private string GetMapDes(int rowIndex, int colIndex)
    {
        switch ((PointType)mapInfo[rowIndex, colIndex])
        {
            case PointType.EMPTY:
                return "□";//空地
            case PointType.OBSTACLE:
                return "■";//障碍物
            case PointType.START:
                return "○";//起点
            case PointType.END:
                return "●";//终点
            case PointType.PATH:
                return "◎";//路线
            case PointType.TEST_POINT:
                return "△";//测试点位
        }

        return "□";
    }


    public enum PointType
    {
        /// <summary>
        /// 空地
        /// </summary>
        EMPTY,

        /// <summary>
        /// 障碍物
        /// </summary>
        OBSTACLE,

        /// <summary>
        /// 起点
        /// </summary>
        START,

        /// <summary>
        /// 终点
        /// </summary>
        END,

        /// <summary>
        /// 路径点
        /// </summary>
        PATH,

        /// <summary>
        /// 测试点位
        /// </summary>
        TEST_POINT
    }

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

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


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


        GetPathList(startRowIndex, startColIndex, finalRowIndex, finalColIndex);

        GetResList(finalRowIndex, finalColIndex, ref resList);
        for (int i = 0; i < resList.Count; i++)
        {
            if (resList[i].rowIndex == startRowIndex && resList[i].colIndex == startColIndex)
            {
                return;
            }
            mapInfo[resList[i].rowIndex, resList[i].colIndex] = (int)PointType.PATH;
        }
    }


    /// <summary>
    /// 获取路径点位
    /// </summary>
    /// <param name="rowIndex"></param>
    /// <param name="colIndex"></param>
    /// <param name="finalRowIndex"></param>
    /// <param name="finalColIndex"></param>
    private void GetPathList(int rowIndex, int colIndex, int finalRowIndex, int finalColIndex)
    {

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

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

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


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



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

        //找出最小的点
        dic_open = dic_open.OrderBy(p => p.Value.f).ToDictionary(p => p.Key, o => o.Value);
        if (dic_open.Count <= 0)
        {
     　　　　return;
        }
        
        MapInfo tempParent = dic_open.First().Value;


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

        GetPathList(tempParent.rowIndex, tempParent.colIndex, finalRowIndex, finalColIndex);
    }


    /// <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 rowIndex, int colIndex, ref List<MapInfo> resList)
    {
        int parentX = -1;
        int parentY = -1;
        if (dic_open.ContainsKey(rowIndex + "," + colIndex))
        {
            parentX = dic_open[rowIndex + "," + colIndex].parent.rowIndex;
            parentY = dic_open[rowIndex + "," + colIndex].parent.colIndex;
        }

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


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

        MapInfo resP = new MapInfo();
        resP.rowIndex = parentX;
        resP.colIndex = parentY;
        resList.Add(resP);
        if (resP.rowIndex != startRowIndex || resP.colIndex != startColIndex)//如果父节点不是起点，则继续往前推进
        {
            GetResList(resP.rowIndex, resP.colIndex, 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].rowIndex + "," + temp[i].colIndex);
        }
    }


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

    /// <summary>
    /// 獲取下方的點位
    /// </summary>
    /// <param name="rowIndex"></param>
    /// <param name="colIndex"></param>
    /// <returns></returns>
    private MapInfo GetDownPoint(int rowIndex, int colIndex)
    {
        if (rowIndex + 1 >= mapMaxRow)
        {
            return null;
        }

        MapInfo mapInfo = new MapInfo();
        mapInfo.rowIndex = rowIndex + 1;
        mapInfo.colIndex = colIndex;
        mapInfo.g = GetG(mapInfo.rowIndex, mapInfo.colIndex, rowIndex, colIndex);
        mapInfo.h = GetH(mapInfo.rowIndex, mapInfo.colIndex, finalRowIndex, finalColIndex);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.rowIndex = rowIndex;
        mapInfo.parent.colIndex = colIndex;
        return mapInfo;
    }

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

    /// <summary>
    /// 获取右边点位
    /// </summary>
    /// <param name="rowIndex"></param>
    /// <param name="colIndex"></param>
    /// <returns></returns>
    private MapInfo GetRightPoint(int rowIndex, int colIndex)
    {
        if (colIndex + 1 >= mapMaxCol)
        {
            return null;
        }

        MapInfo mapInfo = new MapInfo();
        mapInfo.rowIndex = rowIndex;
        mapInfo.colIndex = colIndex + 1;
        mapInfo.g = GetG(mapInfo.rowIndex, mapInfo.colIndex, rowIndex, colIndex);
        mapInfo.h = GetH(mapInfo.rowIndex, mapInfo.colIndex, finalRowIndex, finalColIndex);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.rowIndex = rowIndex;
        mapInfo.parent.colIndex = colIndex;
        return mapInfo;
    }

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

        MapInfo mapInfo = new MapInfo();
        mapInfo.rowIndex = rowIndex - 1;
        mapInfo.colIndex = colIndex - 1;
        mapInfo.g = GetG(mapInfo.rowIndex, mapInfo.colIndex, rowIndex, colIndex);
        mapInfo.h = GetH(mapInfo.rowIndex, mapInfo.colIndex, finalRowIndex, finalColIndex);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.rowIndex = rowIndex;
        mapInfo.parent.colIndex = colIndex;
        return mapInfo;
    }


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

        MapInfo mapInfo = new MapInfo();
        mapInfo.rowIndex = rowIndex - 1;
        mapInfo.colIndex = colIndex + 1;
        mapInfo.g = GetG(mapInfo.rowIndex, mapInfo.colIndex, rowIndex, colIndex);
        mapInfo.h = GetH(mapInfo.rowIndex, mapInfo.colIndex, finalRowIndex, finalColIndex);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.rowIndex = rowIndex;
        mapInfo.parent.colIndex = colIndex;
        return mapInfo;
    }

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

        MapInfo mapInfo = new MapInfo();
        mapInfo.rowIndex = rowIndex + 1;
        mapInfo.colIndex = colIndex - 1;
        mapInfo.g = GetG(mapInfo.rowIndex, mapInfo.colIndex, rowIndex, colIndex);
        mapInfo.h = GetH(mapInfo.rowIndex, mapInfo.colIndex, finalRowIndex, finalColIndex);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.rowIndex = rowIndex;
        mapInfo.parent.colIndex = colIndex;
        return mapInfo;
    }

    /// <summary>
    /// 获取右下点位
    /// </summary>
    /// <param name="rowIndex"></param>
    /// <param name="colIndex"></param>
    /// <returns></returns>
    private MapInfo GetRightDownPoint(int rowIndex, int colIndex)
    {
        if (rowIndex + 1 >= mapMaxRow || colIndex + 1 >= mapMaxCol)
        {
            return null;
        }

        MapInfo mapInfo = new MapInfo();
        mapInfo.rowIndex = rowIndex + 1;
        mapInfo.colIndex = colIndex + 1;
        mapInfo.g = GetG(mapInfo.rowIndex, mapInfo.colIndex, rowIndex, colIndex);
        mapInfo.h = GetH(mapInfo.rowIndex, mapInfo.colIndex, finalRowIndex, finalColIndex);
        mapInfo.f = mapInfo.g + mapInfo.h;
        mapInfo.parent.rowIndex = rowIndex;
        mapInfo.parent.colIndex = colIndex;
        return mapInfo;
    }


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



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


        if (rowIndex == parentRowIndex || colIndex == parentColIndex)//说明是同一列或者行
        {
            res += 10;
        }
        else
        {
            //说明是斜方向的
            res += 14;
        }
        return res;
    }


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

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

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

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

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

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

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


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

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

        return list_roundPoint;
    }


    /// <summary>
    /// 地图节点信息
    /// </summary>
    public class MapInfo
    {
        public int rowIndex = 0;
        public int colIndex = 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 rowIndex = -1;
        public int colIndex = -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();
}