路径查找算法应用之A*算法

应用A*路径搜索算法在画板上查找连接起始节点和终止节点之间的最短最直的折线。

环境:Visual Studio 2017 + .Net Framework 4.5

应用场景:在画板上查找起始点和目标点之间的最短最直路径,最后画出连接两个点之间的折线。

算法简介:A*算法是一种性能较高的最优路径搜索算法,由Stanford Research Institute(now SRI International)的Peter Hart,Nils Nilsson和Bertram Raphael于1968年发表。A*算法可看做是对Dijkstra算法的扩展和优化,其性能一般情况下比Dijkstra算法快得多。在本文的应用场景中,(根据测试)通常比Dijkstra算法快三倍以上,甚至可能比Dijkstra算法快十几倍甚至几十倍。

A*算法的应用范围也比较广泛,如机器人行走路径规划,游戏中的NPC移动计算等。

更详细的算法说明请参考维基百科A* search algorithm

 

实现思想:

1,通过Locator把起始点坐标和目标点坐标对齐到步长(step,默认为20,)的整数倍。这样,起始点和目标点就成了原来的起始点目标点的近似点。

2,把包含起始点和目标点的障碍物(如图中所示,为矩形框)排除掉,不然折线遇到障碍物无法通过。

下图中的矩形框的虚边为避障区域,为了防止折线和障碍物碰撞。

3,把起始点添加到待遍历点的集合中(本文中为SortedList<Vertex>)。

4,从待遍历点的集合中取出第一个点(当前的最优点),遍历其东、南、西、北四个方向的相邻节点。东西两个方向和当前点的Y坐标相同,南北两个方向和当前点的X坐标相同。

相邻点距当前点的距离为step参数设定的步长。step越大,搜索速度越快,然而,可能导致折线无法通过间距较小的障碍物。

如果某个方向的相邻点不存在,则创建新的相邻点(如果相邻点不在障碍物内部的话);同时,设置新创建点的四个相邻点(也许新创建点的相邻点已经被创建了)。

把新创建的相邻点的Visited属性设置为false(当前实现中,Visited属性默认为false),然后对新创建点的所有相邻点排序,取最优点,设置为新创建点的前一个点(调用SetPrev方法)。

再把新创建的点添加到待遍历点的集合中(本文中为SortedList<Vertex>)。

当遍历完当前点的四个方向后,把当前点的Visited属性设置为true,并从带遍历点的集合中移除。

说明:当前算法的实现中仅考虑总距离(从起始点到当前点的距离加上猜测距离,起始点距离为0)、猜测距离(从当前点到目标点的距离,为从当前点到目标点的折线长度)和拐点个数(X或Y坐标变化时拐点个数加1)。

5,递归第4步。要么找到和目标点坐标相同的点(即,找到了目标点),要么待遍历点的集合为空(即,无法找到通往目标点的路径)。

6,当找到通往目标点的路径之后,通过Straightener(调直器)调直路径,减少拐点。

7,处理起始点和目标点。用原来的起始点和目标点替换坐标对齐到step整数倍的起始点和目标点,并调直其相邻拐点的X坐标或Y坐标。

8,返回最终路径。

 

如下两个图所示

第一张图为A*算法查找出来的最优路径(不一定是最短路径,依赖于算法的实现)

第二张图为调直后的最直路径(拐点最少)

 

 

代码

