路径查找算法应用之A*算法
阅读原文时间:2022年03月17日阅读:1

环境: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 }

AStarAlgorithm

using System.Collections.Generic;
using System.Drawing;
using System.Linq;

namespace Pathfinding
{
///

/// 定位器(用于避障,查找相邻点或者对齐坐标等) ///
public class Locator
{
/// /// 画布 ///
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
{
///

/// 路径调制器 ///
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();  
    }

    /// <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
{
///

/// 有序链表 /// 此类不是线程安全的 ///
///
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<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
{
///

/// 方向 ///
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,
}
}

Orientation

namespace Pathfinding
{
public static class OrientationExtension
{
///

/// 是否为东西方向 ///
///
///
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
{
///

/// 获取第二个点相对于第一个点的方位 /// 此方法只判断是否为正南,正北,正东或正西四个方向。 /// 如果两个点坐标一样,则返回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;  
    }

    /// <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