Dogcows Code - chaz/carfire/blob - 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 Microsoft.Xna.Framework;
  11
  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
  21
  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);
  31
  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);
  41
  42         #endregion
  43
  44
  45         #region Public Methods
  46
  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);
  56
  57             mGrid = grid;
  58             mGridWidth = mGrid.GetUpperBound(0) + 1;
  59             mGridHeight = mGrid.GetUpperBound(1) + 1;
  60         }
  61
  62
  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         }
  76
  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         }
  90
  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         }
 104
 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];
 118
 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)
 123             {
 124                 Cell cell = mFringe.GetNext();
 125                 cell.IsOpen = false;
 126
 127                 if (cell.Point == finish)
 128                 {
 129                     List<Point> list = new List<Point>();
 130
 131                     cell = cell.Parent;
 132                     while (cell != null && cell.Point != start)
 133                     {
 134                         list.Add(cell.Point);
 135                         cell = cell.Parent;
 136                     }
 137
 138                     list.Reverse();
 139                     return list;
 140                 }
 141
 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];
 156
 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);
 161
 162                         if (inQueue == null)
 163                         {
 164                             Cell neighbor = new Cell(point, cost, heuristic(point, finish), cell);
 165                             mFringe.Add(neighbor);
 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             }
 177
 178             return null;
 179         }
 180
 181
 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         }
 197
 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         }
 212
 213         #endregion
 214
 215
 216         #region Private Types
 217
 218         class Cell : IComparable<Cell>
 219         {
 220             public Point Point;
 221             public Cell Parent;
 222             public bool IsOpen;
 223
 224             public int G
 225             {
 226                 get { return mG; }
 227                 set { mG = value; mF = mG + mH; }
 228             }
 229
 230             public int H
 231             {
 232                 get { return mH; }
 233                 set { mH = value; mF = mG + mH; }
 234             }
 235
 236             public int F { get { return mF; } }
 237
 238
 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             }
 247
 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             }
 257
 258             public int CompareTo(Cell other)
 259             {
 260                 return F - other.F;
 261             }
 262
 263
 264             int mG;
 265             int mH;
 266             int mF;
 267         }
 268
 269         #endregion
 270
 271
 272         #region Private Variables
 273
 274         bool[,] mGrid;
 275         int mGridWidth;
 276         int mGridHeight;
 277
 278         IPriorityQueue<Cell> mFringe;
 279         Cell[,] mCells;
 280
 281         #endregion
 282     }
 283 }