由于工程太大,仅上传必要的代码文件。


  1 using System;
  2 using System.Collections.Generic;
  3 using System.Drawing;
  4 
  5 namespace Pathfinding
  6 {
  7     /// <summary>
  8     /// A*算法
  9     /// </summary>
 10     public class AStarAlgorithm
 11     {
 12         private Vertex m_goal;
 13         private Locator m_locator;
 14         private SortedList<Vertex> m_openSet;
 15         private Orientation m_orientation;
 16         private Vertex m_start;
 17         /// <summary>
 18         /// 查找最优路径
 19         /// </summary>
 20         /// <param name="canvas">画布</param>
 21         /// <param name="obstacles">障碍物</param>
 22         /// <param name="step">步长</param>
 23         /// <param name="voDistance">避障距离</param>
 24         /// <param name="initOrient">第一层查找的方向</param>
 25         /// <param name="start">起始点</param>
 26         /// <param name="goal">目标点</param>
 27         /// <returns></returns>
 28         public Point[] Find(Rectangle canvas, List<Rectangle> obstacles, int step, int voDistance, Orientation initOrient, Point start, Point goal)
 29         {
 30             if (start == goal)
 31                 return null;
 32 
 33             if (start.GetDistanceTo(goal) < step)
 34                 return this.ProcessShortPath(start, goal);
 35 
 36             this.Init(canvas, obstacles, step, voDistance, initOrient, start, goal);
 37             this.AddIntoOpenSet(this.m_start);
 38 
 39             Vertex optimal = null;
 40             while (this.m_openSet.Count > 0)
 41             {
 42                 optimal = this.GetOptimalVertex();
 43 
 44                 if (this.IsGoal(optimal))
 45                 {
 46                     this.WalkTarget();
 47                     var path = Straightener.Straighten(this.m_locator, this.m_goal.Lines);
 48                     this.ProcessEndpoint(start, 0, path);
 49                     this.ProcessEndpoint(goal, path.Length - 1, path);
 50 
 51                     return path;
 52                 }
 53 
 54                 this.Walk(optimal);
 55             }
 56 
 57             return null;
 58         }
 59 
 60         /// <summary>
 61         /// 添加待遍历的点
 62         /// </summary>
 63         /// <param name="vertex"></param>
 64         private void AddIntoOpenSet(Vertex vertex)
 65         {
 66             if (!vertex.IsVisited)
 67                 this.m_openSet.Add(vertex);
 68         }
 69 
 70         /// <summary>
 71         /// 获取最优点
 72         /// </summary>
 73         /// <returns></returns>
 74         private Vertex GetOptimalVertex()
 75         {
 76             var cheapest = this.m_openSet.TakeFirst();
 77             cheapest.IsCurrent = true;
 78 
 79             return cheapest;
 80         }
 81 
 82         /// <summary>
 83         /// 估算到目标点的距离
 84         /// </summary>
 85         /// <param name="vertex"></param>
 86         /// <returns></returns>
 87         private int GuessDistanceToGoal(Vertex vertex)
 88         {
 89             return Math.Abs(vertex.X - this.m_goal.X) + Math.Abs(vertex.Y - this.m_goal.Y);
 90         }
 91 
 92         /// <summary>
 93         /// 初始化数据
 94         /// </summary>
 95         /// <param name="canvas"></param>
 96         /// <param name="obstacles"></param>
 97         /// <param name="step"></param>
 98         /// <param name="voDistance"></param>
 99         /// <param name="initOrient"></param>
100         /// <param name="start"></param>
101         /// <param name="goal"></param>
102         private void Init(Rectangle canvas, List<Rectangle> obstacles, int step, int voDistance, Orientation initOrient, Point start, Point goal)
103         {
104             this.m_locator = new Locator(canvas, obstacles, step, voDistance);
105 
106             this.m_locator.AlignPoint(ref start);
107             this.m_locator.AlignPoint(ref goal);
108             this.m_locator.ExcludeObstacles(start);
109             this.m_locator.ExcludeObstacles(goal);
110 
111             this.m_start = new Vertex()
112             {
113                 Location = start
114             };
115             this.m_goal = new Vertex()
116             {
117                 Location = goal
118             };
119             this.m_openSet = new SortedList<Vertex>();
120             this.m_start.GuessDistance = this.GuessDistanceToGoal(this.m_start);
121             this.m_orientation = initOrient;
122         }
123 
124         /// <summary>
125         /// 是否是目标点
126         /// </summary>
127         /// <param name="vertex"></param>
128         /// <returns></returns>
129         private bool IsGoal(Vertex vertex)
130         {
131             if (vertex.Location == this.m_goal.Location)
132             {
133                 this.m_goal = vertex;
134                 return true;
135             }
136 
137             return false;
138         }
139 
140         /// <summary>
141         /// 处理端点(起始点或目标点)
142         /// </summary>
143         /// <param name="endpoint"></param>
144         /// <param name="idx"></param>
145         /// <param name="path"></param>
146         private void ProcessEndpoint(Point endpoint, int idx, Point[] path)
147         {
148             Point approximatePoint = path[idx];
149             if (0 == idx)
150             {
151                 path[0] = endpoint;
152                 idx += 1;
153             }
154             else
155             {
156                 path[idx] = endpoint;
157                 idx -= 1;
158             }
159 
160             if (approximatePoint.X == path[idx].X)
161                 path[idx].X = endpoint.X;
162             else
163                 path[idx].Y = endpoint.Y;
164         }
165 
166         /// <summary>
167         /// 处理短路径
168         /// </summary>
169         /// <param name="start"></param>
170         /// <param name="goal"></param>
171         /// <returns></returns>
172         private Point[] ProcessShortPath(Point start, Point goal)
173         {
174             var dx = Math.Abs(goal.X - start.X);
175             var dy = Math.Abs(goal.Y - start.Y);
176             if (dx >= dy)
177                 return new Point[] { start, new Point(goal.X, start.Y), goal };
178             else
179                 return new Point[] { start, new Point(start.X, goal.Y), goal };
180         }
181 
182         /// <summary>
183         /// 设置前一个点
184         /// </summary>
185         /// <param name="vertex"></param>
186         private void SetPrev(Vertex vertex)
187         {
188             var neighbors = vertex.Neighbors;
189             neighbors.Sort();
190             vertex.SetPrev(neighbors[0]);
191             vertex.GuessDistance = this.GuessDistanceToGoal(vertex);
192         }
193 
194 
195         #region Traverse Neighbors
196 
197         /// <summary>
198         /// 创建东边的相邻点
199         /// </summary>
200         /// <param name="vertex"></param>
201         private void CreateEastNeighbor(Vertex vertex)
202         {
203             var location = new Point(vertex.X + this.m_locator.Step, vertex.Y);
204             if (this.m_locator.AlignPoint(ref location)
205                 && Orientation.East == vertex.Location.GetOrientation(location))
206             {
207                 var neighbor = new Vertex()
208                 {
209                     Location = location
210                 };
211 
212                 //213                 //     |
214                 // ◐---●---○
215                 //     |
216                 //
217                 vertex.EastNeighbor = neighbor;
218                 //     ◐---◐
219                 //     |   |
220                 // ◐---●---○
221                 //     |
222                 //
223                 neighbor.NorthNeighbor = vertex.NorthNeighbor?.EastNeighbor;
224                 //225                 //     |
226                 // ◐---●---○
227                 //     |   |
228                 //     ◐---◐
229                 neighbor.SouthNeighbor = vertex.SouthNeighbor?.EastNeighbor;
230 
231                 this.SetPrev(neighbor);
232                 this.AddIntoOpenSet(neighbor);
233             }
234             else
235                 vertex.CouldWalkEast = false;
236         }
237 
238         /// <summary>
239         /// 创建北边的相邻点
240         /// </summary>
241         /// <param name="vertex"></param>
242         private void CreateNorthNeighbor(Vertex vertex)
243         {
244             var location = new Point(vertex.X, vertex.Y - this.m_locator.Step);
245             if (this.m_locator.AlignPoint(ref location)
246                 && Orientation.North == vertex.Location.GetOrientation(location))
247             {
248                 var neighbor = new Vertex()
249                 {
250                     Location = location
251                 };
252 
253                 //254                 //     |
255                 // ◐---●---◐
256                 //     |
257                 //
258                 vertex.NorthNeighbor = neighbor;
259                 //     ○---◐
260                 //     |   |
261                 // ◐---●---◐
262                 //     |
263                 //
264                 neighbor.EastNeighbor = vertex.EastNeighbor?.NorthNeighbor;
265                 // ◐---○
266                 // |   |
267                 // ◐---●---◐
268                 //     |
269                 //
270                 neighbor.WestNeighbor = vertex.WestNeighbor?.NorthNeighbor;
271 
272                 this.SetPrev(neighbor);
273                 this.AddIntoOpenSet(neighbor);
274             }
275             else
276                 vertex.CouldWalkNorth = false;
277         }
278 
279         /// <summary>
280         /// 创建南边的相邻点
281         /// </summary>
282         /// <param name="vertex"></param>
283         private void CreateSouthNeighbor(Vertex vertex)
284         {
285             var location = new Point(vertex.X, vertex.Y + this.m_locator.Step);
286             if (this.m_locator.AlignPoint(ref location)
287                 && Orientation.South == vertex.Location.GetOrientation(location))
288             {
289                 var neighbor = new Vertex()
290                 {
291                     Location = location
292                 };
293 
294                 //295                 //     |
296                 // ◐---●---◐
297                 //     |
298                 //
299                 vertex.SouthNeighbor = neighbor;
300                 //301                 //     |
302                 // ◐---●---◐
303                 //     |   |
304                 //     ○---◐
305                 neighbor.EastNeighbor = vertex.EastNeighbor?.SouthNeighbor;
306                 //307                 //     |
308                 // ◐---●---◐
309                 // |   |
310                 // ◐---○
311                 neighbor.WestNeighbor = vertex.WestNeighbor?.SouthNeighbor;
312 
313                 this.SetPrev(neighbor);
314                 this.AddIntoOpenSet(neighbor);
315             }
316             else
317                 vertex.CouldWalkSouth = false;
318         }
319 
320         /// <summary>
321         /// 创建西边的相邻点
322         /// </summary>
323         /// <param name="vertex"></param>
324         private void CreateWestNeighbor(Vertex vertex)
325         {
326             var location = new Point(vertex.X - this.m_locator.Step, vertex.Y);
327             if (this.m_locator.AlignPoint(ref location)
328                 && Orientation.West == vertex.Location.GetOrientation(location))
329             {
330                 var neighbor = new Vertex()
331                 {
332                     Location = location
333                 };
334 
335                 //336                 //     |
337                 // ○---●---◐
338                 //     |
339                 //
340                 vertex.WestNeighbor = neighbor;
341                 //342                 //     |
343                 // ○---●---◐
344                 // |   |
345                 // ◐---◐
346                 neighbor.SouthNeighbor = vertex.SouthNeighbor?.WestNeighbor;
347                 // ◐---◐
348                 // |   |
349                 // ○---●---◐
350                 //     |
351                 //
352                 neighbor.NorthNeighbor = vertex.NorthNeighbor?.WestNeighbor;
353 
354                 this.SetPrev(neighbor);
355                 this.AddIntoOpenSet(neighbor);
356             }
357             else
358                 vertex.CouldWalkWest = false;
359         }
360 
361         /// <summary>
362         /// <para>遍历四个方位的相邻点:东、南、西、北</para>
363         /// <para>●(实心圆)表示访问过的点</para>
364         /// <para>◐(半实心圆)表示可能访问过的点</para>
365         /// <para>○(空心圆)表示未访问过的点</para>
366         /// </summary>
367         /// <param name="vertex"></param>
368         private void Walk(Vertex vertex)
369         {
370             //371             //     |
372             // ◐---●---◐
373             //     |
374             //
375 
376             var count = 0;
377             while (count++ < 4)
378             {
379                 switch (this.m_orientation)
380                 {
381                     case Orientation.East:
382                         this.WalkEast(vertex);
383                         this.m_orientation = Orientation.South;
384                         break;
385                     case Orientation.South:
386                         this.WalkSouth(vertex);
387                         this.m_orientation = Orientation.West;
388                         break;
389                     case Orientation.West:
390                         this.WalkWest(vertex);
391                         this.m_orientation = Orientation.North;
392                         break;
393                     case Orientation.North:
394                         this.WalkNorth(vertex);
395                         this.m_orientation = Orientation.East;
396                         break;
397                     default:
398                         this.m_orientation = Orientation.East;
399                         break;
400                 }
401             }
402 
403             vertex.IsVisited = true;
404             vertex.IsCurrent = false;
405         }
406 
407         /// <summary>
408         /// 遍历东边的相邻点
409         /// </summary>
410         /// <param name="vertex"></param>
411         private void WalkEast(Vertex vertex)
412         {
413             if (vertex.CouldWalkEast && vertex.EastNeighbor is null)
414                 this.CreateEastNeighbor(vertex);
415         }
416 
417         /// <summary>
418         /// 遍历北边的相邻点
419         /// </summary>
420         /// <param name="vertex"></param>
421         private void WalkNorth(Vertex vertex)
422         {
423             if (vertex.CouldWalkNorth && vertex.NorthNeighbor is null)
424                 this.CreateNorthNeighbor(vertex);
425         }
426 
427         /// <summary>
428         /// 遍历南边的相邻点
429         /// </summary>
430         /// <param name="vertex"></param>
431         private void WalkSouth(Vertex vertex)
432         {
433             if (vertex.CouldWalkSouth && vertex.SouthNeighbor is null)
434                 this.CreateSouthNeighbor(vertex);
435         }
436 
437         /// <summary>
438         /// 遍历目标点
439         /// </summary>
440         private void WalkTarget()
441         {
442             // 遍历目标点及其相邻点
443             this.Walk(this.m_goal);
444 
445             if (null != this.m_goal.EastNeighbor)
446                 this.Walk(this.m_goal.EastNeighbor);
447             if (null != this.m_goal.SouthNeighbor)
448                 this.Walk(this.m_goal.SouthNeighbor);
449             if (null != this.m_goal.WestNeighbor)
450                 this.Walk(this.m_goal.WestNeighbor);
451             if (null != this.m_goal.NorthNeighbor)
452                 this.Walk(this.m_goal.NorthNeighbor);
453 
454             this.SetPrev(this.m_goal);
455         }
456 
457         /// <summary>
458         /// 遍历西边的相邻点
459         /// </summary>
460         /// <param name="vertex"></param>
461         private void WalkWest(Vertex vertex)
462         {
463             if (vertex.CouldWalkWest && vertex.WestNeighbor is null)
464                 this.CreateWestNeighbor(vertex);
465         }
466 
467         #endregion Traverse Neighbors
468     }
469 }

