new PathFinder.GetPathAsync to push path finding onto the thread pool; new PathFinder...
[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 System.Threading;
11 using Microsoft.Xna.Framework;
12
13 namespace CarFire
14 {
15 /// <summary>
16 /// A class to navigate from here to there through a grid of
17 /// open and closed cells.
18 /// </summary>
19 public class PathFinder
20 {
21 #region Public Types
22
23 /// <summary>
24 /// The heuristic function should return some value representing
25 /// the distance between two points. A common approach is to
26 /// return the manhattan distance.
27 /// </summary>
28 /// <param name="a">A point.</param>
29 /// <param name="b">The endpoint.</param>
30 /// <returns>The heuristic.</returns>
31 public delegate int Heuristic(Point a, Point b);
32
33 /// <summary>
34 /// The cost function should take two points representing two
35 /// adjacent cells and return a cost measure of how expensive it
36 /// is to move from one cell to the other.
37 /// </summary>
38 /// <param name="a">A point.</param>
39 /// <param name="b">Another point.</param>
40 /// <returns>The cost.</returns>
41 public delegate int CostFunction(Point a, Point b);
42
43
44 /// <summary>
45 /// An async task object to manage a search task that has been
46 /// put on the thread pool.
47 /// </summary>
48 public class AsyncTask
49 {
50 /// <summary>
51 /// Construct an async task object.
52 /// </summary>
53 /// <param name="finder">A path finder.</param>
54 /// <param name="start">The cell to start at.</param>
55 /// <param name="finish">The desired destination.</param>
56 /// <param name="heuristic">The heuristic function.</param>
57 /// <param name="costFunction">The cost function.</param>
58 public AsyncTask(PathFinder finder, Point start, Point finish, Heuristic heuristic, CostFunction costFunction)
59 {
60 mPathFinder = finder;
61 mStart = start;
62 mFinish = finish;
63 mHeuristic = heuristic;
64 mCostFunction = costFunction;
65 }
66
67
68 /// <summary>
69 /// Determine whether or not the task has completed.
70 /// </summary>
71 public bool IsCompleted { get { return mIsDone; } }
72
73 /// <summary>
74 /// Get the resulting path.
75 /// </summary>
76 public List<Point> Path { get { return mPath; } }
77
78
79 #region Private Members
80
81 public void Run(object context)
82 {
83 mPath = mPathFinder.GetPath(mStart, mFinish, mHeuristic, mCostFunction);
84 mIsDone = true;
85 }
86
87
88 PathFinder mPathFinder;
89 Point mStart;
90 Point mFinish;
91 Heuristic mHeuristic;
92 CostFunction mCostFunction;
93
94 List<Point> mPath;
95 bool mIsDone;
96
97 #endregion
98 }
99
100 #endregion
101
102
103 #region Public Methods
104
105 /// <summary>
106 /// Construct a path finder with a grid. The grid is a matrix
107 /// of boolean values, true meaning the cell is walkable and false
108 /// meaning the cell is closed.
109 /// </summary>
110 /// <param name="grid">The grid to find paths on.</param>
111 public PathFinder(bool[,] grid)
112 {
113 Debug.Assert(grid != null);
114
115 mGrid = grid;
116 mGridWidth = mGrid.GetUpperBound(0) + 1;
117 mGridHeight = mGrid.GetUpperBound(1) + 1;
118 }
119
120
121 /// <summary>
122 /// The A* algorithm for finding the best path through a grid of cells.
123 /// The manhattan distance heuristic and a simple distance-based cost
124 /// function will be used.
125 /// </summary>
126 /// <param name="start">The cell to start at.</param>
127 /// <param name="finish">The desired destination.</param>
128 /// <returns>A list of points representing the path through the grid,
129 /// starting point not included, or null if no path could be found.</return>
130 public List<Point> GetPath(Point start, Point finish)
131 {
132 return GetPath(start, finish, GetManhattanDistance, GetCost);
133 }
134
135 /// <summary>
136 /// The A* algorithm for finding the best path through a grid of cells.
137 /// A simple distance-based cost function will be used.
138 /// </summary>
139 /// <param name="start">The cell to start at.</param>
140 /// <param name="finish">The desired destination.</param>
141 /// <param name="heuristic">The heuristic function.</param>
142 /// <returns>A list of points representing the path through the grid,
143 /// starting point not included, or null if no path could be found.</return>
144 public List<Point> GetPath(Point start, Point finish, Heuristic heuristic)
145 {
146 return GetPath(start, finish, heuristic, GetCost);
147 }
148
149 /// <summary>
150 /// The A* algorithm for finding the best path through a grid of cells.
151 /// The manhattan distance heuristic will be used.
152 /// </summary>
153 /// <param name="start">The cell to start at.</param>
154 /// <param name="finish">The desired destination.</param>
155 /// <param name="costFunction">The cost function</param>
156 /// <returns>A list of points representing the path through the grid,
157 /// starting point not included, or null if no path could be found.</return>
158 public List<Point> GetPath(Point start, Point finish, CostFunction costFunction)
159 {
160 return GetPath(start, finish, GetManhattanDistance, costFunction);
161 }
162
163 /// <summary>
164 /// The A* algorithm for finding the best path through a grid of cells.
165 /// </summary>
166 /// <param name="start">The cell to start at.</param>
167 /// <param name="finish">The desired destination.</param>
168 /// <param name="heuristic">The heuristic function.</param>
169 /// <param name="costFunction">The cost function.</param>
170 /// <returns>A list of points representing the path through the grid,
171 /// starting point not included, or null if no path could be found.</return>
172 public List<Point> GetPath(Point start, Point finish, Heuristic heuristic, CostFunction costFunction)
173 {
174 mFringe = new BinaryHeap<Cell>();
175 mCells = new Cell[mGridWidth, mGridHeight];
176
177 Cell startCell = new Cell(start, 0, heuristic(start, finish));
178 mFringe.Add(startCell);
179 mCells[start.X, start.Y] = startCell;
180 while (mFringe.Count > 0)
181 {
182 Cell cell = mFringe.GetNext();
183 cell.IsOpen = false;
184
185 if (cell.Point == finish)
186 {
187 List<Point> list = new List<Point>();
188 list.Add(cell.Point);
189
190 cell = cell.Parent;
191 if (cell != null) for (; cell.Point != start; cell = cell.Parent) list.Add(cell.Point);
192
193 list.Reverse();
194 return list;
195 }
196
197 List<Point> neighbors = new List<Point>(8);
198 neighbors.Add(new Point(cell.Point.X, cell.Point.Y - 1));
199 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y));
200 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y));
201 neighbors.Add(new Point(cell.Point.X, cell.Point.Y + 1));
202 #if ALLOW_DIAGONAL_MOVEMENT
203 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y - 1));
204 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y - 1));
205 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 1));
206 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 1));
207 #endif
208 foreach (Point point in neighbors)
209 {
210 if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight &&
211 mGrid[point.X, point.Y])
212 {
213 int cost = cell.G + costFunction(cell.Point, point);
214
215 Cell inQueue = mCells[point.X, point.Y];
216 if (inQueue == null)
217 {
218 Cell neighbor = new Cell(point, cost, heuristic(point, finish), cell);
219 mFringe.Add(neighbor);
220 mCells[point.X, point.Y] = neighbor;
221 }
222 else if (inQueue.IsOpen && cost < inQueue.G)
223 {
224 inQueue.G = cost;
225 inQueue.Parent = cell;
226 mFringe.Promote(inQueue);
227 }
228 }
229 }
230 }
231
232 return null;
233 }
234
235
236 /// <summary>
237 /// Get a shortest path by putting the task on the thread pool.
238 /// </summary>
239 /// <param name="start">The cell to start at.</param>
240 /// <param name="finish">The desired destination.</param>
241 /// <returns>The async task object.</returns>
242 public AsyncTask GetPathAsync(Point start, Point finish)
243 {
244 AsyncTask task = new AsyncTask(this, start, finish, GetManhattanDistance, GetCost);
245 ThreadPool.QueueUserWorkItem(new WaitCallback(task.Run));
246 return task;
247 }
248
249
250 /// <summary>
251 /// Find the closest open cell that is near another cell.
252 /// </summary>
253 /// <param name="coordinates">The coordinates.</param>
254 /// <returns>An open cell at or near the given coordinates,
255 /// or null if no open nearby cell could be found.</returns>
256 public Point? GetNearbyOpenCell(Point coordinates)
257 {
258 if (0 <= coordinates.X && coordinates.X < mGridWidth && 0 <= coordinates.Y && coordinates.Y < mGridHeight &&
259 mGrid[coordinates.X, coordinates.Y])
260 {
261 return coordinates;
262 }
263
264 mFringe = new BinaryHeap<Cell>();
265 mCells = new Cell[mGridWidth, mGridHeight];
266
267 Cell startCell = new Cell(coordinates, 0, 0);
268 mFringe.Add(startCell);
269 mCells[coordinates.X, coordinates.Y] = startCell;
270 while (mFringe.Count > 0)
271 {
272 Cell cell = mFringe.GetNext();
273
274 List<Point> neighbors = new List<Point>(8);
275 neighbors.Add(new Point(cell.Point.X, cell.Point.Y - 1));
276 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y));
277 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y));
278 neighbors.Add(new Point(cell.Point.X, cell.Point.Y + 1));
279 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y - 1));
280 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y - 1));
281 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 1));
282 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 1));
283 foreach (Point point in neighbors)
284 {
285 if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight)
286 {
287 if (mGrid[point.X, point.Y])
288 {
289 return point;
290 }
291 else
292 {
293 int cost = cell.G + GetCost(cell.Point, point);
294
295 Cell inQueue = mCells[point.X, point.Y];
296 if (inQueue == null)
297 {
298 Cell neighbor = new Cell(point, cost, 0);
299 mFringe.Add(neighbor);
300 mCells[point.X, point.Y] = neighbor;
301 }
302 }
303 }
304 }
305 }
306
307 return null;
308 }
309
310
311 /// <summary>
312 /// Get the manhattan distance between two points. This is a simple but
313 /// effective and commonly-used heuristic.
314 /// </summary>
315 /// <param name="a">A point.</param>
316 /// <param name="b">Another point.</param>
317 /// <returns>The manhattan distance.</returns>
318 public static int GetManhattanDistance(Point a, Point b)
319 {
320 int w = b.X - a.X;
321 int h = b.Y - a.Y;
322 if (w < 0) w = -w;
323 if (h < 0) h = -h;
324 return w + h;
325 }
326
327 /// <summary>
328 /// Get the cost to travel from one point to another. This is a simple
329 /// cost function based purely on distance. On a square grid, diagonal
330 /// cells are further away than adjacent cells; therefore, adjacent moves
331 /// are favored.
332 /// </summary>
333 /// <param name="a">A point.</param>
334 /// <param name="b">Another point.</param>
335 /// <returns>The cost.</returns>
336 public static int GetCost(Point a, Point b)
337 {
338 if (a.X != b.X && a.Y != b.Y) return 14;
339 return 10;
340 }
341
342 #endregion
343
344
345 #region Private Types
346
347 class Cell : IComparable<Cell>
348 {
349 public Point Point;
350 public Cell Parent;
351 public bool IsOpen;
352
353 public int G
354 {
355 get { return mG; }
356 set { mG = value; mF = mG + mH; }
357 }
358
359 public int H
360 {
361 get { return mH; }
362 set { mH = value; mF = mG + mH; }
363 }
364
365 public int F { get { return mF; } }
366
367
368 public Cell(Point point, int g, int h)
369 {
370 Point = point;
371 IsOpen = true;
372 mG = g;
373 mH = h;
374 mF = g + h;
375 }
376
377 public Cell(Point point, int g, int h, Cell parent)
378 {
379 Point = point;
380 Parent = parent;
381 IsOpen = true;
382 mG = g;
383 mH = h;
384 mF = g + h;
385 }
386
387 public int CompareTo(Cell other)
388 {
389 return F - other.F;
390 }
391
392
393 int mG;
394 int mH;
395 int mF;
396 }
397
398 #endregion
399
400
401 #region Private Variables
402
403 bool[,] mGrid;
404 int mGridWidth;
405 int mGridHeight;
406
407 IPriorityQueue<Cell> mFringe;
408 Cell[,] mCells;
409
410 #endregion
411 }
412 }
This page took 0.065038 seconds and 4 git commands to generate.