git-svn-id: https://bd85.net/svn/cs3505_group@152 92bb83a3-7c8f-8a45-bc97-515c4e399668
[chaz/carfire] / CarFire / CarFire / CarFire / PathFinder.cs
1 
2 // Uncomment this to disable diagonal movemet.
3 //#define ALLOW_DIAGONAL_MOVEMENT
4
5 using System;
6 using System.Collections.Generic;
7 using System.Linq;
8 using System.Text;
9 using System.Diagnostics;
10 using Microsoft.Xna.Framework;
11
12 namespace CarFire
13 {
14 /// <summary>
15 /// A class to navigate from here to there through a grid of
16 /// open and closed cells.
17 /// </summary>
18 public class PathFinder
19 {
20 #region Public Types
21
22 /// <summary>
23 /// The heuristic function should return some value representing
24 /// the distance between two points. A common approach is to
25 /// return the manhattan distance.
26 /// </summary>
27 /// <param name="a">A point.</param>
28 /// <param name="b">The endpoint.</param>
29 /// <returns>The heuristic.</returns>
30 public delegate int Heuristic(Point a, Point b);
31
32 /// <summary>
33 /// The cost function should take two points representing two
34 /// adjacent cells and return a cost measure of how expensive it
35 /// is to move from one cell to the other.
36 /// </summary>
37 /// <param name="a">A point.</param>
38 /// <param name="b">Another point.</param>
39 /// <returns>The cost.</returns>
40 public delegate int CostFunction(Point a, Point b);
41
42 #endregion
43
44
45 #region Public Methods
46
47 /// <summary>
48 /// Construct a path finder with a grid. The grid is a matrix
49 /// of boolean values, true meaning the cell is walkable and false
50 /// meaning the cell is closed.
51 /// </summary>
52 /// <param name="grid">The grid to find paths on.</param>
53 public PathFinder(bool[,] grid)
54 {
55 Debug.Assert(grid != null);
56
57 mGrid = grid;
58 mGridWidth = mGrid.GetUpperBound(0) + 1;
59 mGridHeight = mGrid.GetUpperBound(1) + 1;
60 }
61
62
63 /// <summary>
64 /// The A* algorithm for finding the best path through a grid of cells.
65 /// The manhattan distance heuristic and a simple distance-based cost
66 /// function will be used.
67 /// </summary>
68 /// <param name="start">The cell to start at.</param>
69 /// <param name="finish">The desired destination.</param>
70 /// <returns>A list of points representing the path through the grid,
71 /// ends points not included, or null if no path could be found.</return>
72 public List<Point> GetPath(Point start, Point finish)
73 {
74 return GetPath(start, finish, GetManhattanDistance, GetCost);
75 }
76
77 /// <summary>
78 /// The A* algorithm for finding the best path through a grid of cells.
79 /// A simple distance-based cost function will be used.
80 /// </summary>
81 /// <param name="start">The cell to start at.</param>
82 /// <param name="finish">The desired destination.</param>
83 /// <param name="heuristic">The heuristic function.</param>
84 /// <returns>A list of points representing the path through the grid,
85 /// ends points not included, or null if no path could be found.</return>
86 public List<Point> GetPath(Point start, Point finish, Heuristic heuristic)
87 {
88 return GetPath(start, finish, heuristic, GetCost);
89 }
90
91 /// <summary>
92 /// The A* algorithm for finding the best path through a grid of cells.
93 /// The manhattan distance heuristic will be used.
94 /// </summary>
95 /// <param name="start">The cell to start at.</param>
96 /// <param name="finish">The desired destination.</param>
97 /// <param name="costFunction">The cost function</param>
98 /// <returns>A list of points representing the path through the grid,
99 /// ends points not included, or null if no path could be found.</return>
100 public List<Point> GetPath(Point start, Point finish, CostFunction costFunction)
101 {
102 return GetPath(start, finish, GetManhattanDistance, costFunction);
103 }
104
105 /// <summary>
106 /// The A* algorithm for finding the best path through a grid of cells.
107 /// </summary>
108 /// <param name="start">The cell to start at.</param>
109 /// <param name="finish">The desired destination.</param>
110 /// <param name="heuristic">The heuristic function.</param>
111 /// <param name="costFunction">The cost function.</param>
112 /// <returns>A list of points representing the path through the grid,
113 /// ends points not included, or null if no path could be found.</return>
114 public List<Point> GetPath(Point start, Point finish, Heuristic heuristic, CostFunction costFunction)
115 {
116 mFringe = new BinaryHeap<Cell>();
117 mCells = new Cell[mGridWidth, mGridHeight];
118
119 Cell startCell = new Cell(start, 0, heuristic(start, finish));
120 mFringe.Add(startCell);
121 mCells[start.X, start.Y] = startCell;
122 while (mFringe.Count > 0)
123 {
124 Cell cell = mFringe.GetNext();
125 cell.IsOpen = false;
126
127 if (cell.Point == finish)
128 {
129 List<Point> list = new List<Point>();
130
131 cell = cell.Parent;
132 while (cell != null && cell.Point != start)
133 {
134 list.Add(cell.Point);
135 cell = cell.Parent;
136 }
137
138 list.Reverse();
139 return list;
140 }
141
142 List<Point> neighbors = new List<Point>(8);
143 neighbors.Add(new Point(cell.Point.X, cell.Point.Y - 1));
144 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y));
145 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y));
146 neighbors.Add(new Point(cell.Point.X, cell.Point.Y + 1));
147 #if ALLOW_DIAGONAL_MOVEMENT
148 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y - 1));
149 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y - 1));
150 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 1));
151 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 1));
152 #endif
153 foreach (Point point in neighbors)
154 {
155 if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight &&
156 mGrid[point.X, point.Y])
157 {
158 int cost = cell.G + costFunction(cell.Point, point);
159
160 Cell inQueue = mCells[point.X, point.Y];
161 if (inQueue == null)
162 {
163 Cell neighbor = new Cell(point, cost, heuristic(point, finish), cell);
164 mFringe.Add(neighbor);
165 mCells[point.X, point.Y] = neighbor;
166 }
167 else if (inQueue.IsOpen && cost < inQueue.G)
168 {
169 inQueue.G = cost;
170 inQueue.Parent = cell;
171 mFringe.Promote(inQueue);
172 }
173 }
174 }
175 }
176
177 return null;
178 }
179
180
181 /// <summary>
182 /// Get the manhattan distance between two points. This is a simple but
183 /// effective and commonly-used heuristic.
184 /// </summary>
185 /// <param name="a">A point.</param>
186 /// <param name="b">Another point.</param>
187 /// <returns>The manhattan distance.</returns>
188 public static int GetManhattanDistance(Point a, Point b)
189 {
190 int w = b.X - a.X;
191 int h = b.Y - a.Y;
192 if (w < 0) w = -w;
193 if (h < 0) h = -h;
194 return w + h;
195 }
196
197 /// <summary>
198 /// Get the cost to travel from one point to another. This is a simple
199 /// cost function based purely on distance. On a square grid, diagonal
200 /// cells are further away than adjacent cells; therefore, adjacent moves
201 /// are favored.
202 /// </summary>
203 /// <param name="a">A point.</param>
204 /// <param name="b">Another point.</param>
205 /// <returns>The cost.</returns>
206 public static int GetCost(Point a, Point b)
207 {
208 if (a.X != b.X && a.Y != b.Y) return 14;
209 return 10;
210 }
211
212 #endregion
213
214
215 #region Private Types
216
217 class Cell : IComparable<Cell>
218 {
219 public Point Point;
220 public Cell Parent;
221 public bool IsOpen;
222
223 public int G
224 {
225 get { return mG; }
226 set { mG = value; mF = mG + mH; }
227 }
228
229 public int H
230 {
231 get { return mH; }
232 set { mH = value; mF = mG + mH; }
233 }
234
235 public int F { get { return mF; } }
236
237
238 public Cell(Point point, int g, int h)
239 {
240 Point = point;
241 IsOpen = true;
242 mG = g;
243 mH = h;
244 mF = g + h;
245 }
246
247 public Cell(Point point, int g, int h, Cell parent)
248 {
249 Point = point;
250 Parent = parent;
251 IsOpen = true;
252 mG = g;
253 mH = h;
254 mF = g + h;
255 }
256
257 public int CompareTo(Cell other)
258 {
259 return F - other.F;
260 }
261
262
263 int mG;
264 int mH;
265 int mF;
266 }
267
268 #endregion
269
270
271 #region Private Variables
272
273 bool[,] mGrid;
274 int mGridWidth;
275 int mGridHeight;
276
277 IPriorityQueue<Cell> mFringe;
278 Cell[,] mCells;
279
280 #endregion
281 }
282 }
This page took 0.05627 seconds and 4 git commands to generate.