AStarAlgorithm


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

namespace Pathfinding
{
    /// <summary>
    /// 定位器(用于避障,查找相邻点或者对齐坐标等)
    /// </summary>
    public class Locator
    {
        /// <summary>
        /// 画布
        /// </summary>
        private readonly Rectangle m_canvas;

        /// <summary>
        /// 障碍物
        /// </summary>
        private readonly List<Rectangle> m_obstacles;

        /// <summary>
        /// 步长
        /// </summary>
        private readonly int m_step;

        /// <summary>
        /// 避障距离
        /// </summary>
        private readonly int m_voDistance;

        public Locator(Rectangle canvas, List<Rectangle> obstacles, int step = 20, int voDistance = 10)
        {
            this.m_canvas = canvas;
            if (step < 20)
                step = 20;
            this.m_step = (step >= 20) ? step : 20;
            this.m_voDistance = (voDistance >= 10) ? voDistance : 10;
            this.m_obstacles = this.BuildObstacles(obstacles);
        }

        /// <summary>
        /// 画板
        /// </summary>
        public Rectangle Canvas => this.m_canvas;

        /// <summary>
        /// 步长
        /// </summary>
        public int Step => this.m_step;

        /// <summary>
        /// 避障距离
        /// </summary>
        public int VODistance => this.m_voDistance;

