环境：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
4，从待遍历点的集合中取出第一个点（当前的最优点），遍历其东、南、西、北四个方向的相邻节点。东西两个方向和当前点的Y坐标相同，南北两个方向和当前点的X坐标相同。
相邻点距当前点的距离为step参数设定的步长。step越大，搜索速度越快，然而，可能导致折线无法通过间距较小的障碍物。
如果某个方向的相邻点不存在，则创建新的相邻点（如果相邻点不在障碍物内部的话）；同时，设置新创建点的四个相邻点（也许新创建点的相邻点已经被创建了）。
把新创建的相邻点的Visited属性设置为false（当前实现中，Visited属性默认为false），然后对新创建点的所有相邻点排序，取最优点，设置为新创建点的前一个点（调用SetPrev方法）。
再把新创建的点添加到待遍历点的集合中（本文中为SortedList
当遍历完当前点的四个方向后，把当前点的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 ///8 /// A*算法 9 /// 10 public class AStarAlgorithm 11 { 12 private Vertex m_goal; 13 private Locator m_locator; 14 private SortedList m_openSet; 15 private Orientation m_orientation; 16 private Vertex m_start; 17 /// 18 /// 查找最优路径 19 /// 20 /// 画布 21 /// 障碍物 22 /// 步长 23 /// 避障距离 24 /// 第一层查找的方向 25 /// 起始点 26 /// 目标点 27 /// 28 public Point[] Find(Rectangle canvas, List 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 /// 61 /// 添加待遍历的点 62 /// 63 /// 64 private void AddIntoOpenSet(Vertex vertex) 65 { 66 if (!vertex.IsVisited) 67 this.m_openSet.Add(vertex); 68 } 69 70 /// 71 /// 获取最优点 72 /// 73 /// 74 private Vertex GetOptimalVertex() 75 { 76 var cheapest = this.m_openSet.TakeFirst(); 77 cheapest.IsCurrent = true; 78 79 return cheapest; 80 } 81 82 /// 83 /// 估算到目标点的距离 84 /// 85 /// 86 /// 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 /// 93 /// 初始化数据 94 /// 95 /// 96 /// 97 /// 98 /// 99 /// 100 /// 101 /// 102 private void Init(Rectangle canvas, List 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 (); 120 this.m_start.GuessDistance = this.GuessDistanceToGoal(this.m_start); 121 this.m_orientation = initOrient; 122 } 123 124 /// 125 /// 是否是目标点 126 /// 127 /// 128 /// 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 /// 141 /// 处理端点（起始点或目标点） 142 /// 143 /// 144 /// 145 /// 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 /// 167 /// 处理短路径 168 /// 169 /// 170 /// 171 /// 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 /// 183 /// 设置前一个点 184 /// 185 /// 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 /// 198 /// 创建东边的相邻点 199 /// 200 /// 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 /// 239 /// 创建北边的相邻点 240 /// 241 /// 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 /// 280 /// 创建南边的相邻点 281 /// 282 /// 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 /// 321 /// 创建西边的相邻点 322 /// 323 /// 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 /// 362 /// 遍历四个方位的相邻点：东、南、西、北 363 /// ●(实心圆)表示访问过的点 364 /// ◐(半实心圆)表示可能访问过的点 365 /// ○(空心圆)表示未访问过的点 366 /// 367 /// 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 /// 408 /// 遍历东边的相邻点 409 /// 410 /// 411 private void WalkEast(Vertex vertex) 412 { 413 if (vertex.CouldWalkEast && vertex.EastNeighbor is null) 414 this.CreateEastNeighbor(vertex); 415 } 416 417 /// 418 /// 遍历北边的相邻点 419 /// 420 /// 421 private void WalkNorth(Vertex vertex) 422 { 423 if (vertex.CouldWalkNorth && vertex.NorthNeighbor is null) 424 this.CreateNorthNeighbor(vertex); 425 } 426 427 /// 428 /// 遍历南边的相邻点 429 /// 430 /// 431 private void WalkSouth(Vertex vertex) 432 { 433 if (vertex.CouldWalkSouth && vertex.SouthNeighbor is null) 434 this.CreateSouthNeighbor(vertex); 435 } 436 437 /// 438 /// 遍历目标点 439 /// 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 /// 458 /// 遍历西边的相邻点 459 /// 460 /// 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 }
using System.Collections.Generic; using System.Drawing; using System.Linq; namespace Pathfinding { ////// 定位器（用于避障，查找相邻点或者对齐坐标等） /// public class Locator { /// /// 画布 /// private readonly Rectangle m_canvas; /// /// 障碍物 /// private readonly List m_obstacles; /// /// 步长 /// private readonly int m_step; /// /// 避障距离 /// private readonly int m_voDistance; public Locator(Rectangle canvas, List 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); } /// /// 画板 /// public Rectangle Canvas => this.m_canvas; /// /// 步长 /// public int Step => this.m_step; /// /// 避障距离 /// public int VODistance => this.m_voDistance; /// /// 对齐坐标（把“点”的坐标值对齐到Step的整数倍） /// /// public Point AlignPoint(Point point) { return new Point(this.AlignCoord(point.X, 1), this.AlignCoord(point.Y, 1)); } /// /// 对齐坐标（把“点”的坐标值对齐到Step的整数倍，同时校验“点”是否在画布内或是否和障碍物冲突） /// 如果对齐前或对齐后的“点”坐标不在画布内或者和障碍物冲突，则返回false；否则，返回true。 /// /// /// 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); } /// /// 排除包含“点”的障碍物 /// /// public void ExcludeObstacles(Point point) { this.m_obstacles.RemoveAll(o => o.Contains(point)); } /// /// 判断点是否在障碍物内 /// /// /// public bool InObstacle(Point point) { return this.m_obstacles.Exists(obst => obst.Contains(point)); } /// /// 判断水平线段或垂直线段（ab）是否和障碍物相交 /// a、b的顺序和结果无关 /// /// /// /// 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); } /// /// 判断水平线段(ab，其中a.X ≤ b.X)是否和障碍物相交 /// /// /// /// 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)); } /// /// 判断垂直线段(ab，其中a.Y ≤ b.Y)是否和障碍物相交 /// /// /// /// 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)); } /// /// 对齐坐标的值 /// /// /// /// 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); } /// /// 构造障碍物（用于调试） /// /// /// private List BuildDebugObstacles(List obstacles) { if (obstacles is null || obstacles.Count <= 0) return new List (); 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(); } /// /// 构造障碍物 /// /// /// private List BuildObstacles(List obstacles) { if (obstacles is null || obstacles.Count <= 0) return new List (); 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(); } } }
using System; using System.Collections.Generic; using System.Drawing; using System.Linq; namespace Pathfinding { ////// 路径调制器 /// public class Straightener { /// /// 满足调直的前提条件（路径最少包含4个点） /// private const int MIN_COUNT_POINTS = 4; /// /// 定位器 /// private Locator m_locator; /// /// 原始路径 /// private LinkedList m_originalPath; /// /// 调直后的路径 /// private LinkedList m_straightenedPath; private Straightener() { this.Reset(); } /// /// 调直路径，减少拐点（参数为空或小于4个点不做任何处理） /// /// /// 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 (path); worker.Straighten(); return worker.m_straightenedPath.ToArray(); } /// /// 创建假设的拐点 /// /// /// private static Point CreateHypotheticalInflection(LinkedListNode 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); } /// /// 计算线段（abcd）上拐点的个数 /// /// /// /// /// /// 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); } /// /// 添加拐点 /// /// 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; } /// /// 判断线段（abc）是否和障碍物相交 /// /// /// /// /// private bool IntersectWithObstacle(Point a, Point b, Point c) { return this.m_locator.IntersectWithObstacle(a, b) || this.m_locator.IntersectWithObstacle(b, c); } /// /// 删除冗余拐点 /// /// private void RemoveRedundantInflections(LinkedListNode 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; } } /// /// 删除回转拐点 /// /// private void RemoveTurnBackInflections(LinkedListNode 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 (); } 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; } } } }
using System; using System.Collections; using System.Collections.Generic; namespace Pathfinding { ////// 有序链表 /// 此类不是线程安全的 /// /// public class SortedList : IEnumerable where T : IComparable { private int m_count = 0; private Node m_first; private Node m_last; public SortedList() { // do nothing } public SortedList(IEnumerable collection) { this.AddRange(collection); } /// /// 链表中的元素个数 /// public int Count => this.m_count; /// /// 第一个元素 /// 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; /// /// 最后一个元素 /// 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 collection) { if (collection is null) return; foreach (var item in collection) this.Add(item); } /// /// 清除所有元素 /// public void Clear() { this.m_first = null; this.m_last = null; this.m_count = 0; } /// /// 判断链表是否包含指定的元素 /// /// /// 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; } /// /// 获取IEnumerator /// /// public IEnumerator GetEnumerator() { return new Enumerator(this); } IEnumerator IEnumerable.GetEnumerator() { return this.GetEnumerator(); } /// /// 删除指定的元素 /// /// 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--; } } /// /// 返回并删除第一个元素 /// /// 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; } /// /// 返回并删除最后一个元素 /// /// 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 ToList() { if (this.IsEmpty) return null; var l = new List (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; } /// /// 枚举器 /// private class Enumerator : IEnumerator { private readonly SortedList m_list; private readonly Node m_prevFirst = new Node(default(T)); private Node m_current; public Enumerator(SortedList 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; } } /// /// 链表节点 /// 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; } } } }
namespace Pathfinding { ////// 方向 /// public enum Orientation { /// /// 无方向 /// None = 0, /// /// 东 /// East = 0x1, /// /// 南 /// South = 0x10, /// /// 西 /// West = 0x100, /// /// 北 /// North = 0x1000, /// /// 东西 /// EastWest = East | West, /// /// 南北 /// NorthSouth = South | North, /// /// 东南 /// SouthEast = East | South, /// /// 西南 /// SouthWest = South | West, /// /// 西北 /// NorthWest = West | North, /// /// 东北 /// NorthEast = North | East, } }
namespace Pathfinding { public static class OrientationExtension { ////// 是否为东西方向 /// /// /// public static bool IsEastWest(this Orientation orient) { return orient == Orientation.East || orient == Orientation.West || orient == Orientation.EastWest; } /// /// 是否为南北方向 /// /// /// public static bool IsNorthSouth(this Orientation orient) { return orient == Orientation.South || orient == Orientation.North || orient == Orientation.NorthSouth; } /// /// 把方向转换为EastWest或NorthSouth /// 如果方向不是东西方向或南北方向，则返回None /// /// /// public static Orientation ConvertToEWOrNS(this Orientation orient) { if (orient.IsEastWest()) return Orientation.EastWest; else if (orient.IsNorthSouth()) return Orientation.NorthSouth; else return Orientation.None; } } }
using System; using System.Drawing; namespace Pathfinding { public static class PointExtension { ////// 获取第二个点相对于第一个点的方位 /// 此方法只判断是否为正南，正北，正东或正西四个方向。 /// 如果两个点坐标一样，则返回Orientation.None。 /// /// /// /// East、South、West、North 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; } /// /// 判断两点之间的相对位置：东西方向或南北方向 /// 如果两个点坐标一样或不是东西或南北方向，则返回Orientation.None。 /// /// /// /// EastWest或NorthSouth 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; } /// /// 两点的位置是否为东西方向：Y坐标相等，且X坐标不相等 /// /// /// /// public static bool IsEastWest(this Point from, Point to) { return from.Y == to.Y && to.X != from.X; } /// /// 两点的位置是否为南北方向：X坐标相等，且Y坐标不相等 /// /// /// /// public static bool IsNorthSouth(this Point from, Point to) { return from.X == to.X && to.Y != from.Y; } /// /// 计算两点之间的距离（仅计算东西和南北方向的距离） /// /// /// /// 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); } /// /// 计算两点之间的距离 /// /// /// /// 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)); } /// /// 判断两个点是否在东西方向或南北方向的同一条直线上 /// /// /// /// public static bool InStraightLine(this Point a, Point b) { return a.X == b.X || a.Y == b.Y; } } }