A*算法

1. //定义格子的枚举类型  
2. public enum GridType  
3. {  
4.   
5.     Normal, Obstacle, Start, End  
6. }  

[csharp]  view plain  copy

1. 定义格子类  
2. public class Grid : IComparable  
3. {  
4.     public int x;  
5.     public int y;  
6.   
7.     public int f;//总消耗  G+H  
8.     public int g;//到起点消耗  
9.     public int h;//到终点消耗  
10.   
11.     public GridType type;  
12.     public Grid fatherNode;  
13.   
14.     //重写CompareTo 方法，用于取出F值最小的格子  
15.     public int CompareTo(object obj)  
16.     {  
17.         Grid grid = (Grid)obj;  
18.         if (this.f < grid.f)  
19.         {  
20.             return -1;  
21.         }  
22.         if (this.f > grid.f)  
23.         {  
24.             return 1;  
25.         }  
26.         return 0;  
27.     }  
28. }  

[csharp]  view plain  copy

1. public class AStar : MonoBehaviour  
2. {  
3.   
4.     //设置行列数和格子大小  
5.     public int row = 5;  
6.     public int col = 10;  
7.     public int size = 70;  
8.   
9.     public Grid[,] grids;  
10.   
11.     //开数组，闭数组，和寻路路径  
12.     public ArrayList openList;  
13.     public ArrayList closeList;  
14.     public ArrayList lujing;  
15.   
16.     //假定起点和终点X,Y值  
17.     private int xStart = 2;  
18.     private int yStart = 1;  
19.     private int xEnd = 2;  
20.     private int yEnd = 5;  
21.   
22.     //初始化寻路格子  
23.     void Init()  
24.     {  
25.         grids = new Grid[row, col];  
26.         for (int i = 0; i < row; i++)  
27.         {  
28.             for (int j = 0; j < col; j++)  
29.             {  
30.                 grids[i, j] = new Grid();  
31.                 grids[i, j].x = i;  
32.                 grids[i, j].y = j;  
33.             }  
34.         }  
35.         //起始点  
36.         grids[xStart, yStart].type = GridType.Start;  
37.         grids[xStart, yStart].h = Manhattan(xStart, yStart);  
38.         //结束点  
39.         grids[xEnd, yEnd].type = GridType.End;  
40.   
41.         //设置障碍物  
42.         for (int i = 1; i <= 3; i++)  
43.         {  
44.             grids[i, 3].type = GridType.Obstacle;  
45.         }  
46.   
47.         closeList = new ArrayList();  
48.         lujing = new ArrayList();  
49.         openList = new ArrayList();  
50.         openList.Add(grids[xStart, yStart]);  
51.   
52.     }  
53.   
54.     int Manhattan(int x, int y)   //用于计算到终点的H  
55.     {  
56.         return (int)(Mathf.Abs(xEnd - x) + Mathf.Abs(yEnd - y)) * 10;  
57.     }  
58.     void Start()  
59.     {  
60.         Init();  
61.     }  
62.     void Update()  
63.     {  
64.   
65.     }  
66.     //绘制格子便于观察  
67.     void DrawGrid()  
68.     {  
69.         for (int i = 0; i < row; i++)  
70.         {  
71.             for (int j = 0; j < col; j++)  
72.             {  
73.                 Color color;  
74.                 if (grids[i, j].type == GridType.Start)  
75.                 {  
76.                     color = Color.green;  
77.                 }  
78.                 else if (grids[i, j].type == GridType.End)  
79.                 {  
80.                     color = Color.red;  
81.                 }  
82.                 else if (grids[i, j].type == GridType.Obstacle)  
83.                 {  
84.                     color = Color.blue;  
85.                 }  
86.                 else if (closeList.Contains(grids[i, j]))  
87.                 {  
88.                     color = Color.yellow;  
89.                 }  
90.                 else  
91.                 {  
92.                     color = Color.gray;  
93.                 }  
94.                 GUI.backgroundColor = color;  
95.                 GUI.Button(new Rect(j * size, i * size, size, size), FGH(grids[i, j]));  
96.             }  
97.         }  
98.     }  
99.   
100.     //显示F G H值  
101.     string FGH(Grid grid)  
102.     {  
103.         string str = "F" + " " + grid.f + "\n";  
104.         str += "G" + " " + grid.g + "\n";  
105.         str += "H" + " " + grid.h + "\n";  
106.         str += "(" + grid.x + "," + grid.y + ")";  
107.         return str;  
108.     }  
109.     void OnGUI()  
110.     {  
111.         DrawGrid();  
112.         //显示出开数组，闭数组和行走路径  
113.         for (int i = 0; i < openList.Count; i++)  
114.         {  
115.             GUI.Button(new Rect(i * size, (row + 1) * size, size, size),FGH((Grid)openList[i]));  
116.         }  
117.         for (int i = 0; i < closeList.Count; i++)  
118.         {  
119.             GUI.Button(new Rect(i * size, (row + 2) * size, size, size),FGH((Grid)closeList[i]));  
120.         }  
121.         for (int i = 0; i < lujing.Count; i++)  
122.         {  
123.             GUI.Button(new Rect(i * size, (row + 3) * size, size, size),FGH((Grid)lujing[i]));  
124.         }  
125.   
126.         if (GUI.Button(new Rect(col * size, size, size, size), "NEXT"))  
127.         {  
128.             NextStep();//一步一步走  
129.         }  
130.     }  
131.   
132.     //获取每个格子的父节点，用于从终点往回查询路径  
133.     void getFatherNode(Grid grid)  
134.     {  
135.         if (grid.fatherNode != null)  
136.         {  
137.             lujing.Add(grid.fatherNode);  
138.             getFatherNode(grid.fatherNode);  
139.             // return getFatherNode(grid.fatherNode);  
140.         }  
141.         //else  
142.         //lujing.Add(grid);  
143.         //return grid;  
144.     }  
145.     void NextStep()  
146.     {  
147.         //当开数组为空时，寻路结束  
148.         if (openList.Count == 0)  
149.         {  
150.             print("Over!");  
151.   
152.             return;  
153.         }  
154.         //把F值最小的grid取出来  
155.         Grid grid = (Grid)openList[0];  
156.         //当寻到终点的时候  
157.         if (grid.type == GridType.End)  
158.         {  
159.             print("Over");  
160.             lujing.Add(grid);  
161.             //打印路线  
162.             getFatherNode(grid);  
163.             //因为路径是从后往前查找父节点，所以需要reverse则为正确路径  
164.             lujing.Reverse();  
165.             return;  
166.         }  
167.         //遍历处自己外的周围8个格子  
168.         for (int i = -1; i <= 1; i++)  
169.         {  
170.             for (int j = -1; j <= 1; j++)  
171.             {  
172.                 //如果只想遍历上下左右4个格子可加个条件 (i==0||j==0)  
173.                 if (!(i == 0 && j == 0))  
174.                 {  
175.                     int x = grid.x + i;  
176.                     int y = grid.y + j;  
177.                     //不超过边界，不是障碍物，不在closeList数组里  
178.                     if (x >= 0 && x < row && y >= 0 && y < col && grids[x, y].type != GridType.Obstacle && !closeList.Contains(grids[x, y]))  
179.                     {  
180.                         //计算G  
181.                         int g = grid.g + (int)(Mathf.Sqrt(Mathf.Abs(i) + Mathf.Abs(j)) * 10);  
182.                         if (grids[x, y].g == 0 || grids[x, y].g > g)  
183.                         {  
184.                             grids[x, y].g = g;  
185.                             grids[x, y].fatherNode = grid;  
186.                         }  
187.                         grids[x, y].h = Manhattan(x, y);  
188.                         grids[x, y].f = grids[x, y].g + grids[x, y].h;  
189.                         if (!openList.Contains(grids[x, y]))  
190.                         {  
191.                             openList.Add(grids[x, y]);  
192.                         }  
193.                         //重新从小到大排序;  
194.                         openList.Sort();  
195.                     }  
196.                 }  
197.             }  
198.         }  
199.   
200.         //走过的点关闭  
201.         closeList.Add(grid);  
202.         //关闭的点从OPEN里移除  
203.         openList.Remove(grid);  
204.     }