        /// <summary>
        /// 对齐坐标(把“点”的坐标值对齐到Step的整数倍)
        /// </summary>
        /// <param name="point"></param>
        public Point AlignPoint(Point point)
        {
            return new Point(this.AlignCoord(point.X, 1), this.AlignCoord(point.Y, 1));
        }

        /// <summary>
        /// <para>对齐坐标(把“点”的坐标值对齐到Step的整数倍,同时校验“点”是否在画布内或是否和障碍物冲突)</para>
        /// <para>如果对齐前或对齐后的“点”坐标不在画布内或者和障碍物冲突,则返回false;否则,返回true。</para>
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        public bool AlignPoint(ref Point point)
        {
            if (!this.m_canvas.Contains(point))
                return false;

            point = this.AlignPoint(point);

            if (!this.m_canvas.Contains(point))
                return false;

            return !this.InObstacle(point);
        }

        /// <summary>
        /// 排除包含“点”的障碍物
        /// </summary>
        /// <param name="point"></param>
        public void ExcludeObstacles(Point point)
        {
            this.m_obstacles.RemoveAll(o => o.Contains(point));
        }

        /// <summary>
        /// 判断点是否在障碍物内
        /// </summary>
        /// <param name="point"></param>
        /// <returns></returns>
        public bool InObstacle(Point point)
        {
            return this.m_obstacles.Exists(obst => obst.Contains(point));
        }

        /// <summary>
        /// <para>判断水平线段或垂直线段(ab)是否和障碍物相交</para>
        /// <para>a、b的顺序和结果无关</para>
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public bool IntersectWithObstacle(Point a, Point b)
        {
            if (a.X == b.X)
                return this.IntersectVerticallyWithObstacle(a, b);
            else // a.Y == b.Y
                return this.IntersectHorizontallyWithObstacle(a, b);
        }

        /// <summary>
        /// 判断水平线段(ab,其中a.X ≤ b.X)是否和障碍物相交
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public bool IntersectHorizontallyWithObstacle(Point a, Point b)
        {
            if (a.X > b.X)
            {
                var t = a;
                a = b;
                b = t;
            }

            return this.m_obstacles.Exists(obst =>
                (obst.Top <= a.Y && a.Y <= obst.Bottom && ((a.X <= obst.Left && obst.Left <= b.X) || (a.X <= obst.Right && obst.Right <= b.X)))
                || obst.Contains(a)
                || obst.Contains(b));
        }

