--- /dev/null
+using System;\r
+using System.Collections.Generic;\r
+using System.Linq;\r
+using System.Text;\r
+using System.Diagnostics;\r
+using Microsoft.Xna.Framework;\r
+\r
+namespace CarFire\r
+{\r
+ /// <summary>\r
+ /// A class to navigate from here to there through a grid of\r
+ /// open and closed cells.\r
+ /// </summary>\r
+ public class PathFinder\r
+ {\r
+ #region Public Types\r
+\r
+ /// <summary>\r
+ /// The heuristic function should return some value representing\r
+ /// the distance between two points. A common approach is to\r
+ /// return the manhattan distance.\r
+ /// </summary>\r
+ /// <param name="a">A point.</param>\r
+ /// <param name="b">The endpoint.</param>\r
+ /// <returns>The heuristic.</returns>\r
+ public delegate int Heuristic(Point a, Point b);\r
+\r
+ /// <summary>\r
+ /// The cost function should take two points representing two\r
+ /// adjacent cells and return a cost measure of how expensive it\r
+ /// is to move from one cell to the other.\r
+ /// </summary>\r
+ /// <param name="a">A point.</param>\r
+ /// <param name="b">Another point.</param>\r
+ /// <returns>The cost.</returns>\r
+ public delegate int CostFunction(Point a, Point b);\r
+\r
+ #endregion\r
+\r
+\r
+ #region Public Methods\r
+\r
+ /// <summary>\r
+ /// Construct a path finder with a grid. The grid is a matrix\r
+ /// of boolean values, true meaning the cell is walkable and false\r
+ /// meaning the cell is closed.\r
+ /// </summary>\r
+ /// <param name="grid">The grid to find paths on.</param>\r
+ public PathFinder(bool[,] grid)\r
+ {\r
+ Debug.Assert(grid != null);\r
+\r
+ mGrid = grid;\r
+ mGridWidth = mGrid.GetUpperBound(0) + 1;\r
+ mGridHeight = mGrid.GetUpperBound(1) + 1;\r
+ }\r
+\r
+\r
+ /// <summary>\r
+ /// The A* algorithm for finding the best path through a grid of cells.\r
+ /// The manhattan distance heuristic and a simple distance-based cost\r
+ /// function will be used.\r
+ /// </summary>\r
+ /// <param name="start">The cell to start at.</param>\r
+ /// <param name="finish">The desired destination.</param>\r
+ /// <returns>A list of points representing the path through the grid,\r
+ /// ends points not included, or null if no path could be found.</return>\r
+ public List<Point> GetPath(Point start, Point finish)\r
+ {\r
+ return GetPath(start, finish, GetManhattanDistance, GetCost);\r
+ }\r
+\r
+ /// <summary>\r
+ /// The A* algorithm for finding the best path through a grid of cells.\r
+ /// A simple distance-based cost function will be used.\r
+ /// </summary>\r
+ /// <param name="start">The cell to start at.</param>\r
+ /// <param name="finish">The desired destination.</param>\r
+ /// <param name="heuristic">The heuristic function.</param>\r
+ /// <returns>A list of points representing the path through the grid,\r
+ /// ends points not included, or null if no path could be found.</return>\r
+ public List<Point> GetPath(Point start, Point finish, Heuristic heuristic)\r
+ {\r
+ return GetPath(start, finish, heuristic, GetCost);\r
+ }\r
+\r
+ /// <summary>\r
+ /// The A* algorithm for finding the best path through a grid of cells.\r
+ /// The manhattan distance heuristic will be used.\r
+ /// </summary>\r
+ /// <param name="start">The cell to start at.</param>\r
+ /// <param name="finish">The desired destination.</param>\r
+ /// <param name="costFunction">The cost function</param>\r
+ /// <returns>A list of points representing the path through the grid,\r
+ /// ends points not included, or null if no path could be found.</return>\r
+ public List<Point> GetPath(Point start, Point finish, CostFunction costFunction)\r
+ {\r
+ return GetPath(start, finish, GetManhattanDistance, costFunction);\r
+ }\r
+\r
+ /// <summary>\r
+ /// The A* algorithm for finding the best path through a grid of cells.\r
+ /// </summary>\r
+ /// <param name="start">The cell to start at.</param>\r
+ /// <param name="finish">The desired destination.</param>\r
+ /// <param name="heuristic">The heuristic function.</param>\r
+ /// <param name="costFunction">The cost function.</param>\r
+ /// <returns>A list of points representing the path through the grid,\r
+ /// ends points not included, or null if no path could be found.</return>\r
+ public List<Point> GetPath(Point start, Point finish, Heuristic heuristic, CostFunction costFunction)\r
+ {\r
+ mFringe = new BinaryHeap<Cell>();\r
+ mCells = new Cell[mGridWidth, mGridHeight];\r
+\r
+ Cell startCell = new Cell(start, 0, GetManhattanDistance(start, finish));\r
+ mFringe.Add(startCell);\r
+ mCells[start.X, start.Y] = startCell;\r
+ while (mFringe.Count > 0)\r
+ {\r
+ Cell cell = mFringe.GetNext();\r
+ cell.IsOpen = false;\r
+\r
+ if (cell.Point == finish)\r
+ {\r
+ List<Point> list = new List<Point>();\r
+\r
+ cell = cell.Parent;\r
+ while (cell.Point != start)\r
+ {\r
+ list.Add(cell.Point);\r
+ cell = cell.Parent;\r
+ }\r
+\r
+ list.Reverse();\r
+ return list;\r
+ }\r
+\r
+ List<Point> neighbors = new List<Point>(8);\r
+ neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y - 1));\r
+ neighbors.Add(new Point(cell.Point.X + 0, cell.Point.Y - 1));\r
+ neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y - 1));\r
+ neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 0));\r
+ neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 0));\r
+ neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 1));\r
+ neighbors.Add(new Point(cell.Point.X + 0, cell.Point.Y + 1));\r
+ neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 1));\r
+ foreach (Point point in neighbors)\r
+ {\r
+ Cell inQueue = mCells[point.X, point.Y];\r
+\r
+ if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight &&\r
+ mGrid[point.X, point.Y])\r
+ {\r
+ int cost = cell.G + GetCost(cell.Point, point);\r
+\r
+ if (inQueue == null)\r
+ {\r
+ Cell neighbor = new Cell(point, cost, GetManhattanDistance(point, finish), cell);\r
+ mFringe.Add(neighbor);\r
+ mCells[point.X, point.Y] = neighbor;\r
+ }\r
+ else if (inQueue.IsOpen && cost < inQueue.G)\r
+ {\r
+ inQueue.G = cost;\r
+ inQueue.Parent = cell;\r
+ mFringe.Promote(inQueue);\r
+ }\r
+ }\r
+ }\r
+ }\r
+\r
+ return null;\r
+ }\r
+\r
+\r
+ /// <summary>\r
+ /// Get the manhattan distance between two points. This is a simple but\r
+ /// effective and commonly-used heuristic.\r
+ /// </summary>\r
+ /// <param name="a">A point.</param>\r
+ /// <param name="b">Another point.</param>\r
+ /// <returns>The manhattan distance.</returns>\r
+ public static int GetManhattanDistance(Point a, Point b)\r
+ {\r
+ int w = b.X - a.X;\r
+ int h = b.Y - a.Y;\r
+ if (w < 0) w = -w;\r
+ if (h < 0) h = -h;\r
+ return w + h;\r
+ }\r
+\r
+ /// <summary>\r
+ /// Get the cost to travel from one point to another. This is a simple\r
+ /// cost function based purely on distance. On a square grid, diagonal\r
+ /// cells are further away than adjacent cells; therefore, adjacent moves\r
+ /// are favored.\r
+ /// </summary>\r
+ /// <param name="a">A point.</param>\r
+ /// <param name="b">Another point.</param>\r
+ /// <returns>The cost.</returns>\r
+ public static int GetCost(Point a, Point b)\r
+ {\r
+ if (a.X != b.X && a.Y != b.Y) return 14;\r
+ return 10;\r
+ }\r
+\r
+ #endregion\r
+\r
+\r
+ #region Private Types\r
+\r
+ class Cell : IComparable<Cell>\r
+ {\r
+ public Point Point;\r
+ public Cell Parent;\r
+ public bool IsOpen;\r
+\r
+ public int G\r
+ {\r
+ get { return mG; }\r
+ set { mG = value; mF = mG + mH; }\r
+ }\r
+\r
+ public int H\r
+ {\r
+ get { return mH; }\r
+ set { mH = value; mF = mG + mH; }\r
+ }\r
+\r
+ public int F { get { return mF; } }\r
+\r
+\r
+ public Cell(Point point, int g, int h)\r
+ {\r
+ Point = point;\r
+ IsOpen = true;\r
+ mG = g;\r
+ mH = h;\r
+ mF = g + h;\r
+ }\r
+\r
+ public Cell(Point point, int g, int h, Cell parent)\r
+ {\r
+ Point = point;\r
+ Parent = parent;\r
+ IsOpen = true;\r
+ mG = g;\r
+ mH = h;\r
+ mF = g + h;\r
+ }\r
+\r
+ public int CompareTo(Cell other)\r
+ {\r
+ return F - other.F;\r
+ }\r
+\r
+\r
+ int mG;\r
+ int mH;\r
+ int mF;\r
+ }\r
+\r
+ #endregion\r
+\r
+\r
+ #region Private Variables\r
+\r
+ bool[,] mGrid;\r
+ int mGridWidth;\r
+ int mGridHeight;\r
+\r
+ IPriorityQueue<Cell> mFringe;\r
+ Cell[,] mCells;\r
+\r
+ #endregion\r
+ }\r
+}\r
projects / chaz / carfire / blobdiff