C# A*算法

猫要去骨头的地方

我们使用方块（一个正方形）作为寻路算法的单元

源码

//每个节点

    class NodeData
    {
        public int X;
        public int Y;
        public NodeData preNode;
        public int F;//到目的地估算值
    }

//定义的数据

private int[,] map = new int[8, 9];
private Dictionary<int, NodeData> m_closeNodeDic = new Dictionary<int, NodeData>();
private Dictionary<int, NodeData> m_openNodeDic = new Dictionary<int, NodeData>();

 

//初始化map 以及开始查找

    private void TestAStar()
        {
            //阻挡
            for (int i = 0; i < map.GetLength(0); i++)
            {
                map[i, 0] = -1;
                map[i, map.GetLength(1) -1] = -1;
            }

            for (int i = 0; i < map.GetLength(1); i++)
            {
                map[0, i] = -1;
                map[map.GetLength(0) - 1, i] = -1;
            }

            //固定障碍物
            map[2, 4] = -1;
            map[3, 4] = -1;
            map[4, 4] = -1;
            map[5, 4] = -1;
            map[5, 2] = -1;

            for (int i = 0; i < map.GetLength(0); i++)
            {
                string str = "";
                for (int j = 0; j < map.GetLength(1); j++)
                {
                    if(string.IsNullOrEmpty(str))
                    {
                        str += map[i,j];
                    }
                    else
                    {
                        str += " " + map[i, j];
                    }
                }
                DLogger.Error(str);
            }

            NodeData startNode = new NodeData();
            startNode.X = 4;
            startNode.Y = 2;

            FindPath(map,startNode,3,8);
        }

  private int GetDicKey(int x,int y)
        {
            return x * 10000 + y;
        }

        private int GetH(int curX, int curY, int targetX, int targetY)
        {
            return Mathf.Abs(targetX - curX)*10 + Mathf.Abs(targetY - curY)*10;
        }

        private void AddOpenList(int[,] map, NodeData startNode, int curX, int curY, int targetX, int targetY)
        {
            if (curX < 0 || curY < 0 || curX >= map.GetLength(0) || curY >= map.GetLength(1))
                return;

            //如果是障碍物
            if (map[curX, curY] < 0)
                return;

            if (m_closeNodeDic.ContainsKey(GetDicKey(curX, curY)))
                return;

            int curH = GetH(curX,curY,targetX,targetY);

            int curG = 10;
            NodeData preNode = startNode.preNode;
            while(preNode != null)
            {
                preNode = preNode.preNode;
                curG += 10;
            }
            int curF = curG + curH;
            NodeData nodeData;
            if(m_openNodeDic.TryGetValue(GetDicKey(curX,curY), out nodeData))
            {
                //更新F
                if(nodeData.F < curF)
                {
                    nodeData.preNode = startNode;
                    nodeData.F = curF;
                }
            }
            else
            {
                nodeData = new NodeData();
                nodeData.X = curX;
                nodeData.Y = curY;
                nodeData.preNode = startNode;
                nodeData.F = curF;
                m_openNodeDic.Add(GetDicKey(curX, curY), nodeData);
            }
        }

        private void FindPath(int[,] map,NodeData startNode, int targetX, int targetY)
        {
            m_openNodeDic.Remove(GetDicKey(startNode.X, startNode.Y));
            m_closeNodeDic.Add(GetDicKey(startNode.X, startNode.Y), startNode);

            AddOpenList(map, startNode, startNode.X - 1, startNode.Y,targetX, targetY);
            AddOpenList(map, startNode, startNode.X + 1, startNode.Y, targetX, targetY);
            AddOpenList(map, startNode, startNode.X, startNode.Y - 1, targetX, targetY);
            AddOpenList(map, startNode, startNode.X, startNode.Y + 1, targetX, targetY);

            //检查下是否已经到目的地了
            NodeData targetNode;
            if(m_openNodeDic.TryGetValue(GetDicKey(targetX,targetY), out targetNode))
            {
                while(targetNode.preNode != null)
                {
                    DLogger.Error("X:{0} Y:{1}", targetNode.preNode.X, targetNode.preNode.Y);
                    targetNode = targetNode.preNode;
                }
            }
            else
            {
                //没找到
                if(m_openNodeDic.Count > 0)
                {
                    //找到最小F值的node
                    Dictionary<int, NodeData> open_SortedByF = m_openNodeDic.OrderBy(o => o.Value.F).ToDictionary(p => p.Key, o => o.Value);

                    var itor = open_SortedByF.GetEnumerator();
                    itor.MoveNext();
                    FindPath(map, itor.Current.Value, targetX, targetY);
                }
                else
                {
                    //到不了
                    DLogger.Error("目的地到不了 =================");
                }
            }
        }