        /// <summary>
        /// 判断垂直线段(ab,其中a.Y ≤ b.Y)是否和障碍物相交
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public bool IntersectVerticallyWithObstacle(Point a, Point b)
        {
            if (a.Y > b.Y)
            {
                var t = a;
                a = b;
                b = t;
            }

            return this.m_obstacles.Exists(obst =>
                (obst.Left <= a.X && a.X <= obst.Right && ((a.Y <= obst.Top && obst.Top <= b.Y) || (a.Y <= obst.Bottom && obst.Bottom <= b.Y)))
                || obst.Contains(a)
                || obst.Contains(b));
        }

        /// <summary>
        /// 对齐坐标的值
        /// </summary>
        /// <param name="val"></param>
        /// <param name="direction"></param>
        /// <returns></returns>
        private int AlignCoord(int val, int direction)
        {
            int md = val % this.m_step;
            if (0 == md)
                return val;
            else if (md <= this.m_step / 2)
                return val - md;
            else
                return val - md + (direction * this.m_step);
        }

        /// <summary>
        /// 构造障碍物(用于调试)
        /// </summary>
        /// <param name="obstacles"></param>
        /// <returns></returns>
        private List<Rectangle> BuildDebugObstacles(List<Rectangle> obstacles)
        {
            if (obstacles is null || obstacles.Count <= 0)
                return new List<Rectangle>();
            else
                return obstacles.Select(o => new Rectangle(o.X - this.m_voDistance,
                                                           o.Y - this.m_voDistance,
                                                           o.Width + this.m_voDistance * 2,
                                                           o.Height + m_voDistance * 2)).ToList();
        }

        /// <summary>
        /// 构造障碍物
        /// </summary>
        /// <param name="obstacles"></param>
        /// <returns></returns>
        private List<Rectangle> BuildObstacles(List<Rectangle> obstacles)
        {
            if (obstacles is null || obstacles.Count <= 0)
                return new List<Rectangle>();
            else
                return obstacles.Select(o => new Rectangle(o.X - this.m_voDistance + 1,
                                                           o.Y - this.m_voDistance + 1,
                                                           o.Width + this.m_voDistance * 2 - 2,
                                                           o.Height + m_voDistance * 2 - 2)).ToList();
        }
    }
}

Locator


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

namespace Pathfinding
{
    /// <summary>
    /// 路径调制器
    /// </summary>
    public class Straightener
    {
        /// <summary>
        /// 满足调直的前提条件(路径最少包含4个点)
        /// </summary>
        private const int MIN_COUNT_POINTS = 4;
        /// <summary>
        /// 定位器
        /// </summary>
        private Locator m_locator;
        /// <summary>
        /// 原始路径
        /// </summary>
        private LinkedList<Point> m_originalPath;
        /// <summary>
        /// 调直后的路径
        /// </summary>
        private LinkedList<Point> m_straightenedPath;

        private Straightener()
        {
            this.Reset();
        }

        /// <summary>
        /// 调直路径,减少拐点(参数为空或小于4个点不做任何处理)
        /// </summary>
        /// <param name="path"></param>
        /// <returns></returns>
        public static Point[] Straighten(Locator locator, Point[] path)
        {
            if (locator is null || path is null || path.Length < MIN_COUNT_POINTS)
                return path;

            var worker = new Straightener();
            worker.m_locator = locator;
            worker.m_originalPath = new LinkedList<Point>(path);
            worker.Straighten();

            return worker.m_straightenedPath.ToArray();
        }

        /// <summary>
        /// 创建假设的拐点
        /// </summary>
        /// <param name="node"></param>
        /// <returns></returns>
        private static Point CreateHypotheticalInflection(LinkedListNode<Point> node)
        {
            if (node.Value.X == node.Next.Value.X)
                return new Point(node.Next.Next.Value.X, node.Value.Y);
            else
                return new Point(node.Value.X, node.Next.Next.Value.Y);
        }

        /// <summary>
        /// 计算线段(abcd)上拐点的个数
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="c"></param>
        /// <param name="d"></param>
        /// <returns></returns>
        private static int GetCountInflections(Point a, Point b, Point c, Point d)
        {
            var inflections = 0;
            if (c.X != a.X && c.Y != a.Y)
                inflections++;
            if (d.X != b.X && d.Y != b.Y)
                inflections++;

            return inflections;
        }

        private static int GetDistance(Point a, Point b)
        {
            if (a.X == b.X)
                return Math.Abs(a.Y - b.Y);
            else
                return Math.Abs(a.X - b.X);
        }
        /// <summary>
        /// 添加拐点
        /// </summary>
        /// <param name="inflection"></param>
        private void AddInflection(Point inflection)
        {
            if (null != this.m_straightenedPath.Last
                && this.m_straightenedPath.Last.Value == inflection)
                return;

            var last = this.m_straightenedPath.AddLast(inflection);
            if (null != last.Previous?.Previous
                && (last.Value.X == last.Previous.Previous.Value.X
                    || last.Value.Y == last.Previous.Previous.Value.Y))
                this.m_straightenedPath.Remove(last.Previous);
        }

