Update FindWay Algorithm

Author: IceLanguage

Assets/Scripts/01-findPath/Algorithm/Astar.cs

@@ -13,13 +13,24 @@ public class Astar:FindPathAlgorithm
         //关闭列表
         private static List<Node> close_List = new List<Node>();
 
+        //g(n)初始节点到n节点的实际代价
+        private static Func<Node, double> G = node => CostDict[node];
+
+        //h(n)是从n到目标节点最佳路径的估计代价。
+        private static Func<Node,Node, double> H = (node,end) => 
+        (Math.Pow(node.x - end.x, 2)/100f + Math.Pow(node.z - end.z, 2)/100f);
+
+        private static Node end;
         public static Queue<Node> FindWay
         (
             Node start,Node end,
             Dictionary<Node, int> nodesMap)
         {
+            Astar.end = end;
             Init(nodesMap);
 
+            //f(n) = g(n) + h(n)，其中f(n)是节点n从初始点到目标点的估价函数
+            Func<Node, double> F = node => G(node) + H(node, end);
 
             //First Node
             CostDict[start] = 0;
@@ -28,11 +39,10 @@ public static Queue<Node> FindWay
             open_List.Add(start);
 
             Node? curnode = null;
-            Func<Node, int> GetMin = node => CostDict[node];
             while (!(IsInOpenList(end))) //当终点存在开启列表中就意味着寻路结束了
             {
                 //查找开启列表中cost最小的节点
-                curnode = GetMinNodeFromArr(open_List, end, GetMin);
+                curnode = GetMinNodeFromArr(open_List, end, F);
 
                 if(null == curnode)
                 {
@@ -129,143 +139,8 @@ private static void CheckNodeNearby(Node cur)
             }
         }
 
-        /// <summary>
-        /// 不使用closelist ,算法思想和FindWay没有本质不同
-        /// </summary>
-        /// <param name="start"></param>
-        /// <param name="end"></param>
-        /// <param name="nodesMap"></param>
-        /// <param name="NodeCount"></param>
-        /// <returns></returns>
-        public static Queue<Node> FindWay2(
-            Node start,
-            Node end,
-            Dictionary<Node, int> nodesMap)
-        {
-            //Init
-            FindPathAlgorithm.Init(nodesMap);
-
-            //First Node
-            canGetMinNodelist.Add(start);
-            int curcount = 1;
-            CostDict[start] = 0;
-            RoadDict[start].Enqueue(start);
-            Node? curnode = null;
-            Node cur = start;
-            Func<Node, int> GetMin = node => CostDict[node];
-            //Find
-            while (curcount < NodeCount && cur != end)
-            {
-                //查找访问过节点中cost最小的节点
-                curnode = GetMinNodeFromArr(canGetMinNodelist, end, GetMin);
-
-                //无法到达终点
-                if (null == curnode)
-                    break;
-
-                //查找curnode下一个节点中距离终点最近的点
-                cur = (Node)curnode;
-                Node? minnode = GetMinNode(cur, end, GetMin);
-
-                if (null == minnode)
-                {
-                    //当前节点无法到达终点
-                    canGetMinNodelist.Remove(cur);
-                    continue;
-                }
-                else
-                {
-
-
-                    Node _min = (Node)minnode;
-
-                    //标记为已访问
-                    visit[_min] = true;
-                    curcount++;
-                    canGetMinNodelist.Add(_min);
-
-                    //更新路径
-                    CostDict[_min] = CostDict[cur] + NodesMap[_min];
-                    RoadDict[_min].Clear();
-                    foreach (var e in RoadDict[cur])
-                    {
-                        RoadDict[_min].Enqueue(e);
-                    }
-                    RoadDict[_min].Enqueue(_min);
-                }
-            }
-
-            return RoadDict[end];
-        }
+        
 
-        /// <summary>
-        /// 以距离为导向的寻路
-        /// </summary>
-        /// <param name="start"></param>
-        /// <param name="end"></param>
-        /// <param name="nodesMap"></param>
-        /// <returns></returns>
-        public static Queue<Node> FindWayGreedy(
-          Node start,
-          Node end,
-          Dictionary<Node, int> nodesMap
-          )
-        {
-            //Init
-           
-            FindPathAlgorithm.Init(nodesMap);
-
-            //First Node
-            CostDict[start] = 0;
-            RoadDict[start].Enqueue(start);
-
-            Func<Node, int> GetMin = node => (int)(Math.Pow(node.x - end.x, 2) + Math.Pow(node.z - end.z, 2));
-
-            canGetMinNodelist.Add(start);
-            Node? curnode = null;
-            Node cur = start;
-            //Find
-            while (cur != end)
-            {
-                //查找访问过节点中距离终点最近的节点
-                curnode = GetMinNodeFromArr(canGetMinNodelist, end, GetMin);
-
-                //无法到达终点
-                if (null == curnode)
-                    break;
-
-                //查找curnode下一个节点中距离终点最近的点
-                cur = (Node)curnode;
-                Node? minnode = GetMinNode(cur, end, GetMin);
-
-                if (null == minnode)
-                {
-                    //当前节点无法到达终点
-                    canGetMinNodelist.Remove(cur);
-                    continue;
-                }
-                else
-                {
-
-
-                    Node _min = (Node)minnode;
-
-                    //标记为已访问
-                    visit[_min] = true;
-                    canGetMinNodelist.Add(_min);
-
-                    //更新路径
-                    CostDict[_min] = CostDict[cur] + NodesMap[_min];
-                    RoadDict[_min].Clear();
-                    foreach (var e in RoadDict[cur])
-                    {
-                        RoadDict[_min].Enqueue(e);
-                    }
-                    RoadDict[_min].Enqueue(_min);
-                }
-            }
-
-            return RoadDict[end];
-        }
+   
     }
 }

