pathfinder.cs

game implemented java

using System;
using System.Collections;
using System.Drawing;
using System.Windows.Forms;
using RedBlackCS;

namespace WindowsApplication2
{
	/// <summary>
	/// An ungeneralized implementation of the A* algorithm 
	/// It is the only object which uses the RedBlackCS namespace
	/// </summary>
	public struct AStarNode
	{
		public int id;			//Intersection ID
		public int prevId;		//pseudo-pointer to previous Intersection in path
		public double dist;		//actual distance travelled
		public double FofN;		//heuristic estimate of total distance to goal
	}
	public class Pathfinder
	{
		private readonly int node_start, node_goal;
		private IntersectionList nodes;
		private CityMap theMap;
		private RedBlack OPEN, CLOSED;
		private double[] pointer;	
		//An array of pointers designed to allow faster creation of the end path

		public Pathfinder(int startInt, int goalInt, CityMap myMap)
		{
			OPEN = new RedBlack(myMap.getIntersectionList.Count, "OPEN");
			CLOSED = new RedBlack(myMap.getIntersectionList.Count, "CLOSED");
			pointer = new double[myMap.getIntersectionList.Count];

			node_start = startInt;
			node_goal = goalInt;
			nodes = myMap.getIntersectionList;
			theMap = myMap;
			
			AStarNode start = new AStarNode();
			start.id = node_start;
			start.prevId = -1;
			start.dist = 0;
			start.FofN = toGoal(((Intersection)nodes[node_start]).getPoint);
			
			OPEN.Add(start);
			pointer[node_start] = -1;
		}

		public ArrayList getPath ()
		{
			while (OPEN.Size() > 0)
			{//No path exists if OPEN is empty
				AStarNode min = OPEN.GetMin();
				//Remove the minimum path from the OPEN list and add it to the CLOSED list
				OPEN.Remove(min.id);
				CLOSED.Add(min);
				if (min.id == node_goal)
				{//If the shortest path terminates at the goal, we are done
					return GetArray(min.id);
				}
				//Otherwise, create a list of intersections reachable from the end
				//of the shortest path
				ArrayList nPrime = ((Intersection)nodes[min.id]).getConnections;
				for (int i=0;i<nPrime.Count;i++)
				{
					MapSeg currSeg = theMap.getSegment((int)nPrime[i]);
					int intID = currSeg.From_Intersection;
					if (intID == min.id)
						intID = currSeg.To_Intersection;
					double newCost = min.dist + Distance(((Intersection)nodes[min.id]).getPoint,((Intersection)nodes[intID]).getPoint);
					if (Math.Min(OPEN.GetValueAtID(intID),CLOSED.GetValueAtID(intID)) > newCost)
					{//Add each reachable intersection to the OPEN list (and remove from the CLOSED list)
					// so long as the current path reaches it faster
						OPEN.Remove(intID);
						CLOSED.Remove(intID);

						AStarNode toAdd = new AStarNode();
						toAdd.prevId = min.id;
						pointer[intID] = min.id;
						toAdd.dist = newCost;
						toAdd.FofN = newCost + toGoal(((Intersection)nodes[intID]).getPoint);
						toAdd.id = intID;
						OPEN.Add(toAdd);
					}
				}
			}
			//No path exists
			return null;
		}
		private ArrayList GetArray(int id)
		{//Recurse through pseudo-pointers to build list defining the shortest path
			if (pointer[id] < 0)
			{
				ArrayList toReturn = new ArrayList();
				toReturn.Add(id);
				return toReturn;
			}
			else
			{
				ArrayList toReturn = GetArray((int)pointer[id]);
				toReturn.Add(id);
				return toReturn;
			}
		}
		private double toGoal (Point p)
		{//Shorthand for Distance (p, Goal)
			return Distance (p,((Intersection)nodes[node_goal]).getPoint);  
		}
		private double Distance (Point a, Point b)
		{
			double xdiff = a.X - b.X;  
			double ydiff = a.Y - b.Y;  
			return Math.Sqrt(xdiff * xdiff + ydiff * ydiff);  
		}
	}
}