Simple astar algorithm ( C# implementation of simplemain/astar )

AStar

Simple astar algorithm ( C# implementation of simplemain/astar )

Data.cs

namespace AStar
{
	public class Data
	{
		internal Point point;
		internal double g;
		internal double h;
		internal Data parent;

		public Data(Point p, double g, double h, Data parent)
		{
			this.point = p;
			this.g = g;
			this.h = h;
			this.parent = parent;
		}

		public double f()
		{
			return g + h;
		}
	}
}

MinHeap.cs

using System.Collections.Generic;

namespace AStar
{
	public class MinHeap
	{
		private readonly List<Data> queue = new List<Data>();
		private int endPnt = 0;
		private readonly Dictionary<string, Data> index = new Dictionary<string, Data>();
	
		public Data getAndRemoveMin()
		{
			if (isEmpty())
			{
				return null;
			}
		
			Data head = queue[0];
			Data last = queue[endPnt - 1];
			queue[0] = last;
			endPnt--;
			index.Remove(getKey(head.point));
		
			topDown();
		
			return head;
		}
	
		public Data find(Point pnt)
		{
			Data data;
			index.TryGetValue(getKey(pnt), out data);
			return data;
		}
	
		public void add(Data data)
		{
			if (queue.Count > endPnt)
			{
				queue[endPnt] = data;
			}
			else
			{
				queue.Add(data);
			}
			endPnt++;
		
			index[getKey(data.point)] = data;
		
			bottomUp();
		}
	
		public bool isEmpty()
		{
			return endPnt <= 0;
		}
	
		private string getKey(Point pnt)
		{
			return string.Format("{0}-{1}", pnt.x, pnt.y);
		}
	
		private void topDown()
		{
			for (int cur = 0; cur < endPnt; )
			{
				int left  = 2 * cur + 1;
				int right = 2 * cur + 2;
			
				Data dc = queue[cur];
				Data dl = left < endPnt ? queue[left] : null;
				Data dr = right < endPnt ? queue[right] : null;
			
				int next = -1;
				Data dn = dc;
				if (dl != null && dl.f() < dn.f())
				{
					next = left;
					dn = dl;
				}
				if (dr != null && dr.f() < dn.f())
				{
					next = right;
					dn = dr;
				}
			
				if (next >= 0 && next < endPnt)
				{
					queue[next] = dc;
					queue[cur] = dn;
					cur = next;
				}
				else
				{
					break;
				}
			}
		}

		private void bottomUp()
		{
			for (int cur = endPnt - 1; cur >= 0; )
			{
				int parent = (cur - 1) / 2;
				if (parent < 0)
				{
					break;
				}
			
				Data dc = queue[cur];
				Data dp = queue[parent];
			
				if (dc.f() < dp.f())
				{
					queue[parent] = dc;
					queue[cur] = dp;
					cur = parent;
				}
				else
				{
					break;
				}
			}
		}
	}
}

Point.cs

namespace AStar
{
	public struct Point
	{
		internal int x, y;

		public Point(int x, int y)
		{
			this.x = x;
			this.y = y;
		}

		public override bool Equals(object obj)
		{
			if (!(obj is Point))
				return false;

			Point p = (Point)obj;
			return x == p.x && y == p.y;
		}
	}
}

Program.cs

using System;

namespace AStar
{
	class Program
	{
		// 地图元素
		const char START = 'S';  // 起点
		const char END = 'E';  // 终点
		const char SPACE = '.';  // 空地
		const char WALL = 'W';  // 墙
		const char VISITED = '+';  // 被访问过
		const char ON_PATH = '@';  // 在结果路径上

		// 地图字符串
		static readonly String[] S_MAP = {
			". . . . . . . . . . . . . . . . . . . .", 
			". . . W W W W . . . . . . . . . . . . .",
			". . . . . . W . . . . . . . . . . . . .", 
			". . . . . . W . . . . . . . . . . . . .", 
			". . S . . . W . . . W W W W . . . . . .", 
			". . . . . . W . . . . . . W . . . . . .", 
			". . . . . . W . . . . . . W . . E . . .", 
			". . . . . . W . . . . . . W . . . . . .", 
			". . . W W W W . . . . . . . . . . . . .", 
			". . . . . . . . . . . . . . . . . . . ."
		};

		// 地图
		static char[,] MAP = new char[S_MAP[0].Replace(" ", "").Length, S_MAP.Length];
		// 地图最大尺寸
		static Point MAX_PNT = new Point(MAP.GetLength(0), MAP.GetLength(1));
		// 起点
		static Point START_PNT;
		// 终点
		static Point END_PNT;
		// 寻路方法
		static int searchMethod;

		public static void Main(string[] args)
		{
			do
			{
				Console.WriteLine("Choose search method: (0-3)");
				Console.WriteLine("\t0: BFS");
				Console.WriteLine("\t1: Euclidian Distance");
				Console.WriteLine("\t2: Pow Euclidian Distance");
				Console.WriteLine("\t3: Manhattan Distance");
				var sm = Console.ReadLine();
				if (int.TryParse(sm, out searchMethod))
				{
					Console.Clear();

					genMap();
					printMap();

					search();

					printMap();
				}
				else
					break;
			}
			while (true);
		}

		/**
		 * 用地图字符串产生地图数据
		 */
		static void genMap()
		{
			int idx = 0;
			foreach (string s in S_MAP)
			{
				char[] cs = s.Replace(" ", "").ToCharArray();
				for (int i = 0; i < cs.Length; i++)
				{
					MAP[i, idx] = cs[i];
					switch (cs[i])
					{
						case START:
							START_PNT = new Point(i, idx);
							break;
						case END:
							END_PNT = new Point(i, idx);
							break;
					}
				}
				idx++;
			}
		}

		/**
		 * 打印地图
		 */
		static void printMap()
		{
			for (int j = 0; j < MAX_PNT.y; j++)
			{
				for (int i = 0; i < MAX_PNT.x; i++)
				{
					Console.Write("{0} ", MAP[i, j]);
				}
				Console.WriteLine();
			}
			Console.WriteLine();
		}

		/**
		 * 搜索算法
		 */
		static void search()
		{
			MinHeap heap = new MinHeap(); // 用最小堆来记录扩展的点
			int[,] directs = {{1, 0}, {0, 1}, {-1, 0}, {0, -1}}; // 可以扩展的四个方向
		
			heap.add(new Data(START_PNT, 0, 0, null)); // 把起始点放入堆
			Data lastData = null; // 找到的最后一个点的数据,用来反推路径
		
			for (bool finish = false; !finish && !heap.isEmpty(); )
			{
				Data data = heap.getAndRemoveMin(); // 取出f值最小的点
				Point point = data.point;
				if (MAP[point.x, point.y] == SPACE) // 将取出的点标识为已访问点
				{
					MAP[point.x, point.y] = VISITED;
				}

				for (int i = 0; i < 4; ++i)  // 遍历四个方向的点
				{
					Point newPnt = new Point(point.x + directs[i, 0], point.y + directs[i, 1]);
					if (newPnt.x >= 0 && newPnt.x < MAX_PNT.x && newPnt.y >= 0 && newPnt.y < MAX_PNT.y)
					{
						char e = MAP[newPnt.x, newPnt.y];
						if (e == END) // 如果是终点,则跳出循环,不用再找
						{
							lastData = data;
							finish = true;
							break;
						}
						if (e != SPACE) // 如果不是空地,就不需要再扩展
						{
							continue;
						}
					
						Data inQueueData = heap.find(newPnt);
						if (inQueueData != null) // 如果在堆里,则更新g值
						{
							if (inQueueData.g > data.g + 1)
							{
								inQueueData.g = data.g + 1;
								inQueueData.parent = data;
							}
						}
						else // 如果不在堆里,则放入堆中
						{
							double hv = h(newPnt);
							Data newData = new Data(newPnt, data.g + 1, hv, data);
							heap.add(newData);
						}
					}
				}
			}

			// 反向找出路径
			for (Data pathData = lastData; pathData != null; ) 
			{
				Point pnt = pathData.point;
				if (MAP[pnt.x, pnt.y] == VISITED)
				{
					MAP[pnt.x, pnt.y] = ON_PATH;
				}
				pathData = pathData.parent;
			
			}
		}

		/**
		 * h函数
		 */
		static double h(Point pnt)
		{
			switch (searchMethod)
			{
				case 1: return hEuclidianDistance(pnt);
				case 2: return hPowEuclidianDistance(pnt);
				case 3: return hManhattanDistance(pnt);
				default: return hBFS(pnt);
			}
		}

		/**
		 * 曼哈顿距离,小于等于实际值
		 */
		static double hManhattanDistance(Point pnt)
		{
			return Math.Abs(pnt.x - END_PNT.x) + Math.Abs(pnt.y - END_PNT.y);
		}

		/**
		 * 欧式距离,小于等于实际值
		 */
		static double hEuclidianDistance(Point pnt)
		{
			return Math.Sqrt(Math.Pow(pnt.x - END_PNT.x, 2) + Math.Pow(pnt.y - END_PNT.y, 2));
		}

		/**
		 * 欧式距离平方,大于等于实际值
		 */
		static double hPowEuclidianDistance(Point pnt)
		{
			return Math.Pow(pnt.x - END_PNT.x, 2) + Math.Pow(pnt.y - END_PNT.y, 2);
		}

		/**
		 * BFS的h值,恒为0
		 */
		static double hBFS(Point pnt)
		{
			return 0;
		}
	}
}