## A*算法介绍及C#实现

#### 参考：

http://en.wikipedia.org/wiki/A*_search_algorithm

[翻译]A*寻路初探 GameDev.net

A* Pathfinding for Beginners

#### 介绍:

AStar算法是最短路径搜索的一种，类属于启发式搜索. :), 照搬。。。。

F = G + H

G: 从现在访问这点到开始点的消耗

H: 从现在这点到结束点的估计消耗

F: 做判定访问优先级的依据

1. Open, Close队列

Open里保存已经打开但是并未访问的元素

2. FScore, GScore, HScore 列表

3. ComeFrom 每个点的父亲关系

#### 代码实现(C#版本)

   1: using System;
   2: using System.Collections;
   3: using System.Collections.Generic;
   4:
   5: namespace Roger.Testing
   6: {
   7:     public class Testing
   8:     {
   9:         public static void Main()
  10:         {
  11:             List<bool> map = new List<bool>
  12:             {
  13:                 true, true, true, false, false,
  14:                 false, true, false, true,false,
  15:                 false, false, false, true,false,
  16:                 false,true,false, false, false,
  17:                 false, false, false, true, false
  18:             };
  19:             int width = 5;
  20:             int start = 10;
  21:             int end = 24;
  22:
  23:             if (map[start] || map[end]) {
  24:                 Console.Write("起始点或者结束点为不合法障碍物，！");
  25:                 Console.Read();
  26:                 return;
  27:             }
  28:
  29:             AStarSearch finder = new AStarSearch(map, width);
  30:             Dictionary<int, int> path = finder.FindPath(start, end);
  31:
  32:             if (path == null)
  33:             {
  34:                 Console.WriteLine("没有找到对应的路径"); Console.Read(); return;
  35:             }
  36:
  37:             // 得到最后一个Key的值
  38:             int key = end;
  39:             List<int> link = new List<int>() { end };
  40:             while (path[key] != start)
  41:             {
  42:                 key = path[key];
  43:                 link.Add(key);
  44:             }
  45:             link.Add(start);
  46:
  47:             for (int i = 0; i < map.Count; i++ )
  48:             {
  49:                 if (i % width == 0) Console.WriteLine();
  50:                 if (map[i]) Console.Write("#");
  51:                 else if (link.Contains(i)) Console.Write("1");
  52:                 else Console.Write("*");
  53:
  54:                 Console.Write(" ");
  55:             }
  56:
  57:             Console.Read();
  58:         }
  59:     }
  60:
  61:     /// <summary>
  62:     /// A* 最短路径搜索
  63:     /// </summary>
  64:     public class AStarSearch
  65:     {
  66:         private List<bool> Map;
  67:         private int Width;
  68:         private int Height;
  69:         private bool IsValidPosition(int start)
  70:         {
  71:             return start >= 0 && start <= Map.Count;
  72:         }
  73:         public AStarSearch(List<bool> map, int Width)
  74:         {
  75:             if (map == null) throw new ArgumentNullException();
  76:
  77:             Map = map;
  78:             this.Width = Width;
  79:             this.Height = Map.Count / Width;
  80:         }
  81:
  82:         private Queue<int> Open;
  83:         private Queue<int> Close;
  84:
  85:         public Dictionary<int, int> FindPath(int start, int end)
  86:         {
  87:             if (!IsValidPosition(start) || !IsValidPosition(end)) throw new ArgumentOutOfRangeException();
  88:             this.Start = start; this.End = end;
  89:             Open = new Queue<int>();
  90:             Close = new Queue<int>();
  91:             GScore = new Dictionary<int, int>();
  92:             FScore = new Dictionary<int, int>();
  93:             ComeFrom = new Dictionary<int, int>();
  94:
  95:             // 将开始节点入队列
  96:             Open.Enqueue(start);
  97:
  98:             int x = start;
  99:             while (Open.Count > 0)
 100:             {
 101:                 x = GetLowestF();
 102:                 if (x == End)
 103:                 {
 104:                     // Trace From
 105:                     return ComeFrom;
 106:                 }
 107:
 108:                 Open.Dequeue();
 109:                 Close.Enqueue(x);
 110:
 111:                 foreach (int y in GetNodesAround(x))
 112:                 {
 113:                     if (Close.Contains(y))
 114:                     {
 115:                         continue;
 116:                     }
 117:
 118:                     int newGValue = GetCost(x) + GetDistance(x, y);
 119:                     bool newIsBetter = false;
 120:
 121:                     if (!Open.Contains(y))
 122:                     {
 123:                         Open.Enqueue(y);
 124:                         newIsBetter = true;
 125:                     }
 126:                     else if (newGValue < GScore[y])
 127:                         newIsBetter = true;
 128:
 129:
 130:                     if(newIsBetter)
 131:                     {
 132:                         if (ComeFrom.ContainsKey(y))
 133:                             ComeFrom[y] = x;
 134:                         else
 135:                             ComeFrom.Add(y, x);
 136:
 137:                         GScore[y] = newGValue;
 138:                         FScore[y] = GScore[y] + GetHeuristic(y);
 139:                     }
 140:
 141:                 }
 142:             }
 143:
 144:             return null;
 145:         }
 146:
 147:         private int Start;
 148:         private int End;
 149:
 150:
 151:         private IList<int> GetNodesAround(int pos)
 152:         {
 153:             List<int> list = new List<int>(4);
 154:             int x = pos % Width; int y = pos / Width;
 155:             // Left
 156:             if (x > 0 && !Map[x - 1 + y * Width]) list.Add(x - 1 + y * Width);
 157:             // Up
 158:             if (y > 0 && !Map[x + (y - 1) * Width]) list.Add(x + (y - 1) * Width);
 159:             // Right
 160:             if (x < Width-1 && !Map[x + 1 + y * Width]) list.Add(x + 1 + y * Width);
 161:             // Down
 162:             if (y < Height-1 && !Map[x + (y + 1) * Width]) list.Add(x + (y + 1) * Width);
 163:
 164:             return list;
 165:         }
 166:
 167:         private int GetCost(int current)
 168:         {
 169:             // UNDONE
 170:             int xDistance = (int)Math.Abs(Start % Width - current % Width);
 171:             int yDistance = (int)Math.Abs(Start / Width - current / Width);
 172:
 173:             if (!GScore.ContainsKey(current))
 174:                 GScore.Add(current ,(xDistance + yDistance) * 10);
 175:
 176:             return GScore[current];
 177:         }
 178:
 179:         private int GetLowestF()
 180:         {
 181:             int temp = GetFScore(Open.Peek());
 182:             int lowest = Open.Peek();
 183:
 184:             foreach (int i in Open)
 185:             {
 186:                 if (temp > GetFScore(i))
 187:                 {
 188:                     temp = GetFScore(i);
 189:                     lowest = i;
 190:                 }
 191:             }
 192:
 193:             return lowest;
 194:         }
 195:
 196:         private int GetFScore(int pos)
 197:         {
 198:             if (!FScore.ContainsKey(pos))
 199:                 FScore.Add(pos, GetCost(pos) + GetHeuristic(pos));
 200:
 201:             return FScore[pos];
 202:         }
 203:
 204:         private Dictionary<int, int> GScore;
 205:         private Dictionary<int, int> FScore;
 206:         private Dictionary<int, int> ComeFrom;
 207:
 208:         // 得到预估的距离
 209:         private int GetHeuristic(int current)
 210:         {
 211:             return GetDistance(current, End);
 212:         }
 213:
 214:         private int GetDistance(int x, int y)
 215:         {
 216:             int xDistance = (int)Math.Abs(y % Width - x % Width);
 217:             int yDistance = (int)Math.Abs(y / Width - x / Width);
 218:
 219:             return 10 * (xDistance + yDistance);
 220:
 221:         }
 222:     }
 223: }

Update: 使用二项堆与SortedDictionary混合排序确实会提高效率，至于原因neoragex2002已经讲清楚了。
BinaryHeap适用于最小或者最大的排序，而SortedDictionary是平衡二叉树（红黑树），运行效率会高一些，再次感谢neoragex2002 :）

posted on 2008-07-15 01:02  xwang  阅读(1933)  评论(7编辑  收藏

• 随笔 - 92
• 文章 - 0
• 评论 - 252
• 引用 - 22