        private void DoStraighten()
        {
            this.Reset();

            var current = this.m_originalPath.First;
            while (null != current.Next?.Next)
            {
                this.AddInflection(current.Value);

                this.RemoveRedundantInflections(current);
                if (current.Next?.Next is null)
                    break;

                var inflection = CreateHypotheticalInflection(current);
                if (!this.IntersectWithObstacle(current.Value, inflection, current.Next.Next.Value))
                {
                    var success = false;

                    if (null != current.Previous)
                    {
                        var i1 = GetCountInflections(current.Previous.Value, current.Value, current.Next.Value, current.Next.Next.Value);
                        var i2 = GetCountInflections(current.Previous.Value, current.Value, inflection, current.Next.Next.Value);
                        if (i2 < i1)
                        {
                            current.Next.Value = inflection;
                            success = true;
                        }
                    }
                    else if (null != current.Next?.Next?.Next)
                    {
                        var i3 = GetCountInflections(current.Value, current.Next.Value, current.Next.Next.Value, current.Next.Next.Next.Value);
                        var i4 = GetCountInflections(current.Value, inflection, current.Next.Next.Value, current.Next.Next.Next.Value);
                        if (i4 < i3)
                        {
                            current.Next.Value = inflection;
                            success = true;
                        }
                    }

                    if (success)
                    {
                        this.RemoveRedundantInflections(current.Next);
                        if (current.Next?.Next is null)
                            break;
                    }
                }

                this.RemoveTurnBackInflections(current);

                current = current.Next;
            }

            this.AddInflection(this.m_originalPath.Last.Previous.Value);
            this.AddInflection(this.m_originalPath.Last.Value);
        }

        private int GetDistance()
        {
            var dist = 0;
            var current = this.m_straightenedPath.First;
            do
            {
                if (null != current.Next)
                {
                    dist += GetDistance(current.Value, current.Next.Value);
                    current = current.Next;
                }
                else
                    break;

            } while (true);

            return dist;
        }

        /// <summary>
        /// 判断线段(abc)是否和障碍物相交
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="c"></param>
        /// <returns></returns>
        private bool IntersectWithObstacle(Point a, Point b, Point c)
        {
            return this.m_locator.IntersectWithObstacle(a, b)
                || this.m_locator.IntersectWithObstacle(b, c);
        }

        /// <summary>
        /// 删除冗余拐点
        /// </summary>
        /// <param name="node"></param>
        private void RemoveRedundantInflections(LinkedListNode<Point> node)
        {
            while (true)
            {
                if (node.Next?.Next is null)
                    break;

                if (node.Value.X == node.Next.Next.Value.X
                    || node.Value.Y == node.Next.Next.Value.Y)
                    this.m_originalPath.Remove(node.Next);
                else
                    break;
            }
        }

        /// <summary>
        /// 删除回转拐点
        /// </summary>
        /// <param name="node"></param>
        private void RemoveTurnBackInflections(LinkedListNode<Point> node)
        {
            if (node.Next?.Next?.Next is null)
                return;

            var point = node.Value;
            var nPoint = node.Next.Value;
            var nnPoint = node.Next.Next.Value;
            var nnnPoint = node.Next.Next.Next.Value;

            // ●为已知拐点;○为假设拐点
            // 消除如下形式的拐点
            //// |
            // ○---●
            // |   |
            // ●---●
            if (point.X == nPoint.X
                && nPoint.Y == nnPoint.Y
                && nnPoint.X == nnnPoint.X)
            {
                var dy1 = point.Y - nPoint.Y;
                var dy2 = nnnPoint.Y - nnPoint.Y;
                var p1 = new Point(nnnPoint.X, point.Y);
                if (Math.Abs(dy2) >= Math.Abs(dy1)
                    && Math.Abs(dy1) / dy1 == Math.Abs(dy2) / dy2
                    && !this.m_locator.IntersectHorizontallyWithObstacle(node.Value, p1))
                {
                    this.m_originalPath.Remove(node.Next);
                    this.m_originalPath.Remove(node.Next);
                    this.m_originalPath.AddAfter(node, p1);
                }
            }
            // ●为已知拐点;○为假设拐点
            // 消除如下形式的拐点
            // ●---○---●
            // |   |
            // ●---●
            else if (point.Y == nPoint.Y
                && nPoint.X == nnPoint.X
                && nnPoint.Y == nnnPoint.Y)
            {
                var dx1 = point.X - nPoint.X;
                var dx2 = nnnPoint.X - nnPoint.X;
                var p2 = new Point(point.X, nnnPoint.Y);
                if (Math.Abs(dx2) >= Math.Abs(dx1)
                    && Math.Abs(dx1) / dx1 == Math.Abs(dx2) / dx2
                    && !this.m_locator.IntersectVerticallyWithObstacle(node.Value, p2))
                {
                    this.m_originalPath.Remove(node.Next);
                    this.m_originalPath.Remove(node.Next);
                    this.m_originalPath.AddAfter(node, p2);
                }
            }
        }

        private void Reset()
        {
            this.m_straightenedPath = new LinkedList<Point>();
        }

        private void Straighten()
        {
            int prevDistance = 0;
            int prevInflections = 0;
            int distance = 0;
            int inflections = 0;

            while (true)
            {
                this.DoStraighten();
                this.m_originalPath = this.m_straightenedPath;

                distance = this.GetDistance();
                inflections = this.m_originalPath.Count;

                if (distance == prevDistance
                    && inflections == prevInflections)
                    break;

                prevDistance = distance;
                prevInflections = inflections;
            }
        }
    }
}

Straightener


using System;
using System.Collections;
using System.Collections.Generic;

namespace Pathfinding
{
    /// <summary>
    /// <para>有序链表</para>
    /// <para>此类不是线程安全的</para>
    /// </summary>
    /// <typeparam name="T"></typeparam>
    public class SortedList<T> : IEnumerable<T> where T : IComparable<T>
    {
        private int m_count = 0;
        private Node m_first;
        private Node m_last;

        public SortedList()
        {
            // do nothing
        }

        public SortedList(IEnumerable<T> collection)
        {
            this.AddRange(collection);
        }

        /// <summary>
        /// 链表中的元素个数
        /// </summary>
        public int Count => this.m_count;

