simplepathfinder.cs

A .NET Path Finder Library Path Finder Library is a .NET library that currently contains one type c

using System;
using System.Collections.Generic;

namespace HenizeSoftware
{
  namespace PathFinding
  {
    /// <summary>
    /// The SimplePathFinder type is for finding the shortest path from point A to point B is some kind of
    /// 2D maze or map. It is extrememly simple and basic.
    /// </summary>
    public class SimplePathFinder
    {
      Map nodeMap;

      Node startNode;
      Node endNode;

      Queue<Node> finalPath = new Queue<Node>();

      bool isPathCalculated;

      /// <summary>
      /// Allows you to edit the two-diminsional map of nodes. Set to true for unwalkable.
      /// </summary>
      /// <param name="x">The x diminsion on the map.</param>
      /// <param name="y">The y diminsion on the map.</param>
      /// <returns>Returns wether node at coordinate x, y is walkable(false) or unwalkable(true)</returns>
      public bool this[int x, int y]
      {
        get { return nodeMap[(ushort)x, (ushort)y]; }
        set
        {
          nodeMap[(ushort)x, (ushort)y] = value;
          isPathCalculated = false;
        }
      }
      /// <summary>
      /// The starting position from where the search for the end position begins.
      /// </summary>
      public Coordinate StartNodePosition
      {
        get { return new Coordinate(startNode.X, startNode.Y); }
        set
        {
          startNode.X = (ushort)value.X;
          startNode.Y = (ushort)value.Y;
          isPathCalculated = false;
        }
      }
      
      /// <summary>
      /// The ending position to where the search to here from the start position ends.
      /// </summary>
      public Coordinate EndNodePosition
      {
        get { return new Coordinate(endNode.X, endNode.Y); }
        set
        {
          endNode.X = (ushort)value.X;
          endNode.Y = (ushort)value.Y;
          isPathCalculated = false;
        }
      }
      /// <summary>
      /// Gets a coordinate array containing the path from the start position to the end position.
      /// </summary>
      public Coordinate[] Path
      {
        get
        {
          if (!isPathCalculated)
            FindPath();
          
          if (finalPath.Count == 0)
            throw new ApplicationException("Path not found, the start node or the end node is enclosed or do not exist, " + 
                                            "the starting node position is the same as the ending node position");

          Stack<Coordinate> coordinateStack = new Stack<Coordinate>();
          foreach (Node pathNode in finalPath)
          {
            coordinateStack.Push(new Coordinate(pathNode.X, pathNode.Y));
          }

          return coordinateStack.ToArray();
        }
      }

      /// <summary>
      /// You must specify the width, height, starting position, ending position, and any unwalkable nodes
      /// during creation of the SimplePathFinder.
      /// </summary>
      /// <param name="width">The width of the 2D map.</param>
      /// <param name="height">The height of the 2D map.</param>
      /// <param name="startNodePos">The coordinate of starting node.</param>
      /// <param name="endNodePos">The coordinate of the ending node.</param>
      /// <param name="unwalkableNodes">An array of coordinates containing the positions
      /// of any unwalkable nodes in the map. Pass in an empty array or null if there is none.</param>
      public SimplePathFinder(int width, int height, Coordinate startNodePos, 
                              Coordinate endNodePos, Coordinate[] unwalkableNodes)
      {
        nodeMap = new Map((ushort)width, (ushort)height, unwalkableNodes);
        endNode = new Node(null, null, nodeMap, endNodePos.X, endNodePos.Y);
        startNode = new Node(null, endNode, nodeMap, startNodePos.X, startNodePos.Y);
      }
      
      void FindPath()
      {
        SortedNodeList Open = new SortedNodeList();

        if (startNode == null || endNode == null || nodeMap == null)
          throw new ApplicationException("StartNodePosition, EndNodePosition, or the Map is not instantiated.");

        Open.Add(startNode);

        while (Open.Count > 0)
        {
          Node currentNode = Open.RemoveFirst();

          if (currentNode.Equals(endNode))
          {
            endNode.ParentNode = currentNode.ParentNode;
            break;
          }

          Node[] successors = currentNode.GetSuccessors();

          foreach (Node successorNode in successors)
          {
            int oFound = Open.IndexOf(successorNode);
            if(oFound > 0)
            {
              if (Open.NodeAt(oFound) <= currentNode)
              {
                continue;
              }
            }

            if (oFound >= 0)
              Open.RemoveAt(oFound);

            Open.Add(successorNode);
          }
        }

        Node p = endNode;
        
        //loop through and contruct the final path by following the parents 
        //of each node starting from the end node
        while(true)
        {
          finalPath.Enqueue(p);
          
          p = p.ParentNode;
          
          if (p == null)
            break;
          if (p.ParentNode != null && p.ParentNode.ParentNode != null)
          {
            //cleans and shortens the path by removing unecessary moves
            if (p.ParentNode.ParentNode.IsAjacentTo(p))
              p.ParentNode = p.ParentNode.ParentNode;
            //removes single node zigzags on x diminsion
            if (p.IsDiagonalTo(p.ParentNode) && p.ParentNode.IsDiagonalTo(p.ParentNode.ParentNode) &&
               p.X == p.ParentNode.ParentNode.X)
            {
              p.ParentNode.X = p.X;
            }
            //removes single node zigzags on y diminsion
            if (p.IsDiagonalTo(p.ParentNode) && p.ParentNode.IsDiagonalTo(p.ParentNode.ParentNode) &&
                p.Y == p.ParentNode.ParentNode.Y)
            {
              p.ParentNode.Y = p.Y;
            }
          }
        } 
        
        //if this is true then the there is no path or the start node is ontop of the end node.
        if (finalPath.Count == 1)
          finalPath.Clear();

        isPathCalculated = true;
      }
    }
  }
}