[chaz/carfire] / CarFire / CarFire / CarFire / PathFinder.cs


// 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 Microsoft.Xna.Framework;

namespace CarFire
{
    /// <summary>
    /// A class to navigate from here to there through a grid of
    /// open and closed cells.
    /// </summary>
    public class PathFinder
    {
        #region Public Types

        /// <summary>
        /// The heuristic function should return some value representing
        /// the distance between two points.  A common approach is to
        /// return the manhattan distance.
        /// </summary>
        /// <param name="a">A point.</param>
        /// <param name="b">The endpoint.</param>
        /// <returns>The heuristic.</returns>
        public delegate int Heuristic(Point a, Point b);

        /// <summary>
        /// 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.
        /// </summary>
        /// <param name="a">A point.</param>
        /// <param name="b">Another point.</param>
        /// <returns>The cost.</returns>
        public delegate int CostFunction(Point a, Point b);

        #endregion


        #region Public Methods

        /// <summary>
        /// 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.
        /// </summary>
        /// <param name="grid">The grid to find paths on.</param>
        public PathFinder(bool[,] grid)
        {
            Debug.Assert(grid != null);

            mGrid = grid;
            mGridWidth = mGrid.GetUpperBound(0) + 1;
            mGridHeight = mGrid.GetUpperBound(1) + 1;
        }


        /// <summary>
        /// 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.
        /// </summary>
        /// <param name="start">The cell to start at.</param>
        /// <param name="finish">The desired destination.</param>
        /// <returns>A list of points representing the path through the grid,
        /// ends points not included, or null if no path could be found.</return>
        public List<Point> GetPath(Point start, Point finish)
        {
            return GetPath(start, finish, GetManhattanDistance, GetCost);
        }

        /// <summary>
        /// The A* algorithm for finding the best path through a grid of cells.
        /// A simple distance-based cost function will be used.
        /// </summary>
        /// <param name="start">The cell to start at.</param>
        /// <param name="finish">The desired destination.</param>
        /// <param name="heuristic">The heuristic function.</param>
        /// <returns>A list of points representing the path through the grid,
        /// ends points not included, or null if no path could be found.</return>
        public List<Point> GetPath(Point start, Point finish, Heuristic heuristic)
        {
            return GetPath(start, finish, heuristic, GetCost);
        }

        /// <summary>
        /// The A* algorithm for finding the best path through a grid of cells.
        /// The manhattan distance heuristic will be used.
        /// </summary>
        /// <param name="start">The cell to start at.</param>
        /// <param name="finish">The desired destination.</param>
        /// <param name="costFunction">The cost function</param>
        /// <returns>A list of points representing the path through the grid,
        /// ends points not included, or null if no path could be found.</return>
        public List<Point> GetPath(Point start, Point finish, CostFunction costFunction)
        {
            return GetPath(start, finish, GetManhattanDistance, costFunction);
        }

        /// <summary>
        /// The A* algorithm for finding the best path through a grid of cells.
        /// </summary>
        /// <param name="start">The cell to start at.</param>
        /// <param name="finish">The desired destination.</param>
        /// <param name="heuristic">The heuristic function.</param>
        /// <param name="costFunction">The cost function.</param>
        /// <returns>A list of points representing the path through the grid,
        /// ends points not included, or null if no path could be found.</return>
        public List<Point> GetPath(Point start, Point finish, Heuristic heuristic, CostFunction costFunction)
        {
            mFringe = new BinaryHeap<Cell>();
            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<Point> list = new List<Point>();

                    cell = cell.Parent;
                    while (cell != null && cell.Point != start)
                    {
                        list.Add(cell.Point);
                        cell = cell.Parent;
                    }

                    list.Reverse();
                    return list;
                }

                List<Point> neighbors = new List<Point>(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;
        }


        /// <summary>
        /// Get the manhattan distance between two points.  This is a simple but
        /// effective and commonly-used heuristic.
        /// </summary>
        /// <param name="a">A point.</param>
        /// <param name="b">Another point.</param>
        /// <returns>The manhattan distance.</returns>
        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;
        }

        /// <summary>
        /// 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.
        /// </summary>
        /// <param name="a">A point.</param>
        /// <param name="b">Another point.</param>
        /// <returns>The cost.</returns>
        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<Cell>
        {
            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<Cell> mFringe;
        Cell[,] mCells;

        #endregion
    }
}