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