Dogcows Code - chaz/carfire/blob - CarFire/CarFire/CarFire/PathFinder.cs

   1 using System;
   2 using System.Collections.Generic;
   3 using System.Linq;
   4 using System.Text;
   5 using System.Diagnostics;
   6 using Microsoft.Xna.Framework;
   7
   8 namespace CarFire
   9 {
  10     /// <summary>
  11     /// A class to navigate from here to there through a grid of
  12     /// open and closed cells.
  13     /// </summary>
  14     public class PathFinder
  15     {
  16         #region Public Types
  17
  18         /// <summary>
  19         /// The heuristic function should return some value representing
  20         /// the distance between two points.  A common approach is to
  21         /// return the manhattan distance.
  22         /// </summary>
  23         /// <param name="a">A point.</param>
  24         /// <param name="b">The endpoint.</param>
  25         /// <returns>The heuristic.</returns>
  26         public delegate int Heuristic(Point a, Point b);
  27
  28         /// <summary>
  29         /// The cost function should take two points representing two
  30         /// adjacent cells and return a cost measure of how expensive it
  31         /// is to move from one cell to the other.
  32         /// </summary>
  33         /// <param name="a">A point.</param>
  34         /// <param name="b">Another point.</param>
  35         /// <returns>The cost.</returns>
  36         public delegate int CostFunction(Point a, Point b);
  37
  38         #endregion
  39
  40
  41         #region Public Methods
  42
  43         /// <summary>
  44         /// Construct a path finder with a grid.  The grid is a matrix
  45         /// of boolean values, true meaning the cell is walkable and false
  46         /// meaning the cell is closed.
  47         /// </summary>
  48         /// <param name="grid">The grid to find paths on.</param>
  49         public PathFinder(bool[,] grid)
  50         {
  51             Debug.Assert(grid != null);
  52
  53             mGrid = grid;
  54             mGridWidth = mGrid.GetUpperBound(0) + 1;
  55             mGridHeight = mGrid.GetUpperBound(1) + 1;
  56         }
  57
  58
  59         /// <summary>
  60         /// The A* algorithm for finding the best path through a grid of cells.
  61         /// The manhattan distance heuristic and a simple distance-based cost
  62         /// function will be used.
  63         /// </summary>
  64         /// <param name="start">The cell to start at.</param>
  65         /// <param name="finish">The desired destination.</param>
  66         /// <returns>A list of points representing the path through the grid,
  67         /// ends points not included, or null if no path could be found.</return>
  68         public List<Point> GetPath(Point start, Point finish)
  69         {
  70             return GetPath(start, finish, GetManhattanDistance, GetCost);
  71         }
  72
  73         /// <summary>
  74         /// The A* algorithm for finding the best path through a grid of cells.
  75         /// A simple distance-based cost function will be used.
  76         /// </summary>
  77         /// <param name="start">The cell to start at.</param>
  78         /// <param name="finish">The desired destination.</param>
  79         /// <param name="heuristic">The heuristic function.</param>
  80         /// <returns>A list of points representing the path through the grid,
  81         /// ends points not included, or null if no path could be found.</return>
  82         public List<Point> GetPath(Point start, Point finish, Heuristic heuristic)
  83         {
  84             return GetPath(start, finish, heuristic, GetCost);
  85         }
  86
  87         /// <summary>
  88         /// The A* algorithm for finding the best path through a grid of cells.
  89         /// The manhattan distance heuristic will be used.
  90         /// </summary>
  91         /// <param name="start">The cell to start at.</param>
  92         /// <param name="finish">The desired destination.</param>
  93         /// <param name="costFunction">The cost function</param>
  94         /// <returns>A list of points representing the path through the grid,
  95         /// ends points not included, or null if no path could be found.</return>
  96         public List<Point> GetPath(Point start, Point finish, CostFunction costFunction)
  97         {
  98             return GetPath(start, finish, GetManhattanDistance, costFunction);
  99         }
 100
 101         /// <summary>
 102         /// The A* algorithm for finding the best path through a grid of cells.
 103         /// </summary>
 104         /// <param name="start">The cell to start at.</param>
 105         /// <param name="finish">The desired destination.</param>
 106         /// <param name="heuristic">The heuristic function.</param>
 107         /// <param name="costFunction">The cost function.</param>
 108         /// <returns>A list of points representing the path through the grid,
 109         /// ends points not included, or null if no path could be found.</return>
 110         public List<Point> GetPath(Point start, Point finish, Heuristic heuristic, CostFunction costFunction)
 111         {
 112             mFringe = new BinaryHeap<Cell>();
 113             mCells = new Cell[mGridWidth, mGridHeight];
 114
 115             Cell startCell = new Cell(start, 0, GetManhattanDistance(start, finish));
 116             mFringe.Add(startCell);
 117             mCells[start.X, start.Y] = startCell;
 118             while (mFringe.Count > 0)
 119             {
 120                 Cell cell = mFringe.GetNext();
 121                 cell.IsOpen = false;
 122
 123                 if (cell.Point == finish)
 124                 {
 125                     List<Point> list = new List<Point>();
 126
 127                     cell = cell.Parent;
 128                     while (cell.Point != start)
 129                     {
 130                         list.Add(cell.Point);
 131                         cell = cell.Parent;
 132                     }
 133
 134                     list.Reverse();
 135                     return list;
 136                 }
 137
 138                 List<Point> neighbors = new List<Point>(8);
 139                 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y - 1));
 140                 neighbors.Add(new Point(cell.Point.X + 0, cell.Point.Y - 1));
 141                 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y - 1));
 142                 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 0));
 143                 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 0));
 144                 neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 1));
 145                 neighbors.Add(new Point(cell.Point.X + 0, cell.Point.Y + 1));
 146                 neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 1));
 147                 foreach (Point point in neighbors)
 148                 {
 149                     Cell inQueue = mCells[point.X, point.Y];
 150
 151                     if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight &&
 152                         mGrid[point.X, point.Y])
 153                     {
 154                         int cost = cell.G + GetCost(cell.Point, point);
 155
 156                         if (inQueue == null)
 157                         {
 158                             Cell neighbor = new Cell(point, cost, GetManhattanDistance(point, finish), cell);
 159                             mFringe.Add(neighbor);
 160                             mCells[point.X, point.Y] = neighbor;
 161                         }
 162                         else if (inQueue.IsOpen && cost < inQueue.G)
 163                         {
 164                             inQueue.G = cost;
 165                             inQueue.Parent = cell;
 166                             mFringe.Promote(inQueue);
 167                         }
 168                     }
 169                 }
 170             }
 171
 172             return null;
 173         }
 174
 175
 176         /// <summary>
 177         /// Get the manhattan distance between two points.  This is a simple but
 178         /// effective and commonly-used heuristic.
 179         /// </summary>
 180         /// <param name="a">A point.</param>
 181         /// <param name="b">Another point.</param>
 182         /// <returns>The manhattan distance.</returns>
 183         public static int GetManhattanDistance(Point a, Point b)
 184         {
 185             int w = b.X - a.X;
 186             int h = b.Y - a.Y;
 187             if (w < 0) w = -w;
 188             if (h < 0) h = -h;
 189             return w + h;
 190         }
 191
 192         /// <summary>
 193         /// Get the cost to travel from one point to another.  This is a simple
 194         /// cost function based purely on distance.  On a square grid, diagonal
 195         /// cells are further away than adjacent cells; therefore, adjacent moves
 196         /// are favored.
 197         /// </summary>
 198         /// <param name="a">A point.</param>
 199         /// <param name="b">Another point.</param>
 200         /// <returns>The cost.</returns>
 201         public static int GetCost(Point a, Point b)
 202         {
 203             if (a.X != b.X && a.Y != b.Y) return 14;
 204             return 10;
 205         }
 206
 207         #endregion
 208
 209
 210         #region Private Types
 211
 212         class Cell : IComparable<Cell>
 213         {
 214             public Point Point;
 215             public Cell Parent;
 216             public bool IsOpen;
 217
 218             public int G
 219             {
 220                 get { return mG; }
 221                 set { mG = value; mF = mG + mH; }
 222             }
 223
 224             public int H
 225             {
 226                 get { return mH; }
 227                 set { mH = value; mF = mG + mH; }
 228             }
 229
 230             public int F { get { return mF; } }
 231
 232
 233             public Cell(Point point, int g, int h)
 234             {
 235                 Point = point;
 236                 IsOpen = true;
 237                 mG = g;
 238                 mH = h;
 239                 mF = g + h;
 240             }
 241
 242             public Cell(Point point, int g, int h, Cell parent)
 243             {
 244                 Point = point;
 245                 Parent = parent;
 246                 IsOpen = true;
 247                 mG = g;
 248                 mH = h;
 249                 mF = g + h;
 250             }
 251
 252             public int CompareTo(Cell other)
 253             {
 254                 return F - other.F;
 255             }
 256
 257
 258             int mG;
 259             int mH;
 260             int mF;
 261         }
 262
 263         #endregion
 264
 265
 266         #region Private Variables
 267
 268         bool[,] mGrid;
 269         int mGridWidth;
 270         int mGridHeight;
 271
 272         IPriorityQueue<Cell> mFringe;
 273         Cell[,] mCells;
 274
 275         #endregion
 276     }
 277 }