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 每个点的父亲关系

操作概要解释:

每次都找他周围看起来最容易达到终点的元素,如果周围的点G值比他小,则先走G值小的位置,再判定该位置F值最小的点,不停循环,直到找到终点为止。

该算法需要先走遍G值小的部分,而后,才会走F值的判断,这样先一直往前,如果不行,则找看起来最有可能快到终点的元素,这样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: }

 

有同志说,用队列的方式效率低下,的确如此,用二项堆这样的方法会快一些,毕竟开始就是排好序的,奇怪的是,我自己写一个堆排序结果却不好,郁闷的紧。

另外用SortDictionary做了测试,最小值做的Value, 没有做Key,因为Key只能用标记点的位置代表

代码包:AStar_Source

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

输出结果如下:

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

导航

统计