// Uncomment this to disable diagonal movemet.
//#define ALLOW_DIAGONAL_MOVEMENT
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using System.Threading;
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);
///
/// An async task object to manage a search task that has been
/// put on the thread pool.
///
public class AsyncTask
{
///
/// Construct an async task object.
///
/// A path finder.
/// The cell to start at.
/// The desired destination.
/// The heuristic function.
/// The cost function.
public AsyncTask(PathFinder finder, Point start, Point finish, Heuristic heuristic, CostFunction costFunction)
{
mPathFinder = finder;
mStart = start;
mFinish = finish;
mHeuristic = heuristic;
mCostFunction = costFunction;
}
///
/// Determine whether or not the task has completed.
///
public bool IsCompleted { get { return mIsDone; } }
///
/// Get the resulting path.
///
public List Path { get { return mPath; } }
#region Private Members
public void Run(object context)
{
mPath = mPathFinder.GetPath(mStart, mFinish, mHeuristic, mCostFunction);
mIsDone = true;
}
PathFinder mPathFinder;
Point mStart;
Point mFinish;
Heuristic mHeuristic;
CostFunction mCostFunction;
List mPath;
bool mIsDone;
#endregion
}
#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,
/// starting point 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,
/// starting point 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,
/// starting point 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,
/// starting point 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, heuristic(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();
list.Add(cell.Point);
cell = cell.Parent;
if (cell != null) for (; cell.Point != start; cell = cell.Parent) list.Add(cell.Point);
list.Reverse();
return list;
}
List neighbors = new List(8);
neighbors.Add(new Point(cell.Point.X, cell.Point.Y - 1));
neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y));
neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y));
neighbors.Add(new Point(cell.Point.X, cell.Point.Y + 1));
#if ALLOW_DIAGONAL_MOVEMENT
neighbors.Add(new Point(cell.Point.X - 1, 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 + 1));
neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 1));
#endif
foreach (Point point in neighbors)
{
if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight &&
mGrid[point.X, point.Y])
{
int cost = cell.G + costFunction(cell.Point, point);
Cell inQueue = mCells[point.X, point.Y];
if (inQueue == null)
{
Cell neighbor = new Cell(point, cost, heuristic(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 a shortest path by putting the task on the thread pool.
///
/// The cell to start at.
/// The desired destination.
/// The async task object.
public AsyncTask GetPathAsync(Point start, Point finish)
{
AsyncTask task = new AsyncTask(this, start, finish, GetManhattanDistance, GetCost);
ThreadPool.QueueUserWorkItem(new WaitCallback(task.Run));
return task;
}
///
/// Find the closest open cell that is near another cell.
///
/// The coordinates.
/// An open cell at or near the given coordinates,
/// or null if no open nearby cell could be found.
public Point? GetNearbyOpenCell(Point coordinates)
{
if (0 <= coordinates.X && coordinates.X < mGridWidth && 0 <= coordinates.Y && coordinates.Y < mGridHeight &&
mGrid[coordinates.X, coordinates.Y])
{
return coordinates;
}
mFringe = new BinaryHeap();
mCells = new Cell[mGridWidth, mGridHeight];
Cell startCell = new Cell(coordinates, 0, 0);
mFringe.Add(startCell);
mCells[coordinates.X, coordinates.Y] = startCell;
while (mFringe.Count > 0)
{
Cell cell = mFringe.GetNext();
List neighbors = new List(8);
neighbors.Add(new Point(cell.Point.X, cell.Point.Y - 1));
neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y));
neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y));
neighbors.Add(new Point(cell.Point.X, 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 - 1));
neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 1));
neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 1));
foreach (Point point in neighbors)
{
if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight)
{
if (mGrid[point.X, point.Y])
{
return point;
}
else
{
int cost = cell.G + GetCost(cell.Point, point);
Cell inQueue = mCells[point.X, point.Y];
if (inQueue == null)
{
Cell neighbor = new Cell(point, cost, 0);
mFringe.Add(neighbor);
mCells[point.X, point.Y] = neighbor;
}
}
}
}
}
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
}
}
| | | |