路徑查詢演算法應用之A*演算法
阿新 • • 發佈：2022-03-16
環境：Visual Studio 2017 + .Net Framework 4.5
應用場景：在畫板上查詢起始點和目標點之間的最短最直路徑，最後畫出連線兩個點之間的折線。
演算法簡介：A*演算法是一種效能較高的最優路徑搜尋演算法，由Stanford Research Institute(now SRI International)的Peter Hart，Nils Nilsson和Bertram Raphael於1968年發表。A*演算法可看做是對Dijkstra演算法的擴充套件和優化，其效能一般情況下比Dijkstra演算法快得多。在本文的應用場景中，（根據測試）通常比Dijkstra演算法快三倍以上，甚至可能比Dijkstra演算法快十幾倍甚至幾十倍。
A*演算法的應用範圍也比較廣泛，如機器人行走路徑規劃，遊戲中的NPC移動計算等。
更詳細的演算法說明請參考維基百科A* search algorithm
實現思想：
1，通過Locator把起始點座標和目標點座標對齊到步長（step，預設為20,）的整數倍。這樣，起始點和目標點就成了原來的起始點目標點的近似點。
2，把包含起始點和目標點的障礙物（如圖中所示，為矩形框）排除掉，不然折線遇到障礙物無法通過。
下圖中的矩形框的虛邊為避障區域，為了防止折線和障礙物碰撞。
3，把起始點新增到待遍歷點的集合中（本文中為SortedList<Vertex>）。
4，從待遍歷點的集合中取出第一個點（當前的最優點），遍歷其東、南、西、北四個方向的相鄰節點。東西兩個方向和當前點的Y座標相同，南北兩個方向和當前點的X座標相同。
相鄰點距當前點的距離為step引數設定的步長。step越大，搜尋速度越快，然而，可能導致折線無法通過間距較小的障礙物。
如果某個方向的相鄰點不存在，則建立新的相鄰點（如果相鄰點不在障礙物內部的話）；同時，設定新建立點的四個相鄰點（也許新建立點的相鄰點已經被建立了）。
把新建立的相鄰點的Visited屬性設定為false（當前實現中，Visited屬性預設為false），然後對新建立點的所有相鄰點排序，取最優點，設定為新建立點的前一個點（呼叫SetPrev方法）。
再把新建立的點新增到待遍歷點的集合中（本文中為SortedList<Vertex>）。
當遍歷完當前點的四個方向後，把當前點的Visited屬性設定為true，並從帶遍歷點的集合中移除。
說明：當前演算法的實現中僅考慮總距離（從起始點到當前點的距離加上猜測距離，起始點距離為0）、猜測距離（從當前點到目標點的距離，為從當前點到目標點的折線長度）和拐點個數（X或Y座標變化時拐點個數加1）。
5，遞迴第4步。要麼找到和目標點座標相同的點（即，找到了目標點），要麼待遍歷點的集合為空（即，無法找到通往目標點的路徑）。
6，當找到通往目標點的路徑之後，通過Straightener（調直器）調直路徑，減少拐點。
7，處理起始點和目標點。用原來的起始點和目標點替換座標對齊到step整數倍的起始點和目標點，並調直其相鄰拐點的X座標或Y座標。
8，返回最終路徑。
如下兩個圖所示
第一張圖為A*演算法查找出來的最優路徑（不一定是最短路徑，依賴於演算法的實現）
第二張圖為調直後的最直路徑（拐點最少）
程式碼
由於工程太大，僅上傳必要的程式碼檔案。
1 using System; 2 using System.Collections.Generic; 3 using System.Drawing; 4 5 namespace Pathfinding 6 { 7 /// <summary> 8 /// A*演算法 9 /// </summary> 10 public class AStarAlgorithm 11 { 12 private Vertex m_goal; 13 private Locator m_locator; 14 private SortedList<Vertex> m_openSet; 15 private Orientation m_orientation; 16 private Vertex m_start; 17 /// <summary> 18 /// 查詢最優路徑 19 /// </summary> 20 /// <param name="canvas">畫布</param> 21 /// <param name="obstacles">障礙物</param> 22 /// <param name="step">步長</param> 23 /// <param name="voDistance">避障距離</param> 24 /// <param name="initOrient">第一層查詢的方向</param> 25 /// <param name="start">起始點</param> 26 /// <param name="goal">目標點</param> 27 /// <returns></returns> 28 public Point[] Find(Rectangle canvas, List<Rectangle> obstacles, int step, int voDistance, Orientation initOrient, Point start, Point goal) 29 { 30 if (start == goal) 31 return null; 32 33 if (start.GetDistanceTo(goal) < step) 34 return this.ProcessShortPath(start, goal); 35 36 this.Init(canvas, obstacles, step, voDistance, initOrient, start, goal); 37 this.AddIntoOpenSet(this.m_start); 38 39 Vertex optimal = null; 40 while (this.m_openSet.Count > 0) 41 { 42 optimal = this.GetOptimalVertex(); 43 44 if (this.IsGoal(optimal)) 45 { 46 this.WalkTarget(); 47 var path = Straightener.Straighten(this.m_locator, this.m_goal.Lines); 48 this.ProcessEndpoint(start, 0, path); 49 this.ProcessEndpoint(goal, path.Length - 1, path); 50 51 return path; 52 } 53 54 this.Walk(optimal); 55 } 56 57 return null; 58 } 59 60 /// <summary> 61 /// 新增待遍歷的點 62 /// </summary> 63 /// <param name="vertex"></param> 64 private void AddIntoOpenSet(Vertex vertex) 65 { 66 if (!vertex.IsVisited) 67 this.m_openSet.Add(vertex); 68 } 69 70 /// <summary> 71 /// 獲取最優點 72 /// </summary> 73 /// <returns></returns> 74 private Vertex GetOptimalVertex() 75 { 76 var cheapest = this.m_openSet.TakeFirst(); 77 cheapest.IsCurrent = true; 78 79 return cheapest; 80 } 81 82 /// <summary> 83 /// 估算到目標點的距離 84 /// </summary> 85 /// <param name="vertex"></param> 86 /// <returns></returns> 87 private int GuessDistanceToGoal(Vertex vertex) 88 { 89 return Math.Abs(vertex.X - this.m_goal.X) + Math.Abs(vertex.Y - this.m_goal.Y); 90 } 91 92 /// <summary> 93 /// 初始化資料 94 /// </summary> 95 /// <param name="canvas"></param> 96 /// <param name="obstacles"></param> 97 /// <param name="step"></param> 98 /// <param name="voDistance"></param> 99 /// <param name="initOrient"></param> 100 /// <param name="start"></param> 101 /// <param name="goal"></param> 102 private void Init(Rectangle canvas, List<Rectangle> obstacles, int step, int voDistance, Orientation initOrient, Point start, Point goal) 103 { 104 this.m_locator = new Locator(canvas, obstacles, step, voDistance); 105 106 this.m_locator.AlignPoint(ref start); 107 this.m_locator.AlignPoint(ref goal); 108 this.m_locator.ExcludeObstacles(start); 109 this.m_locator.ExcludeObstacles(goal); 110 111 this.m_start = new Vertex() 112 { 113 Location = start 114 }; 115 this.m_goal = new Vertex() 116 { 117 Location = goal 118 }; 119 this.m_openSet = new SortedList<Vertex>(); 120 this.m_start.GuessDistance = this.GuessDistanceToGoal(this.m_start); 121 this.m_orientation = initOrient; 122 } 123 124 /// <summary> 125 /// 是否是目標點 126 /// </summary> 127 /// <param name="vertex"></param> 128 /// <returns></returns> 129 private bool IsGoal(Vertex vertex) 130 { 131 if (vertex.Location == this.m_goal.Location) 132 { 133 this.m_goal = vertex; 134 return true; 135 } 136 137 return false; 138 } 139 140 /// <summary> 141 /// 處理端點（起始點或目標點） 142 /// </summary> 143 /// <param name="endpoint"></param> 144 /// <param name="idx"></param> 145 /// <param name="path"></param> 146 private void ProcessEndpoint(Point endpoint, int idx, Point[] path) 147 { 148 Point approximatePoint = path[idx]; 149 if (0 == idx) 150 { 151 path[0] = endpoint; 152 idx += 1; 153 } 154 else 155 { 156 path[idx] = endpoint; 157 idx -= 1; 158 } 159 160 if (approximatePoint.X == path[idx].X) 161 path[idx].X = endpoint.X; 162 else 163 path[idx].Y = endpoint.Y; 164 } 165 166 /// <summary> 167 /// 處理短路徑 168 /// </summary> 169 /// <param name="start"></param> 170 /// <param name="goal"></param> 171 /// <returns></returns> 172 private Point[] ProcessShortPath(Point start, Point goal) 173 { 174 var dx = Math.Abs(goal.X - start.X); 175 var dy = Math.Abs(goal.Y - start.Y); 176 if (dx >= dy) 177 return new Point[] { start, new Point(goal.X, start.Y), goal }; 178 else 179 return new Point[] { start, new Point(start.X, goal.Y), goal }; 180 } 181 182 /// <summary> 183 /// 設定前一個點 184 /// </summary> 185 /// <param name="vertex"></param> 186 private void SetPrev(Vertex vertex) 187 { 188 var neighbors = vertex.Neighbors; 189 neighbors.Sort(); 190 vertex.SetPrev(neighbors[0]); 191 vertex.GuessDistance = this.GuessDistanceToGoal(vertex); 192 } 193 194 195 #region Traverse Neighbors 196 197 /// <summary> 198 /// 建立東邊的相鄰點 199 /// </summary> 200 /// <param name="vertex"></param> 201 private void CreateEastNeighbor(Vertex vertex) 202 { 203 var location = new Point(vertex.X + this.m_locator.Step, vertex.Y); 204 if (this.m_locator.AlignPoint(ref location) 205 && Orientation.East == vertex.Location.GetOrientation(location)) 206 { 207 var neighbor = new Vertex() 208 { 209 Location = location 210 }; 211 212 // ◐ 213 // | 214 // ◐---●---○ 215 // | 216 // ◐ 217 vertex.EastNeighbor = neighbor; 218 // ◐---◐ 219 // | | 220 // ◐---●---○ 221 // | 222 // ◐ 223 neighbor.NorthNeighbor = vertex.NorthNeighbor?.EastNeighbor; 224 // ◐ 225 // | 226 // ◐---●---○ 227 // | | 228 // ◐---◐ 229 neighbor.SouthNeighbor = vertex.SouthNeighbor?.EastNeighbor; 230 231 this.SetPrev(neighbor); 232 this.AddIntoOpenSet(neighbor); 233 } 234 else 235 vertex.CouldWalkEast = false; 236 } 237 238 /// <summary> 239 /// 建立北邊的相鄰點 240 /// </summary> 241 /// <param name="vertex"></param> 242 private void CreateNorthNeighbor(Vertex vertex) 243 { 244 var location = new Point(vertex.X, vertex.Y - this.m_locator.Step); 245 if (this.m_locator.AlignPoint(ref location) 246 && Orientation.North == vertex.Location.GetOrientation(location)) 247 { 248 var neighbor = new Vertex() 249 { 250 Location = location 251 }; 252 253 // ○ 254 // | 255 // ◐---●---◐ 256 // | 257 // ◐ 258 vertex.NorthNeighbor = neighbor; 259 // ○---◐ 260 // | | 261 // ◐---●---◐ 262 // | 263 // ◐ 264 neighbor.EastNeighbor = vertex.EastNeighbor?.NorthNeighbor; 265 // ◐---○ 266 // | | 267 // ◐---●---◐ 268 // | 269 // ◐ 270 neighbor.WestNeighbor = vertex.WestNeighbor?.NorthNeighbor; 271 272 this.SetPrev(neighbor); 273 this.AddIntoOpenSet(neighbor); 274 } 275 else 276 vertex.CouldWalkNorth = false; 277 } 278 279 /// <summary> 280 /// 建立南邊的相鄰點 281 /// </summary> 282 /// <param name="vertex"></param> 283 private void CreateSouthNeighbor(Vertex vertex) 284 { 285 var location = new Point(vertex.X, vertex.Y + this.m_locator.Step); 286 if (this.m_locator.AlignPoint(ref location) 287 && Orientation.South == vertex.Location.GetOrientation(location)) 288 { 289 var neighbor = new Vertex() 290 { 291 Location = location 292 }; 293 294 // ◐ 295 // | 296 // ◐---●---◐ 297 // | 298 // ○ 299 vertex.SouthNeighbor = neighbor; 300 // ◐ 301 // | 302 // ◐---●---◐ 303 // | | 304 // ○---◐ 305 neighbor.EastNeighbor = vertex.EastNeighbor?.SouthNeighbor; 306 // ◐ 307 // | 308 // ◐---●---◐ 309 // | | 310 // ◐---○ 311 neighbor.WestNeighbor = vertex.WestNeighbor?.SouthNeighbor; 312 313 this.SetPrev(neighbor); 314 this.AddIntoOpenSet(neighbor); 315 } 316 else 317 vertex.CouldWalkSouth = false; 318 } 319 320 /// <summary> 321 /// 建立西邊的相鄰點 322 /// </summary> 323 /// <param name="vertex"></param> 324 private void CreateWestNeighbor(Vertex vertex) 325 { 326 var location = new Point(vertex.X - this.m_locator.Step, vertex.Y); 327 if (this.m_locator.AlignPoint(ref location) 328 && Orientation.West == vertex.Location.GetOrientation(location)) 329 { 330 var neighbor = new Vertex() 331 { 332 Location = location 333 }; 334 335 // ◐ 336 // | 337 // ○---●---◐ 338 // | 339 // ◐ 340 vertex.WestNeighbor = neighbor; 341 // ◐ 342 // | 343 // ○---●---◐ 344 // | | 345 // ◐---◐ 346 neighbor.SouthNeighbor = vertex.SouthNeighbor?.WestNeighbor; 347 // ◐---◐ 348 // | | 349 // ○---●---◐ 350 // | 351 // ◐ 352 neighbor.NorthNeighbor = vertex.NorthNeighbor?.WestNeighbor; 353 354 this.SetPrev(neighbor); 355 this.AddIntoOpenSet(neighbor); 356 } 357 else 358 vertex.CouldWalkWest = false; 359 } 360 361 /// <summary> 362 /// <para>遍歷四個方位的相鄰點：東、南、西、北</para> 363 /// <para>●(實心圓)表示訪問過的點</para> 364 /// <para>◐(半實心圓)表示可能訪問過的點</para> 365 /// <para>○(空心圓)表示未訪問過的點</para> 366 /// </summary> 367 /// <param name="vertex"></param> 368 private void Walk(Vertex vertex) 369 { 370 // ◐ 371 // | 372 // ◐---●---◐ 373 // | 374 // ◐ 375 376 var count = 0; 377 while (count++ < 4) 378 { 379 switch (this.m_orientation) 380 { 381 case Orientation.East: 382 this.WalkEast(vertex); 383 this.m_orientation = Orientation.South; 384 break; 385 case Orientation.South: 386 this.WalkSouth(vertex); 387 this.m_orientation = Orientation.West; 388 break; 389 case Orientation.West: 390 this.WalkWest(vertex); 391 this.m_orientation = Orientation.North; 392 break; 393 case Orientation.North: 394 this.WalkNorth(vertex); 395 this.m_orientation = Orientation.East; 396 break; 397 default: 398 this.m_orientation = Orientation.East; 399 break; 400 } 401 } 402 403 vertex.IsVisited = true; 404 vertex.IsCurrent = false; 405 } 406 407 /// <summary> 408 /// 遍歷東邊的相鄰點 409 /// </summary> 410 /// <param name="vertex"></param> 411 private void WalkEast(Vertex vertex) 412 { 413 if (vertex.CouldWalkEast && vertex.EastNeighbor is null) 414 this.CreateEastNeighbor(vertex); 415 } 416 417 /// <summary> 418 /// 遍歷北邊的相鄰點 419 /// </summary> 420 /// <param name="vertex"></param> 421 private void WalkNorth(Vertex vertex) 422 { 423 if (vertex.CouldWalkNorth && vertex.NorthNeighbor is null) 424 this.CreateNorthNeighbor(vertex); 425 } 426 427 /// <summary> 428 /// 遍歷南邊的相鄰點 429 /// </summary> 430 /// <param name="vertex"></param> 431 private void WalkSouth(Vertex vertex) 432 { 433 if (vertex.CouldWalkSouth && vertex.SouthNeighbor is null) 434 this.CreateSouthNeighbor(vertex); 435 } 436 437 /// <summary> 438 /// 遍歷目標點 439 /// </summary> 440 private void WalkTarget() 441 { 442 // 遍歷目標點及其相鄰點 443 this.Walk(this.m_goal); 444 445 if (null != this.m_goal.EastNeighbor) 446 this.Walk(this.m_goal.EastNeighbor); 447 if (null != this.m_goal.SouthNeighbor) 448 this.Walk(this.m_goal.SouthNeighbor); 449 if (null != this.m_goal.WestNeighbor) 450 this.Walk(this.m_goal.WestNeighbor); 451 if (null != this.m_goal.NorthNeighbor) 452 this.Walk(this.m_goal.NorthNeighbor); 453 454 this.SetPrev(this.m_goal); 455 } 456 457 /// <summary> 458 /// 遍歷西邊的相鄰點 459 /// </summary> 460 /// <param name="vertex"></param> 461 private void WalkWest(Vertex vertex) 462 { 463 if (vertex.CouldWalkWest && vertex.WestNeighbor is null) 464 this.CreateWestNeighbor(vertex); 465 } 466 467 #endregion Traverse Neighbors 468 } 469 }AStarAlgorithm
using System.Collections.Generic; using System.Drawing; using System.Linq; namespace Pathfinding { /// <summary> /// 定位器（用於避障，查詢相鄰點或者對齊座標等） /// </summary> public class Locator { /// <summary> /// 畫布 /// </summary> private readonly Rectangle m_canvas; /// <summary> /// 障礙物 /// </summary> private readonly List<Rectangle> m_obstacles; /// <summary> /// 步長 /// </summary> private readonly int m_step; /// <summary> /// 避障距離 /// </summary> private readonly int m_voDistance; public Locator(Rectangle canvas, List<Rectangle> obstacles, int step = 20, int voDistance = 10) { this.m_canvas = canvas; if (step < 20) step = 20; this.m_step = (step >= 20) ? step : 20; this.m_voDistance = (voDistance >= 10) ? voDistance : 10; this.m_obstacles = this.BuildObstacles(obstacles); } /// <summary> /// 畫板 /// </summary> public Rectangle Canvas => this.m_canvas; /// <summary> /// 步長 /// </summary> public int Step => this.m_step; /// <summary> /// 避障距離 /// </summary> public int VODistance => this.m_voDistance; /// <summary> /// 對齊座標（把“點”的座標值對齊到Step的整數倍） /// </summary> /// <param name="point"></param> public Point AlignPoint(Point point) { return new Point(this.AlignCoord(point.X, 1), this.AlignCoord(point.Y, 1)); } /// <summary> /// <para>對齊座標（把“點”的座標值對齊到Step的整數倍，同時校驗“點”是否在畫布內或是否和障礙物衝突）</para> /// <para>如果對齊前或對齊後的“點”座標不在畫布內或者和障礙物衝突，則返回false；否則，返回true。</para> /// </summary> /// <param name="point"></param> /// <returns></returns> public bool AlignPoint(ref Point point) { if (!this.m_canvas.Contains(point)) return false; point = this.AlignPoint(point); if (!this.m_canvas.Contains(point)) return false; return !this.InObstacle(point); } /// <summary> /// 排除包含“點”的障礙物 /// </summary> /// <param name="point"></param> public void ExcludeObstacles(Point point) { this.m_obstacles.RemoveAll(o => o.Contains(point)); } /// <summary> /// 判斷點是否在障礙物內 /// </summary> /// <param name="point"></param> /// <returns></returns> public bool InObstacle(Point point) { return this.m_obstacles.Exists(obst => obst.Contains(point)); } /// <summary> /// <para>判斷水平線段或垂直線段（ab）是否和障礙物相交</para> /// <para>a、b的順序和結果無關</para> /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <returns></returns> public bool IntersectWithObstacle(Point a, Point b) { if (a.X == b.X) return this.IntersectVerticallyWithObstacle(a, b); else // a.Y == b.Y return this.IntersectHorizontallyWithObstacle(a, b); } /// <summary> /// 判斷水平線段(ab，其中a.X ≤ b.X)是否和障礙物相交 /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <returns></returns> public bool IntersectHorizontallyWithObstacle(Point a, Point b) { if (a.X > b.X) { var t = a; a = b; b = t; } return this.m_obstacles.Exists(obst => (obst.Top <= a.Y && a.Y <= obst.Bottom && ((a.X <= obst.Left && obst.Left <= b.X) || (a.X <= obst.Right && obst.Right <= b.X))) || obst.Contains(a) || obst.Contains(b)); } /// <summary> /// 判斷垂直線段(ab，其中a.Y ≤ b.Y)是否和障礙物相交 /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <returns></returns> public bool IntersectVerticallyWithObstacle(Point a, Point b) { if (a.Y > b.Y) { var t = a; a = b; b = t; } return this.m_obstacles.Exists(obst => (obst.Left <= a.X && a.X <= obst.Right && ((a.Y <= obst.Top && obst.Top <= b.Y) || (a.Y <= obst.Bottom && obst.Bottom <= b.Y))) || obst.Contains(a) || obst.Contains(b)); } /// <summary> /// 對齊座標的值 /// </summary> /// <param name="val"></param> /// <param name="direction"></param> /// <returns></returns> private int AlignCoord(int val, int direction) { int md = val % this.m_step; if (0 == md) return val; else if (md <= this.m_step / 2) return val - md; else return val - md + (direction * this.m_step); } /// <summary> /// 構造障礙物（用於除錯） /// </summary> /// <param name="obstacles"></param> /// <returns></returns> private List<Rectangle> BuildDebugObstacles(List<Rectangle> obstacles) { if (obstacles is null || obstacles.Count <= 0) return new List<Rectangle>(); else return obstacles.Select(o => new Rectangle(o.X - this.m_voDistance, o.Y - this.m_voDistance, o.Width + this.m_voDistance * 2, o.Height + m_voDistance * 2)).ToList(); } /// <summary> /// 構造障礙物 /// </summary> /// <param name="obstacles"></param> /// <returns></returns> private List<Rectangle> BuildObstacles(List<Rectangle> obstacles) { if (obstacles is null || obstacles.Count <= 0) return new List<Rectangle>(); else return obstacles.Select(o => new Rectangle(o.X - this.m_voDistance + 1, o.Y - this.m_voDistance + 1, o.Width + this.m_voDistance * 2 - 2, o.Height + m_voDistance * 2 - 2)).ToList(); } } }Locator
using System; using System.Collections.Generic; using System.Drawing; using System.Linq; namespace Pathfinding { /// <summary> /// 路徑調製器 /// </summary> public class Straightener { /// <summary> /// 滿足調直的前提條件（路徑最少包含4個點） /// </summary> private const int MIN_COUNT_POINTS = 4; /// <summary> /// 定位器 /// </summary> private Locator m_locator; /// <summary> /// 原始路徑 /// </summary> private LinkedList<Point> m_originalPath; /// <summary> /// 調直後的路徑 /// </summary> private LinkedList<Point> m_straightenedPath; private Straightener() { this.Reset(); } /// <summary> /// 調直路徑，減少拐點（引數為空或小於4個點不做任何處理） /// </summary> /// <param name="path"></param> /// <returns></returns> public static Point[] Straighten(Locator locator, Point[] path) { if (locator is null || path is null || path.Length < MIN_COUNT_POINTS) return path; var worker = new Straightener(); worker.m_locator = locator; worker.m_originalPath = new LinkedList<Point>(path); worker.Straighten(); return worker.m_straightenedPath.ToArray(); } /// <summary> /// 建立假設的拐點 /// </summary> /// <param name="node"></param> /// <returns></returns> private static Point CreateHypotheticalInflection(LinkedListNode<Point> node) { if (node.Value.X == node.Next.Value.X) return new Point(node.Next.Next.Value.X, node.Value.Y); else return new Point(node.Value.X, node.Next.Next.Value.Y); } /// <summary> /// 計算線段（abcd）上拐點的個數 /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <param name="c"></param> /// <param name="d"></param> /// <returns></returns> private static int GetCountInflections(Point a, Point b, Point c, Point d) { var inflections = 0; if (c.X != a.X && c.Y != a.Y) inflections++; if (d.X != b.X && d.Y != b.Y) inflections++; return inflections; } private static int GetDistance(Point a, Point b) { if (a.X == b.X) return Math.Abs(a.Y - b.Y); else return Math.Abs(a.X - b.X); } /// <summary> /// 新增拐點 /// </summary> /// <param name="inflection"></param> private void AddInflection(Point inflection) { if (null != this.m_straightenedPath.Last && this.m_straightenedPath.Last.Value == inflection) return; var last = this.m_straightenedPath.AddLast(inflection); if (null != last.Previous?.Previous && (last.Value.X == last.Previous.Previous.Value.X || last.Value.Y == last.Previous.Previous.Value.Y)) this.m_straightenedPath.Remove(last.Previous); } private void DoStraighten() { this.Reset(); var current = this.m_originalPath.First; while (null != current.Next?.Next) { this.AddInflection(current.Value); this.RemoveRedundantInflections(current); if (current.Next?.Next is null) break; var inflection = CreateHypotheticalInflection(current); if (!this.IntersectWithObstacle(current.Value, inflection, current.Next.Next.Value)) { var success = false; if (null != current.Previous) { var i1 = GetCountInflections(current.Previous.Value, current.Value, current.Next.Value, current.Next.Next.Value); var i2 = GetCountInflections(current.Previous.Value, current.Value, inflection, current.Next.Next.Value); if (i2 < i1) { current.Next.Value = inflection; success = true; } } else if (null != current.Next?.Next?.Next) { var i3 = GetCountInflections(current.Value, current.Next.Value, current.Next.Next.Value, current.Next.Next.Next.Value); var i4 = GetCountInflections(current.Value, inflection, current.Next.Next.Value, current.Next.Next.Next.Value); if (i4 < i3) { current.Next.Value = inflection; success = true; } } if (success) { this.RemoveRedundantInflections(current.Next); if (current.Next?.Next is null) break; } } this.RemoveTurnBackInflections(current); current = current.Next; } this.AddInflection(this.m_originalPath.Last.Previous.Value); this.AddInflection(this.m_originalPath.Last.Value); } private int GetDistance() { var dist = 0; var current = this.m_straightenedPath.First; do { if (null != current.Next) { dist += GetDistance(current.Value, current.Next.Value); current = current.Next; } else break; } while (true); return dist; } /// <summary> /// 判斷線段（abc）是否和障礙物相交 /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <param name="c"></param> /// <returns></returns> private bool IntersectWithObstacle(Point a, Point b, Point c) { return this.m_locator.IntersectWithObstacle(a, b) || this.m_locator.IntersectWithObstacle(b, c); } /// <summary> /// 刪除冗餘拐點 /// </summary> /// <param name="node"></param> private void RemoveRedundantInflections(LinkedListNode<Point> node) { while (true) { if (node.Next?.Next is null) break; if (node.Value.X == node.Next.Next.Value.X || node.Value.Y == node.Next.Next.Value.Y) this.m_originalPath.Remove(node.Next); else break; } } /// <summary> /// 刪除迴轉拐點 /// </summary> /// <param name="node"></param> private void RemoveTurnBackInflections(LinkedListNode<Point> node) { if (node.Next?.Next?.Next is null) return; var point = node.Value; var nPoint = node.Next.Value; var nnPoint = node.Next.Next.Value; var nnnPoint = node.Next.Next.Next.Value; // ●為已知拐點；○為假設拐點 // 消除如下形式的拐點 // ● // | // ○---● // | | // ●---● if (point.X == nPoint.X && nPoint.Y == nnPoint.Y && nnPoint.X == nnnPoint.X) { var dy1 = point.Y - nPoint.Y; var dy2 = nnnPoint.Y - nnPoint.Y; var p1 = new Point(nnnPoint.X, point.Y); if (Math.Abs(dy2) >= Math.Abs(dy1) && Math.Abs(dy1) / dy1 == Math.Abs(dy2) / dy2 && !this.m_locator.IntersectHorizontallyWithObstacle(node.Value, p1)) { this.m_originalPath.Remove(node.Next); this.m_originalPath.Remove(node.Next); this.m_originalPath.AddAfter(node, p1); } } // ●為已知拐點；○為假設拐點 // 消除如下形式的拐點 // ●---○---● // | | // ●---● else if (point.Y == nPoint.Y && nPoint.X == nnPoint.X && nnPoint.Y == nnnPoint.Y) { var dx1 = point.X - nPoint.X; var dx2 = nnnPoint.X - nnPoint.X; var p2 = new Point(point.X, nnnPoint.Y); if (Math.Abs(dx2) >= Math.Abs(dx1) && Math.Abs(dx1) / dx1 == Math.Abs(dx2) / dx2 && !this.m_locator.IntersectVerticallyWithObstacle(node.Value, p2)) { this.m_originalPath.Remove(node.Next); this.m_originalPath.Remove(node.Next); this.m_originalPath.AddAfter(node, p2); } } } private void Reset() { this.m_straightenedPath = new LinkedList<Point>(); } private void Straighten() { int prevDistance = 0; int prevInflections = 0; int distance = 0; int inflections = 0; while (true) { this.DoStraighten(); this.m_originalPath = this.m_straightenedPath; distance = this.GetDistance(); inflections = this.m_originalPath.Count; if (distance == prevDistance && inflections == prevInflections) break; prevDistance = distance; prevInflections = inflections; } } } }Straightener
using System; using System.Collections; using System.Collections.Generic; namespace Pathfinding { /// <summary> /// <para>有序連結串列</para> /// <para>此類不是執行緒安全的</para> /// </summary> /// <typeparam name="T"></typeparam> public class SortedList<T> : IEnumerable<T> where T : IComparable<T> { private int m_count = 0; private Node m_first; private Node m_last; public SortedList() { // do nothing } public SortedList(IEnumerable<T> collection) { this.AddRange(collection); } /// <summary> /// 連結串列中的元素個數 /// </summary> public int Count => this.m_count; /// <summary> /// 第一個元素 /// </summary> public T First { get { if (null != this.m_first) return this.m_first.Value; else return default(T); } } public bool IsEmpty => this.m_count <= 0; /// <summary> /// 最後一個元素 /// </summary> public T Last { get { if (null != this.m_last) return this.m_last.Value; else return default(T); } } public void Add(T value) { var node = new Node(value); if (this.IsEmpty) { this.m_first = node; this.m_last = node; } else if (value.CompareTo(this.m_first.Value) < 0) { node.Next = this.m_first; this.m_first.Prev = node; this.m_first = node; } else if (this.m_last.Value.CompareTo(value) <= 0) { node.Prev = this.m_last; this.m_last.Next = node; this.m_last = node; } else { Node current = this.m_first; do { if (value.CompareTo(current.Value) < 0) break; current = current.Next; } while (current != null); var prev = current.Prev; prev.Next = node; node.Prev = prev; node.Next = current; current.Prev = node; } this.m_count++; } public void AddRange(IEnumerable<T> collection) { if (collection is null) return; foreach (var item in collection) this.Add(item); } /// <summary> /// 清除所有元素 /// </summary> public void Clear() { this.m_first = null; this.m_last = null; this.m_count = 0; } /// <summary> /// 判斷連結串列是否包含指定的元素 /// </summary> /// <param name="value"></param> /// <returns></returns> public bool Contains(T value) { if (this.IsEmpty) return false; var current = this.m_first; while (null != current) { if (value.CompareTo(current.Value) == 0) return true; current = current.Next; } return false; } public int IndexOf(T value) { if (this.IsEmpty) return -1; var current = this.m_first; var idx = 0; while (null != current) { if (value.CompareTo(current.Value) == 0) return idx; idx++; current = current.Next; } return -1; } /// <summary> /// 獲取IEnumerator<T> /// </summary> /// <returns></returns> public IEnumerator<T> GetEnumerator() { return new Enumerator(this); } IEnumerator IEnumerable.GetEnumerator() { return this.GetEnumerator(); } /// <summary> /// 刪除指定的元素 /// </summary> /// <param name="value"></param> public void Remove(T value) { if (this.IsEmpty) return; var current = this.m_first; while (null != current) { if (value.CompareTo(current.Value) == 0) break; current = current.Next; } if (null != current) { var prev = current.Prev; var next = current.Next; if (null != prev && null != next) { prev.Next = next; next.Prev = prev; } else if (null != prev) this.SetLast(prev); else this.SetFirst(next); this.m_count--; } } /// <summary> /// 返回並刪除第一個元素 /// </summary> /// <returns></returns> public T TakeFirst() { if (this.IsEmpty) return default(T); var value = this.m_first.Value; this.SetFirst(this.m_first.Next); this.m_count--; return value; } /// <summary> /// 返回並刪除最後一個元素 /// </summary> /// <returns></returns> public T TakeLast() { if (this.IsEmpty) return default(T); var value = this.m_last.Value; this.SetLast(this.m_last.Prev); this.m_count--; return value; } public T[] ToArray() { if (this.IsEmpty) return null; var a = new T[this.m_count]; var current = this.m_first; var idx = 0; while (null != current) { a[idx++] = current.Value; current = current.Next; } return a; } public List<T> ToList() { if (this.IsEmpty) return null; var l = new List<T>(this.m_count); var current = this.m_first; while (null != current) { l.Add(current.Value); } return l; } private void SetFirst(Node first) { this.m_first = first; if (this.m_first is null) this.m_last = null; else this.m_first.Prev = null; } private void SetLast(Node last) { this.m_last = last; if (this.m_last is null) this.m_first = null; else this.m_last.Next = null; } /// <summary> /// 列舉器 /// </summary> private class Enumerator : IEnumerator<T> { private readonly SortedList<T> m_list; private readonly Node m_prevFirst = new Node(default(T)); private Node m_current; public Enumerator(SortedList<T> list) { this.m_list = list; this.Reset(); } public T Current { get { if (null != this.m_current) return this.m_current.Value; else return default(T); } } object IEnumerator.Current { get { if (null != this.m_current) return this.m_current.Value; else return default(T); } } public void Dispose() { // do nothing } public bool MoveNext() { if (object.ReferenceEquals(this.m_current, this.m_prevFirst)) this.m_current = this.m_list.m_first; else this.m_current = this.m_current?.Next; return null != this.m_current; } public void Reset() { this.m_current = this.m_prevFirst; } } /// <summary> /// 連結串列節點 /// </summary> private class Node { public Node(T data) { this.Value = data; } public Node Next { get; set; } public Node Prev { get; set; } public T Value { get; } public override string ToString() { if (null != Value) return Value.ToString(); return null; } } } }SortedList
namespace Pathfinding { /// <summary> /// 方向 /// </summary> public enum Orientation { /// <summary> /// 無方向 /// </summary> None = 0, /// <summary> /// 東 /// </summary> East = 0x1, /// <summary> /// 南 /// </summary> South = 0x10, /// <summary> /// 西 /// </summary> West = 0x100, /// <summary> /// 北 /// </summary> North = 0x1000, /// <summary> /// 東西 /// </summary> EastWest = East | West, /// <summary> /// 南北 /// </summary> NorthSouth = South | North, /// <summary> /// 東南 /// </summary> SouthEast = East | South, /// <summary> /// 西南 /// </summary> SouthWest = South | West, /// <summary> /// 西北 /// </summary> NorthWest = West | North, /// <summary> /// 東北 /// </summary> NorthEast = North | East, } }Orientation
namespace Pathfinding { public static class OrientationExtension { /// <summary> /// 是否為東西方向 /// </summary> /// <param name="orient"></param> /// <returns></returns> public static bool IsEastWest(this Orientation orient) { return orient == Orientation.East || orient == Orientation.West || orient == Orientation.EastWest; } /// <summary> /// 是否為南北方向 /// </summary> /// <param name="orient"></param> /// <returns></returns> public static bool IsNorthSouth(this Orientation orient) { return orient == Orientation.South || orient == Orientation.North || orient == Orientation.NorthSouth; } /// <summary> /// <para>把方向轉換為EastWest或NorthSouth</para> /// <para>如果方向不是東西方向或南北方向，則返回None</para> /// </summary> /// <param name="orient"></param> /// <returns></returns> public static Orientation ConvertToEWOrNS(this Orientation orient) { if (orient.IsEastWest()) return Orientation.EastWest; else if (orient.IsNorthSouth()) return Orientation.NorthSouth; else return Orientation.None; } } }OrientationExtension
using System; using System.Drawing; namespace Pathfinding { public static class PointExtension { /// <summary> /// <para>獲取第二個點相對於第一個點的方位</para> /// <para>此方法只判斷是否為正南，正北，正東或正西四個方向。</para> /// <para>如果兩個點座標一樣，則返回Orientation.None。</para> /// </summary> /// <param name="from"></param> /// <param name="to"></param> /// <returns>East、South、West、North</returns> public static Orientation GetOrientation(this Point from, Point to) { if (from.X == to.X) { if (to.Y > from.Y) return Orientation.South; else if (to.Y < from.Y) return Orientation.North; } else if (from.Y == to.Y) { if (to.X > from.X) return Orientation.East; else if (to.X < from.X) return Orientation.West; } return Orientation.None; } /// <summary> /// <para>判斷兩點之間的相對位置：東西方向或南北方向</para> /// <para>如果兩個點座標一樣或不是東西或南北方向，則返回Orientation.None。</para> /// </summary> /// <param name="from"></param> /// <param name="to"></param> /// <returns>EastWest或NorthSouth</returns> public static Orientation GetOrientationEWOrNS(this Point from, Point to) { if (from.X == to.X && to.Y != from.Y) { return Orientation.NorthSouth; } else if (from.Y == to.Y && to.X != from.X) { return Orientation.EastWest; } return Orientation.None; } /// <summary> /// 兩點的位置是否為東西方向：Y座標相等，且X座標不相等 /// </summary> /// <param name="from"></param> /// <param name="to"></param> /// <returns></returns> public static bool IsEastWest(this Point from, Point to) { return from.Y == to.Y && to.X != from.X; } /// <summary> /// 兩點的位置是否為南北方向：X座標相等，且Y座標不相等 /// </summary> /// <param name="from"></param> /// <param name="to"></param> /// <returns></returns> public static bool IsNorthSouth(this Point from, Point to) { return from.X == to.X && to.Y != from.Y; } /// <summary> /// 計算兩點之間的距離（僅計算東西和南北方向的距離） /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <returns></returns> public static int GetAlignedDistanceTo(this Point a, Point b) { if (a.IsEastWest(b)) return Math.Abs(a.X - b.X); else return Math.Abs(a.Y - b.Y); } /// <summary> /// 計算兩點之間的距離 /// </summary> /// <param name="from"></param> /// <param name="to"></param> /// <returns></returns> public static double GetDistanceTo(this Point from, Point to) { return Math.Sqrt(Math.Pow(from.X - to.X, 2.0d) + Math.Pow(from.Y - to.Y, 2.0d)); } /// <summary> /// 判斷兩個點是否在東西方向或南北方向的同一條直線上 /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <returns></returns> public static bool InStraightLine(this Point a, Point b) { return a.X == b.X || a.Y == b.Y; } } }PointExtension