        /// <summary>
        /// 第一个元素
        /// </summary>
        public T First
        {
            get
            {
                if (null != this.m_first)
                    return this.m_first.Value;
                else
                    return default(T);
            }
        }

        public bool IsEmpty => this.m_count <= 0;

        /// <summary>
        /// 最后一个元素
        /// </summary>
        public T Last
        {
            get
            {
                if (null != this.m_last)
                    return this.m_last.Value;
                else
                    return default(T);
            }
        }

        public void Add(T value)
        {
            var node = new Node(value);
            if (this.IsEmpty)
            {
                this.m_first = node;
                this.m_last = node;
            }
            else if (value.CompareTo(this.m_first.Value) < 0)
            {
                node.Next = this.m_first;
                this.m_first.Prev = node;
                this.m_first = node;
            }
            else if (this.m_last.Value.CompareTo(value) <= 0)
            {
                node.Prev = this.m_last;
                this.m_last.Next = node;
                this.m_last = node;
            }
            else
            {
                Node current = this.m_first;
                do
                {
                    if (value.CompareTo(current.Value) < 0)
                        break;

                    current = current.Next;

                } while (current != null);


                var prev = current.Prev;
                prev.Next = node;
                node.Prev = prev;

                node.Next = current;
                current.Prev = node;
            }

            this.m_count++;
        }

        public void AddRange(IEnumerable<T> collection)
        {
            if (collection is null)
                return;

            foreach (var item in collection)
                this.Add(item);
        }

        /// <summary>
        /// 清除所有元素
        /// </summary>
        public void Clear()
        {
            this.m_first = null;
            this.m_last = null;
            this.m_count = 0;
        }

        /// <summary>
        /// 判断链表是否包含指定的元素
        /// </summary>
        /// <param name="value"></param>
        /// <returns></returns>
        public bool Contains(T value)
        {
            if (this.IsEmpty)
                return false;

            var current = this.m_first;
            while (null != current)
            {
                if (value.CompareTo(current.Value) == 0)
                    return true;

                current = current.Next;
            }

            return false;
        }

        public int IndexOf(T value)
        {
            if (this.IsEmpty)
                return -1;

            var current = this.m_first;
            var idx = 0;
            while (null != current)
            {
                if (value.CompareTo(current.Value) == 0)
                    return idx;

                idx++;
                current = current.Next;
            }

            return -1;
        }

        /// <summary>
        /// 获取IEnumerator<T>
        /// </summary>
        /// <returns></returns>
        public IEnumerator<T> GetEnumerator()
        {
            return new Enumerator(this);
        }

        IEnumerator IEnumerable.GetEnumerator()
        {
            return this.GetEnumerator();
        }

        /// <summary>
        /// 删除指定的元素
        /// </summary>
        /// <param name="value"></param>
        public void Remove(T value)
        {
            if (this.IsEmpty)
                return;

            var current = this.m_first;
            while (null != current)
            {
                if (value.CompareTo(current.Value) == 0)
                    break;

                current = current.Next;
            }

            if (null != current)
            {
                var prev = current.Prev;
                var next = current.Next;

                if (null != prev && null != next)
                {
                    prev.Next = next;
                    next.Prev = prev;
                }
                else if (null != prev)
                    this.SetLast(prev);
                else
                    this.SetFirst(next);

                this.m_count--;
            }
        }

        /// <summary>
        /// 返回并删除第一个元素
        /// </summary>
        /// <returns></returns>
        public T TakeFirst()
        {
            if (this.IsEmpty)
                return default(T);

            var value = this.m_first.Value;

            this.SetFirst(this.m_first.Next);
            this.m_count--;

            return value;
        }

        /// <summary>
        /// 返回并删除最后一个元素
        /// </summary>
        /// <returns></returns>
        public T TakeLast()
        {
            if (this.IsEmpty)
                return default(T);

            var value = this.m_last.Value;

            this.SetLast(this.m_last.Prev);
            this.m_count--;

            return value;
        }

        public T[] ToArray()
        {
            if (this.IsEmpty)
                return null;

            var a = new T[this.m_count];
            var current = this.m_first;
            var idx = 0;
            while (null != current)
            {
                a[idx++] = current.Value;
                current = current.Next;
            }

            return a;
        }

        public List<T> ToList()
        {
            if (this.IsEmpty)
                return null;

            var l = new List<T>(this.m_count);
            var current = this.m_first;
            while (null != current)
            {
                l.Add(current.Value);
            }

            return l;
        }

        private void SetFirst(Node first)
        {
            this.m_first = first;
            if (this.m_first is null)
                this.m_last = null;
            else
                this.m_first.Prev = null;
        }

        private void SetLast(Node last)
        {
            this.m_last = last;
            if (this.m_last is null)
                this.m_first = null;
            else
                this.m_last.Next = null;
        }

        /// <summary>
        /// 枚举器
        /// </summary>
        private class Enumerator : IEnumerator<T>
        {
            private readonly SortedList<T> m_list;
            private readonly Node m_prevFirst = new Node(default(T));
            private Node m_current;

            public Enumerator(SortedList<T> list)
            {
                this.m_list = list;
                this.Reset();
            }

            public T Current
            {
                get
                {
                    if (null != this.m_current)
                        return this.m_current.Value;
                    else
                        return default(T);
                }
            }

            object IEnumerator.Current
            {
                get
                {
                    if (null != this.m_current)
                        return this.m_current.Value;
                    else
                        return default(T);
                }
            }

            public void Dispose()
            {
                // do nothing
            }

            public bool MoveNext()
            {
                if (object.ReferenceEquals(this.m_current, this.m_prevFirst))
                    this.m_current = this.m_list.m_first;
                else
                    this.m_current = this.m_current?.Next;

                return null != this.m_current;
            }