Assets/Scripts/01-findPath/Algorithm/BFS.cs

@@ -9,6 +9,77 @@ public class BFS : FindPathAlgorithm
     {
         private static Dictionary<Node, int> reachCostLeave = 
             new Dictionary<Node, int>();
+
+        /// <summary>
+        /// 获得保证最小消耗的路径(需要访问所有节点)BFS
+        /// </summary>
+        /// <param name="start"></param>
+        /// <param name="end"></param>
+        /// <param name="nodesMap"></param>
+        /// <returns></returns>
+        public static Queue<Node> FindWay(
+            Node start,
+            Node end,
+            Dictionary<Node, int> nodesMap
+            )
+        {
+
+            //Init
+            FindPathAlgorithm.Init(nodesMap);
+
+            //First Node
+            canGetMinNodelist.Add(start);
+            int curcount = 1;
+            CostDict[start] = 0;
+            RoadDict[start].Enqueue(start);
+
+            //Find
+            while (curcount < NodeCount && canGetMinNodelist.Count > 0)
+            {
+                //查找访问过节点中距离终点最近的点
+                Node cur = canGetMinNodelist.First();
+
+                //查找cur所能到达的节点列表
+                List<Node> arr = GetNearbyNode(cur, false);
+
+                //遍历列表里的所有的节点
+                for (int i = 0; i < arr.Count; i++)
+                {
+
+                    Node n = arr[i];
+
+                    //设置为已访问
+                    if (!visit[n])
+                    {
+
+                        canGetMinNodelist.Add(n);
+                        visit[n] = true;
+                        curcount++;
+                    }
+
+
+                    //如果新的路径更短，更新路径
+                    if (CostDict[n] > CostDict[cur] + NodesMap[n])
+                    {
+                        CostDict[n] = CostDict[cur] + NodesMap[n];
+
+                        RoadDict[n] = new Queue<Node>(NodeCount);
+                        foreach (var e in RoadDict[cur])
+                        {
+                            RoadDict[n].Enqueue(e);
+                        }
+                        RoadDict[n].Enqueue(n);
+
+                    }
+
+                }
+                canGetMinNodelist.Remove(cur);
+
+
+            }
+
+            return RoadDict[end];
+        }
         /// <summary>
         /// 战棋游戏人物可到达位置计算
         /// </summary>

Assets/Scripts/01-findPath/Algorithm/Dijkstra.cs

@@ -8,22 +8,20 @@ namespace LinHoweFindPath
 {
     public class Dijkstra: FindPathAlgorithm
     {
-        
 
         /// <summary>
-        /// 获得保证最小消耗的路径(需要访问所有节点)BFS
+        /// 
         /// </summary>
         /// <param name="start"></param>
         /// <param name="end"></param>
         /// <param name="nodesMap"></param>
+        /// <param name="NodeCount"></param>
         /// <returns></returns>
         public static Queue<Node> FindWay(
             Node start,
             Node end,
-            Dictionary<Node, int> nodesMap
-            )
+            Dictionary<Node, int> nodesMap)
         {
-
             //Init
             Init(nodesMap);
 
@@ -32,54 +30,54 @@ Dictionary<Node, int> nodesMap
             int curcount = 1;
             CostDict[start] = 0;
             RoadDict[start].Enqueue(start);
-
+            Node? curnode = null;
+            Node cur = start;
+            Func<Node, double> GetMin = node => CostDict[node];
             //Find
-            while (curcount < NodeCount && canGetMinNodelist.Count > 0)
+            while (curcount < NodeCount && cur != end)
             {
-                //查找访问过节点中距离终点最近的点
-                Node cur = canGetMinNodelist.First();
+                //查找访问过节点中cost最小的节点
+                curnode = GetMinNodeFromArr(canGetMinNodelist, end, GetMin);
+
+                //无法到达终点
+                if (null == curnode)
+                    break;
 
-                //查找cur所能到达的节点列表
-                List<Node> arr = GetNearbyNode(cur, false);
+                //查找curnode下一个节点中距离终点最近的点
+                cur = (Node)curnode;
+                Node? minnode = GetMinNode(cur, end, GetMin);
 
-                //遍历列表里的所有的节点
-                for (int i = 0; i < arr.Count; i++)
+                if (null == minnode)
+                {
+                    //当前节点无法到达终点
+                    canGetMinNodelist.Remove(cur);
+                    continue;
+                }
+                else
                 {
 
-                    Node n = arr[i];
 
-                    //设置为已访问
-                    if (!visit[n])
-                    {
+                    Node _min = (Node)minnode;
 
-                        canGetMinNodelist.Add(n);
-                        visit[n] = true;
-                        curcount++;
-                    }
+                    //标记为已访问
+                    visit[_min] = true;
+                    curcount++;
+                    canGetMinNodelist.Add(_min);
 
-
-                    //如果新的路径更短，更新路径
-                    if (CostDict[n] > CostDict[cur] + NodesMap[n])
+                    //更新路径
+                    CostDict[_min] = CostDict[cur] + NodesMap[_min];
+                    RoadDict[_min].Clear();
+                    foreach (var e in RoadDict[cur])
                     {
-                        CostDict[n] = CostDict[cur] + NodesMap[n];
-
-                        RoadDict[n] = new Queue<Node>(NodeCount);
-                        foreach (var e in RoadDict[cur])
-                        {
-                            RoadDict[n].Enqueue(e);
-                        }
-                        RoadDict[n].Enqueue(n);
-
+                        RoadDict[_min].Enqueue(e);
                     }
-
+                    RoadDict[_min].Enqueue(_min);
                 }
-                canGetMinNodelist.Remove(cur);
-
-
             }
 
             return RoadDict[end];
         }
+        
 
     }
 }