2 // Uncomment this to disable diagonal movemet.
3 //#define ALLOW_DIAGONAL_MOVEMENT
6 using System.Collections.Generic;
9 using System.Diagnostics;
10 using Microsoft.Xna.Framework;
15 /// A class to navigate from here to there through a grid of
16 /// open and closed cells.
18 public class PathFinder
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.
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);
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.
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);
45 #region Public Methods
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.
52 /// <param name="grid">The grid to find paths on.</param>
53 public PathFinder(bool[,] grid)
55 Debug.Assert(grid != null);
58 mGridWidth = mGrid.GetUpperBound(0) + 1;
59 mGridHeight = mGrid.GetUpperBound(1) + 1;
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.
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)
74 return GetPath(start, finish, GetManhattanDistance, GetCost);
78 /// The A* algorithm for finding the best path through a grid of cells.
79 /// A simple distance-based cost function will be used.
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)
88 return GetPath(start, finish, heuristic, GetCost);
92 /// The A* algorithm for finding the best path through a grid of cells.
93 /// The manhattan distance heuristic will be used.
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)
102 return GetPath(start, finish, GetManhattanDistance, costFunction);
106 /// The A* algorithm for finding the best path through a grid of cells.
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)
116 mFringe = new BinaryHeap<Cell>();
117 mCells = new Cell[mGridWidth, mGridHeight];
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)
124 Cell cell = mFringe.GetNext();
127 if (cell.Point == finish)
129 List<Point> list = new List<Point>();
132 while (cell.Point != start)
134 list.Add(cell.Point);
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));
153 foreach (Point point in neighbors)
155 Cell inQueue = mCells[point.X, point.Y];
157 if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight &&
158 mGrid[point.X, point.Y])
160 int cost = cell.G + costFunction(cell.Point, point);
164 Cell neighbor = new Cell(point, cost, heuristic(point, finish), cell);
165 mFringe.Add(neighbor);
166 mCells[point.X, point.Y] = neighbor;
168 else if (inQueue.IsOpen && cost < inQueue.G)
171 inQueue.Parent = cell;
172 mFringe.Promote(inQueue);
183 /// Get the manhattan distance between two points. This is a simple but
184 /// effective and commonly-used heuristic.
186 /// <param name="a">A point.</param>
187 /// <param name="b">Another point.</param>
188 /// <returns>The manhattan distance.</returns>
189 public static int GetManhattanDistance(Point a, Point b)
199 /// Get the cost to travel from one point to another. This is a simple
200 /// cost function based purely on distance. On a square grid, diagonal
201 /// cells are further away than adjacent cells; therefore, adjacent moves
204 /// <param name="a">A point.</param>
205 /// <param name="b">Another point.</param>
206 /// <returns>The cost.</returns>
207 public static int GetCost(Point a, Point b)
209 if (a.X != b.X && a.Y != b.Y) return 14;
216 #region Private Types
218 class Cell : IComparable<Cell>
227 set { mG = value; mF = mG + mH; }
233 set { mH = value; mF = mG + mH; }
236 public int F { get { return mF; } }
239 public Cell(Point point, int g, int h)
248 public Cell(Point point, int g, int h, Cell parent)
258 public int CompareTo(Cell other)
272 #region Private Variables
278 IPriorityQueue<Cell> mFringe;