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 /// starting point 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 /// starting point 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 /// starting point 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 /// starting point 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>();
132 cell = cell.Parent;
133 if (cell != null) for (; cell.Point != start; cell = cell.Parent) list.Add(cell.Point);
135 list.Reverse();
136 return list;
137 }
139 List<Point> neighbors = new List<Point>(8);
140 neighbors.Add(new Point(cell.Point.X, cell.Point.Y - 1));
141 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y));
142 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y));
143 neighbors.Add(new Point(cell.Point.X, cell.Point.Y + 1));
144 #if ALLOW_DIAGONAL_MOVEMENT
145 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y - 1));
146 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y - 1));
147 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 1));
148 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 1));
149 #endif
150 foreach (Point point in neighbors)
151 {
152 if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight &&
153 mGrid[point.X, point.Y])
154 {
155 int cost = cell.G + costFunction(cell.Point, point);
157 Cell inQueue = mCells[point.X, point.Y];
158 if (inQueue == null)
159 {
160 Cell neighbor = new Cell(point, cost, heuristic(point, finish), cell);
162 mCells[point.X, point.Y] = neighbor;
163 }
164 else if (inQueue.IsOpen && cost < inQueue.G)
165 {
166 inQueue.G = cost;
167 inQueue.Parent = cell;
168 mFringe.Promote(inQueue);
169 }
170 }
171 }
172 }
174 return null;
175 }
178 /// <summary>
179 /// Get the manhattan distance between two points. This is a simple but
180 /// effective and commonly-used heuristic.
181 /// </summary>
182 /// <param name="a">A point.</param>
183 /// <param name="b">Another point.</param>
184 /// <returns>The manhattan distance.</returns>
185 public static int GetManhattanDistance(Point a, Point b)
186 {
187 int w = b.X - a.X;
188 int h = b.Y - a.Y;
189 if (w < 0) w = -w;
190 if (h < 0) h = -h;
191 return w + h;
192 }
194 /// <summary>
195 /// Get the cost to travel from one point to another. This is a simple
196 /// cost function based purely on distance. On a square grid, diagonal
197 /// cells are further away than adjacent cells; therefore, adjacent moves
198 /// are favored.
199 /// </summary>
200 /// <param name="a">A point.</param>
201 /// <param name="b">Another point.</param>
202 /// <returns>The cost.</returns>
203 public static int GetCost(Point a, Point b)
204 {
205 if (a.X != b.X && a.Y != b.Y) return 14;
206 return 10;
207 }
209 #endregion
212 #region Private Types
214 class Cell : IComparable<Cell>
215 {
216 public Point Point;
217 public Cell Parent;
218 public bool IsOpen;
220 public int G
221 {
222 get { return mG; }
223 set { mG = value; mF = mG + mH; }
224 }
226 public int H
227 {
228 get { return mH; }
229 set { mH = value; mF = mG + mH; }
230 }
232 public int F { get { return mF; } }
235 public Cell(Point point, int g, int h)
236 {
237 Point = point;
238 IsOpen = true;
239 mG = g;
240 mH = h;
241 mF = g + h;
242 }
244 public Cell(Point point, int g, int h, Cell parent)
245 {
246 Point = point;
247 Parent = parent;
248 IsOpen = true;
249 mG = g;
250 mH = h;
251 mF = g + h;
252 }
254 public int CompareTo(Cell other)
255 {
256 return F - other.F;
257 }
260 int mG;
261 int mH;
262 int mF;
263 }
265 #endregion
268 #region Private Variables
270 bool[,] mGrid;
271 int mGridWidth;
272 int mGridHeight;
274 IPriorityQueue<Cell> mFringe;
275 Cell[,] mCells;
277 #endregion
278 }
279 }