1 ﻿
2 // Uncomment this to disable diagonal movemet.
3 //#define ALLOW_DIAGONAL_MOVEMENT
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;
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
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);
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);
42 #endregion
45 #region Public Methods
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);
57 mGrid = grid;
58 mGridWidth = mGrid.GetUpperBound(0) + 1;
59 mGridHeight = mGrid.GetUpperBound(1) + 1;
60 }
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 }
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 }
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 }
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];
119 Cell startCell = new Cell(start, 0, heuristic(start, finish));
121 mCells[start.X, start.Y] = startCell;
122 while (mFringe.Count > 0)
123 {
124 Cell cell = mFringe.GetNext();
125 cell.IsOpen = false;
127 if (cell.Point == finish)
128 {
129 List<Point> list = new List<Point>();
131 cell = cell.Parent;
132 while (cell != null && cell.Point != start)
133 {
135 cell = cell.Parent;
136 }
138 list.Reverse();
139 return list;
140 }
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 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])
159 {
160 int cost = cell.G + costFunction(cell.Point, point);
162 if (inQueue == null)
163 {
164 Cell neighbor = new Cell(point, cost, heuristic(point, finish), cell);
166 mCells[point.X, point.Y] = neighbor;
167 }
168 else if (inQueue.IsOpen && cost < inQueue.G)
169 {
170 inQueue.G = cost;
171 inQueue.Parent = cell;
172 mFringe.Promote(inQueue);
173 }
174 }
175 }
176 }
178 return null;
179 }
182 /// <summary>
183 /// Get the manhattan distance between two points. This is a simple but
184 /// effective and commonly-used heuristic.
185 /// </summary>
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)
190 {
191 int w = b.X - a.X;
192 int h = b.Y - a.Y;
193 if (w < 0) w = -w;
194 if (h < 0) h = -h;
195 return w + h;
196 }
198 /// <summary>
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
202 /// are favored.
203 /// </summary>
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)
208 {
209 if (a.X != b.X && a.Y != b.Y) return 14;
210 return 10;
211 }
213 #endregion
216 #region Private Types
218 class Cell : IComparable<Cell>
219 {
220 public Point Point;
221 public Cell Parent;
222 public bool IsOpen;
224 public int G
225 {
226 get { return mG; }
227 set { mG = value; mF = mG + mH; }
228 }
230 public int H
231 {
232 get { return mH; }
233 set { mH = value; mF = mG + mH; }
234 }
236 public int F { get { return mF; } }
239 public Cell(Point point, int g, int h)
240 {
241 Point = point;
242 IsOpen = true;
243 mG = g;
244 mH = h;
245 mF = g + h;
246 }
248 public Cell(Point point, int g, int h, Cell parent)
249 {
250 Point = point;
251 Parent = parent;
252 IsOpen = true;
253 mG = g;
254 mH = h;
255 mF = g + h;
256 }
258 public int CompareTo(Cell other)
259 {
260 return F - other.F;
261 }
264 int mG;
265 int mH;
266 int mF;
267 }
269 #endregion
272 #region Private Variables
274 bool[,] mGrid;
275 int mGridWidth;
276 int mGridHeight;
278 IPriorityQueue<Cell> mFringe;
279 Cell[,] mCells;
281 #endregion
282 }
283 }