2 // Uncomment this to disable diagonal movemet.
3 //#define ALLOW_DIAGONAL_MOVEMENT
6 using System.Collections.Generic;
9 using System.Diagnostics;
10 using System.Threading;
11 using Microsoft.Xna.Framework;
16 /// A class to navigate from here to there through a grid of
17 /// open and closed cells.
19 public class PathFinder
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.
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);
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.
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);
45 /// An async task object to manage a search task that has been
46 /// put on the thread pool.
48 public class AsyncTask
51 /// Construct an async task object.
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)
63 mHeuristic = heuristic;
64 mCostFunction = costFunction;
69 /// Determine whether or not the task has completed.
71 public bool IsCompleted { get { return mIsDone; } }
74 /// Get the resulting path.
76 public List<Point> Path { get { return mPath; } }
79 #region Private Members
81 public void Run(object context)
83 mPath = mPathFinder.GetPath(mStart, mFinish, mHeuristic, mCostFunction);
88 PathFinder mPathFinder;
92 CostFunction mCostFunction;
103 #region Public Methods
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.
110 /// <param name="grid">The grid to find paths on.</param>
111 public PathFinder(bool[,] grid)
113 Debug.Assert(grid != null);
116 mGridWidth = mGrid.GetUpperBound(0) + 1;
117 mGridHeight = mGrid.GetUpperBound(1) + 1;
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.
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)
132 return GetPath(start, finish, GetManhattanDistance, GetCost);
136 /// The A* algorithm for finding the best path through a grid of cells.
137 /// A simple distance-based cost function will be used.
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)
146 return GetPath(start, finish, heuristic, GetCost);
150 /// The A* algorithm for finding the best path through a grid of cells.
151 /// The manhattan distance heuristic will be used.
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)
160 return GetPath(start, finish, GetManhattanDistance, costFunction);
164 /// The A* algorithm for finding the best path through a grid of cells.
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)
174 mFringe = new BinaryHeap<Cell>();
175 mCells = new Cell[mGridWidth, mGridHeight];
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)
182 Cell cell = mFringe.GetNext();
185 if (cell.Point == finish)
187 List<Point> list = new List<Point>();
188 list.Add(cell.Point);
191 if (cell != null) for (; cell.Point != start; cell = cell.Parent) list.Add(cell.Point);
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));
208 foreach (Point point in neighbors)
210 if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight &&
211 mGrid[point.X, point.Y])
213 int cost = cell.G + costFunction(cell.Point, point);
215 Cell inQueue = mCells[point.X, point.Y];
218 Cell neighbor = new Cell(point, cost, heuristic(point, finish), cell);
219 mFringe.Add(neighbor);
220 mCells[point.X, point.Y] = neighbor;
222 else if (inQueue.IsOpen && cost < inQueue.G)
225 inQueue.Parent = cell;
226 mFringe.Promote(inQueue);
237 /// Get a shortest path by putting the task on the thread pool.
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)
244 AsyncTask task = new AsyncTask(this, start, finish, GetManhattanDistance, GetCost);
245 ThreadPool.QueueUserWorkItem(new WaitCallback(task.Run));
251 /// Find the closest open cell that is near another cell.
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)
258 if (0 <= coordinates.X && coordinates.X < mGridWidth && 0 <= coordinates.Y && coordinates.Y < mGridHeight &&
259 mGrid[coordinates.X, coordinates.Y])
264 mFringe = new BinaryHeap<Cell>();
265 mCells = new Cell[mGridWidth, mGridHeight];
267 Cell startCell = new Cell(coordinates, 0, 0);
268 mFringe.Add(startCell);
269 mCells[coordinates.X, coordinates.Y] = startCell;
270 while (mFringe.Count > 0)
272 Cell cell = mFringe.GetNext();
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)
285 if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight)
287 if (mGrid[point.X, point.Y])
293 int cost = cell.G + GetCost(cell.Point, point);
295 Cell inQueue = mCells[point.X, point.Y];
298 Cell neighbor = new Cell(point, cost, 0);
299 mFringe.Add(neighbor);
300 mCells[point.X, point.Y] = neighbor;
312 /// Get the manhattan distance between two points. This is a simple but
313 /// effective and commonly-used heuristic.
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)
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
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)
338 if (a.X != b.X && a.Y != b.Y) return 14;
345 #region Private Types
347 class Cell : IComparable<Cell>
356 set { mG = value; mF = mG + mH; }
362 set { mH = value; mF = mG + mH; }
365 public int F { get { return mF; } }
368 public Cell(Point point, int g, int h)
377 public Cell(Point point, int g, int h, Cell parent)
387 public int CompareTo(Cell other)
401 #region Private Variables
407 IPriorityQueue<Cell> mFringe;