            public void Reset()
            {
                this.m_current = this.m_prevFirst;
            }
        }

        /// <summary>
        /// 链表节点
        /// </summary>
        private class Node
        {
            public Node(T data)
            {
                this.Value = data;
            }

            public Node Next { get; set; }

            public Node Prev { get; set; }

            public T Value { get; }

            public override string ToString()
            {
                if (null != Value)
                    return Value.ToString();
                return null;
            }
        }
    }
}

SortedList


namespace Pathfinding
{
    /// <summary>
    /// 方向
    /// </summary>
    public enum Orientation
    {
        /// <summary>
        /// 无方向
        /// </summary>
        None = 0,
        /// <summary>
        ////// </summary>
        East = 0x1,
        /// <summary>
        ////// </summary>
        South = 0x10,
        /// <summary>
        /// 西
        /// </summary>
        West = 0x100,
        /// <summary>
        ////// </summary>
        North = 0x1000,
        /// <summary>
        /// 东西
        /// </summary>
        EastWest = East | West,
        /// <summary>
        /// 南北
        /// </summary>
        NorthSouth = South | North,
        /// <summary>
        /// 东南
        /// </summary>
        SouthEast = East | South,
        /// <summary>
        /// 西南
        /// </summary>
        SouthWest = South | West,
        /// <summary>
        /// 西北
        /// </summary>
        NorthWest = West | North,
        /// <summary>
        /// 东北
        /// </summary>
        NorthEast = North | East,
    }
}

Orientation


namespace Pathfinding
{
    public static class OrientationExtension
    {
        /// <summary>
        /// 是否为东西方向
        /// </summary>
        /// <param name="orient"></param>
        /// <returns></returns>
        public static bool IsEastWest(this Orientation orient)
        {
            return orient == Orientation.East
                || orient == Orientation.West
                || orient == Orientation.EastWest;
        }

        /// <summary>
        /// 是否为南北方向
        /// </summary>
        /// <param name="orient"></param>
        /// <returns></returns>
        public static bool IsNorthSouth(this Orientation orient)
        {
            return orient == Orientation.South
                || orient == Orientation.North
                || orient == Orientation.NorthSouth;
        }

        /// <summary>
        /// <para>把方向转换为EastWest或NorthSouth</para>
        /// <para>如果方向不是东西方向或南北方向,则返回None</para>
        /// </summary>
        /// <param name="orient"></param>
        /// <returns></returns>
        public static Orientation ConvertToEWOrNS(this Orientation orient)
        {
            if (orient.IsEastWest())
                return Orientation.EastWest;
            else if (orient.IsNorthSouth())
                return Orientation.NorthSouth;
            else
                return Orientation.None;
        }
    }
}

OrientationExtension


using System;
using System.Drawing;

namespace Pathfinding
{
    public static class PointExtension
    {
        /// <summary>
        /// <para>获取第二个点相对于第一个点的方位</para>
        /// <para>此方法只判断是否为正南,正北,正东或正西四个方向。</para>
        /// <para>如果两个点坐标一样,则返回Orientation.None。</para>
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <returns>East、South、West、North</returns>
        public static Orientation GetOrientation(this Point from, Point to)
        {
            if (from.X == to.X)
            {
                if (to.Y > from.Y)
                    return Orientation.South;
                else if (to.Y < from.Y)
                    return Orientation.North;
            }
            else if (from.Y == to.Y)
            {
                if (to.X > from.X)
                    return Orientation.East;
                else if (to.X < from.X)
                    return Orientation.West;
            }

            return Orientation.None;
        }

        /// <summary>
        /// <para>判断两点之间的相对位置:东西方向或南北方向</para>
        /// <para>如果两个点坐标一样或不是东西或南北方向,则返回Orientation.None。</para>
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <returns>EastWest或NorthSouth</returns>
        public static Orientation GetOrientationEWOrNS(this Point from, Point to)
        {
            if (from.X == to.X && to.Y != from.Y)
            {
                return Orientation.NorthSouth;
            }
            else if (from.Y == to.Y && to.X != from.X)
            {
                return Orientation.EastWest;
            }

            return Orientation.None;
        }

        /// <summary>
        /// 两点的位置是否为东西方向:Y坐标相等,且X坐标不相等
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <returns></returns>
        public static bool IsEastWest(this Point from, Point to)
        {
            return from.Y == to.Y && to.X != from.X;
        }

        /// <summary>
        /// 两点的位置是否为南北方向:X坐标相等,且Y坐标不相等
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <returns></returns>
        public static bool IsNorthSouth(this Point from, Point to)
        {
            return from.X == to.X && to.Y != from.Y;
        }

        /// <summary>
        /// 计算两点之间的距离(仅计算东西和南北方向的距离)
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static int GetAlignedDistanceTo(this Point a, Point b)
        {
            if (a.IsEastWest(b))
                return Math.Abs(a.X - b.X);
            else
                return Math.Abs(a.Y - b.Y);
        }

        /// <summary>
        /// 计算两点之间的距离
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <returns></returns>
        public static double GetDistanceTo(this Point from, Point to)
        {
            return Math.Sqrt(Math.Pow(from.X - to.X, 2.0d) + Math.Pow(from.Y - to.Y, 2.0d));
        }

        /// <summary>
        /// 判断两个点是否在东西方向或南北方向的同一条直线上
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static bool InStraightLine(this Point a, Point b)
        {
            return a.X == b.X || a.Y == b.Y;
        }
    }
}

PointExtension

 

页面下部广告