环境：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 = start114 };115 this.m_goal = new Vertex()116 {117 Location = goal118 };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 else155 {156 path[idx] = endpoint;157 idx -= 1;158 }159 160 if (approximatePoint.X == path[idx].X)161 path[idx].X = endpoint.X;162 else163 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 else179 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 Neighbors196 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 = location210 };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 else235 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 = location251 };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 else276 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 = location292 };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 else317 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 = location333 };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 else358 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 Neighbors468 }469 }
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(); } }}
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; } } }}
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; } } }}
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, }}
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; } }}
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; } }}
