无递归 A星寻路算法

整理硬盘的时候,发现我早些年写的A星寻路算法。特放上来,待有缘人拿去,无递归噢,性能那是杠杠的。

码上伺候

 1 public class Node
 2     {
 3         public int X
 4         {
 5             get;
 6             set;
 7         }
 8 
 9         public int Y
10         {
11             get;
12             set;
13         }
14 
15         public int G
16         {
17             get;
18             set;
19         }
20 
21         public int H
22         {
23             get;
24             set;
25         }
26 
27         public int F
28         {
29             get;
30             set;
31         }
32 
33         public Node parentNode
34         {
35             get;
36             set;
37         }
38 
39         public Node(int x, int y, Node parent)
40         {
41             this.X = x;
42             this.Y = y;
43             this.parentNode = parent;
44         }
45     }
Node

 

  1 public class AStar
  2     {
  3         private int[,] map;
  4         private List<Node> openList;
  5         private List<Node> closeList;
  6         private const int COST_STRAIGHT = 10;
  7         private const int COST_DIAGONAL = 14;
  8         private int row;
  9         private int column;
 10 
 11         public AStar(int[,] map, int row, int column)
 12         {
 13             this.map = map;
 14             this.row = row;
 15             this.column = column;
 16             openList = new List<Node>();
 17             closeList = new List<Node>();
 18         }
 19 
 20         //查找坐标
 21         public int Search(int x1, int y1, int x2, int y2)
 22         {
 23             if (x1 < 0 || x1 >= row || x2 < 0 || x2 >= row || y1 < 0 || y1 >= column || y2 < 0 || y2 >= column)
 24             {
 25                 return -1;
 26             }
 27 
 28             if (map[x1,y1] == 0 || map[x2,y2] == 0)
 29             {
 30                 return -1;
 31             }
 32 
 33             Node start = new Node(x1, y1, null);
 34             Node end = new Node(x2, y2, null);
 35             openList.Add(start);
 36             List<Node> resultList = Search(start, end);
 37             if (resultList.Count == 0)
 38             {
 39                 return 0;
 40             }
 41 
 42             foreach (Node node in resultList)
 43             {
 44                 map[node.X,node.Y] = -1;
 45             }
 46 
 47             return 1;
 48         }
 49 
 50         private List<Node> Search(Node start, Node end)
 51         {
 52             List<Node> result = new List<Node>();
 53             bool isFind = false;
 54             Node node = null;
 55             openList.Sort(new NodeComparator());
 56 
 57             while (openList.Count > 0)
 58             {
 59                 node = openList[0];
 60                 if (node.X == end.X && node.Y == end.Y)
 61                 {
 62                     isFind = true;
 63                     break;
 64                 }
 65                 //
 66                 if ((node.Y - 1) >= 0)
 67                 {
 68                     CheckPath(node.X, node.Y - 1, node, end, COST_STRAIGHT);
 69                 }
 70                 //
 71                 if ((node.Y + 1) < column)
 72                 {
 73                     CheckPath(node.X, node.Y + 1, node, end, COST_STRAIGHT);
 74                 }
 75                 //
 76                 if ((node.X - 1) >= 0)
 77                 {
 78                     CheckPath(node.X - 1, node.Y, node, end, COST_STRAIGHT);
 79                 }
 80                 //
 81                 if ((node.X + 1) < row)
 82                 {
 83                     CheckPath(node.X + 1, node.Y, node, end, COST_STRAIGHT);
 84                 }
 85                 //左上
 86                 if ((node.X - 1) >= 0 && (node.Y - 1) >= 0)
 87                 {
 88                     CheckPath(node.X - 1, node.Y - 1, node, end, COST_DIAGONAL);
 89                 }
 90                 //左下
 91                 if ((node.X - 1) >= 0 && (node.Y + 1) < column)
 92                 {
 93                     CheckPath(node.X - 1, node.Y + 1, node, end, COST_DIAGONAL);
 94                 }
 95                 //右上
 96                 if ((node.X + 1) < row && (node.Y - 1) >= 0)
 97                 {
 98                     CheckPath(node.X + 1, node.Y - 1, node, end, COST_DIAGONAL);
 99                 }
100                 //右下
101                 if ((node.X + 1) < row && (node.Y + 1) < column)
102                 {
103                     CheckPath(node.X + 1, node.Y + 1, node, end, COST_DIAGONAL);
104                 }
105 
106                 openList.Remove(node);
107                 closeList.Add(node);
108             }
109             if (isFind) {
110                 GetPath(result, node);
111             }
112 
113             return result;
114         }
115 
116         private bool CheckPath(int x, int y, Node parentNode, Node end, int cost)
117         {
118             Node node = new Node(x, y, parentNode);
119              if (map[x,y] == 0)
120             {
121                 closeList.Add(node);
122                 return false;
123             }
124             if (IsListContains(closeList, x, y) != -1)
125             {
126                 return false;
127             }
128             int index = -1;
129             if ((index = IsListContains(openList, x, y)) != -1)
130             {
131                 if ((parentNode.G + cost) < openList[index].G)
132                 {
133                     node.parentNode = parentNode;
134                     CountG(node, cost);
135                     CountF(node);
136                     openList[index] = node;
137                 }
138             }
139             else {
140                 node.parentNode = parentNode;
141                 Count(node, end, cost);
142                 openList.Add(node);
143             }
144 
145             return true;
146         }
147 
148         private int IsListContains(List<Node> list, int x, int y)
149         {
150             int i = 0;
151             foreach (Node node in list)
152             {
153                 if (node.X == x && node.Y == y)
154                 {
155                     return i;
156                 }
157                 i += 1;
158             }
159 
160             return -1;
161         }
162 
163         private void GetPath(List<Node> list, Node node)
164         {
165             while (node.parentNode != null)
166             {
167                 list.Add(node);
168                 node = node.parentNode;
169             }
170 
171             list.Add(node);
172 
173         }
174 
175         private void Count(Node node, Node end, int cost)
176         {
177             CountG(node, cost);
178             CountH(node, end);
179             CountF(end);
180         }
181 
182         private void CountG(Node node, int cost)
183         {
184             if (node.parentNode == null)
185             {
186                 node.G = cost;
187             }
188             else
189                 node.G = node.parentNode.G + cost;
190         }
191 
192         private void CountH(Node node, Node end)
193         {
194             node.H = Math.Abs(node.X - end.X) + Math.Abs(node.Y - end.Y);
195         }
196 
197         private void CountF(Node node)
198         {
199             node.F = node.G + node.H;
200         }
201     }
AStar

 

 1 class Program
 2     {
 3         delegate void FuncDelegate();
 4 
 5         static void Measure(FuncDelegate func, string funcName)
 6         {
 7             Stopwatch sw = new Stopwatch();
 8             sw.Start();
 9             func();
10             sw.Stop();
11             Console.WriteLine(" {0} used {1} ms", funcName, sw.ElapsedMilliseconds);
12         }
13 
14         static void DrawTheMap(int[,] map)
15         {
16             for (int x = 0; x < 7; x++)
17             {
18                 for (int y = 0; y < 10; y++)
19                 {
20                     if (map[x, y] == 1)
21                     {
22                         Console.Write("");
23                     }
24                     else if (map[x, y] == 0)
25                     {
26                         Console.Write("");
27                     }
28                     else if (map[x, y] == -1)
29                     {
30                         Console.Write("");
31                     }
32                 }
33                 Console.WriteLine("");
34             }
35         }
36 
37         static void AstarAlgorim()
38         {
39             int[,] map = new int[,] {
40                 {1,1,1,1,1,1,1,0,1,1},
41                 {1,1,1,1,0,1,1,0,1,1},
42                 {1,1,1,1,0,1,1,0,1,1},
43                 {1,1,1,1,0,1,1,0,1,1},
44                 {1,1,1,1,0,1,1,0,1,1},
45                 {1,1,1,1,0,1,1,0,1,1},
46                 {1,1,1,1,0,1,1,1,1,1},
47             };
48 
49             AStar star = new AStar(map, 7, 10);
50             int flag = star.Search(1, 0, 6, 8);
51             if (flag == -1)
52             {
53                 Console.WriteLine("传输数据有误!");
54             }
55             else if (flag == 0)
56             {
57                 Console.WriteLine("没有找到!");
58             }
59             else
60             {
61                 DrawTheMap(map);
62             }
63         }
64 
65         static void Main(string[] args)
66         {
67             Measure(AstarAlgorim, "A星寻路");
68 
69             Console.ReadLine();
70         }
71     }
Program

 

posted @ 2014-02-11 17:07  Kurodo  阅读(709)  评论(1编辑  收藏  举报