路径查找算法应用之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*算法查找出来的最优路径（不一定是最短路径，依赖于算法的实现）

第二张图为调直后的最直路径（拐点最少）

 

 

代码

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

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

namespace Pathfinding
{
    /// <summary>
    /// A*算法
    /// </summary>
    public class AStarAlgorithm
    {
        private Vertex m_goal;
        private Locator m_locator;
        private SortedList<Vertex> m_openSet;
        private Orientation m_orientation;
        private Vertex m_start;
        /// <summary>
        /// 查找最优路径
        /// </summary>
        /// <param name="canvas">画布</param>
        /// <param name="obstacles">障碍物</param>
        /// <param name="step">步长</param>
        /// <param name="voDistance">避障距离</param>
        /// <param name="initOrient">第一层查找的方向</param>
        /// <param name="start">起始点</param>
        /// <param name="goal">目标点</param>
        /// <returns></returns>
        public Point[] Find(Rectangle canvas, List<Rectangle> obstacles, int step, int voDistance, Orientation initOrient, Point start, Point goal)
        {
            if (start == goal)
                return null;

            if (start.GetDistanceTo(goal) < step)
                return this.ProcessShortPath(start, goal);

            this.Init(canvas, obstacles, step, voDistance, initOrient, start, goal);
            this.AddIntoOpenSet(this.m_start);

            Vertex optimal = null;
            while (this.m_openSet.Count > 0)
            {
                optimal = this.GetOptimalVertex();

                if (this.IsGoal(optimal))
                {
                    this.WalkTarget();
                    var path = Straightener.Straighten(this.m_locator, this.m_goal.Lines);
                    this.ProcessEndpoint(start, 0, path);
                    this.ProcessEndpoint(goal, path.Length - 1, path);

                    return path;
                }

                this.Walk(optimal);
            }

            return null;
        }

        /// <summary>
        /// 添加待遍历的点
        /// </summary>
        /// <param name="vertex"></param>
        private void AddIntoOpenSet(Vertex vertex)
        {
            if (!vertex.IsVisited)
                this.m_openSet.Add(vertex);
        }

        /// <summary>
        /// 获取最优点
        /// </summary>
        /// <returns></returns>
        private Vertex GetOptimalVertex()
        {
            var cheapest = this.m_openSet.TakeFirst();
            cheapest.IsCurrent = true;

            return cheapest;
        }

        /// <summary>
        /// 估算到目标点的距离
        /// </summary>
        /// <param name="vertex"></param>
        /// <returns></returns>
        private int GuessDistanceToGoal(Vertex vertex)
        {
            return Math.Abs(vertex.X - this.m_goal.X) + Math.Abs(vertex.Y - this.m_goal.Y);
        }

        /// <summary>
        /// 初始化数据
        /// </summary>
        /// <param name="canvas"></param>
        /// <param name="obstacles"></param>
        /// <param name="step"></param>
        /// <param name="voDistance"></param>
        /// <param name="initOrient"></param>
        /// <param name="start"></param>
        /// <param name="goal"></param>
        private void Init(Rectangle canvas, List<Rectangle> obstacles, int step, int voDistance, Orientation initOrient, Point start, Point goal)
        {
            this.m_locator = new Locator(canvas, obstacles, step, voDistance);

            this.m_locator.AlignPoint(ref start);
            this.m_locator.AlignPoint(ref goal);
            this.m_locator.ExcludeObstacles(start);
            this.m_locator.ExcludeObstacles(goal);

            this.m_start = new Vertex()
            {
                Location = start
            };
            this.m_goal = new Vertex()
            {
                Location = goal
            };
            this.m_openSet = new SortedList<Vertex>();
            this.m_start.GuessDistance = this.GuessDistanceToGoal(this.m_start);
            this.m_orientation = initOrient;
        }

        /// <summary>
        /// 是否是目标点
        /// </summary>
        /// <param name="vertex"></param>
        /// <returns></returns>
        private bool IsGoal(Vertex vertex)
        {
            if (vertex.Location == this.m_goal.Location)
            {
                this.m_goal = vertex;
                return true;
            }

            return false;
        }

        /// <summary>
        /// 处理端点（起始点或目标点）
        /// </summary>
        /// <param name="endpoint"></param>
        /// <param name="idx"></param>
        /// <param name="path"></param>
        private void ProcessEndpoint(Point endpoint, int idx, Point[] path)
        {
            Point approximatePoint = path[idx];
            if (0 == idx)
            {
                path[0] = endpoint;
                idx += 1;
            }
            else
            {
                path[idx] = endpoint;
                idx -= 1;
            }

            if (approximatePoint.X == path[idx].X)
                path[idx].X = endpoint.X;
            else
                path[idx].Y = endpoint.Y;
        }

        /// <summary>
        /// 处理短路径
        /// </summary>
        /// <param name="start"></param>
        /// <param name="goal"></param>
        /// <returns></returns>
        private Point[] ProcessShortPath(Point start, Point goal)
        {
            var dx = Math.Abs(goal.X - start.X);
            var dy = Math.Abs(goal.Y - start.Y);
            if (dx >= dy)
                return new Point[] { start, new Point(goal.X, start.Y), goal };
            else
                return new Point[] { start, new Point(start.X, goal.Y), goal };
        }

        /// <summary>
        /// 设置前一个点
        /// </summary>
        /// <param name="vertex"></param>
        private void SetPrev(Vertex vertex)
        {
            var neighbors = vertex.Neighbors;
            neighbors.Sort();
            vertex.SetPrev(neighbors[0]);
            vertex.GuessDistance = this.GuessDistanceToGoal(vertex);
        }


        #region Traverse Neighbors

        /// <summary>
        /// 创建东边的相邻点
        /// </summary>
        /// <param name="vertex"></param>
        private void CreateEastNeighbor(Vertex vertex)
        {
            var location = new Point(vertex.X + this.m_locator.Step, vertex.Y);
            if (this.m_locator.AlignPoint(ref location)
                && Orientation.East == vertex.Location.GetOrientation(location))
            {
                var neighbor = new Vertex()
                {
                    Location = location
                };

                //     ◐
                //     |
                // ◐---●---○
                //     |
                //     ◐
                vertex.EastNeighbor = neighbor;
                //     ◐---◐
                //     |   |
                // ◐---●---○
                //     |
                //     ◐
                neighbor.NorthNeighbor = vertex.NorthNeighbor?.EastNeighbor;
                //     ◐
                //     |
                // ◐---●---○
                //     |   |
                //     ◐---◐
                neighbor.SouthNeighbor = vertex.SouthNeighbor?.EastNeighbor;

                this.SetPrev(neighbor);
                this.AddIntoOpenSet(neighbor);
            }
            else
                vertex.CouldWalkEast = false;
        }

        /// <summary>
        /// 创建北边的相邻点
        /// </summary>
        /// <param name="vertex"></param>
        private void CreateNorthNeighbor(Vertex vertex)
        {
            var location = new Point(vertex.X, vertex.Y - this.m_locator.Step);
            if (this.m_locator.AlignPoint(ref location)
                && Orientation.North == vertex.Location.GetOrientation(location))
            {
                var neighbor = new Vertex()
                {
                    Location = location
                };

                //     ○
                //     |
                // ◐---●---◐
                //     |
                //     ◐
                vertex.NorthNeighbor = neighbor;
                //     ○---◐
                //     |   |
                // ◐---●---◐
                //     |
                //     ◐
                neighbor.EastNeighbor = vertex.EastNeighbor?.NorthNeighbor;
                // ◐---○
                // |   |
                // ◐---●---◐
                //     |
                //     ◐
                neighbor.WestNeighbor = vertex.WestNeighbor?.NorthNeighbor;

                this.SetPrev(neighbor);
                this.AddIntoOpenSet(neighbor);
            }
            else
                vertex.CouldWalkNorth = false;
        }

        /// <summary>
        /// 创建南边的相邻点
        /// </summary>
        /// <param name="vertex"></param>
        private void CreateSouthNeighbor(Vertex vertex)
        {
            var location = new Point(vertex.X, vertex.Y + this.m_locator.Step);
            if (this.m_locator.AlignPoint(ref location)
                && Orientation.South == vertex.Location.GetOrientation(location))
            {
                var neighbor = new Vertex()
                {
                    Location = location
                };

                //     ◐
                //     |
                // ◐---●---◐
                //     |
                //     ○
                vertex.SouthNeighbor = neighbor;
                //     ◐
                //     |
                // ◐---●---◐
                //     |   |
                //     ○---◐
                neighbor.EastNeighbor = vertex.EastNeighbor?.SouthNeighbor;
                //     ◐
                //     |
                // ◐---●---◐
                // |   |
                // ◐---○
                neighbor.WestNeighbor = vertex.WestNeighbor?.SouthNeighbor;

                this.SetPrev(neighbor);
                this.AddIntoOpenSet(neighbor);
            }
            else
                vertex.CouldWalkSouth = false;
        }

        /// <summary>
        /// 创建西边的相邻点
        /// </summary>
        /// <param name="vertex"></param>
        private void CreateWestNeighbor(Vertex vertex)
        {
            var location = new Point(vertex.X - this.m_locator.Step, vertex.Y);
            if (this.m_locator.AlignPoint(ref location)
                && Orientation.West == vertex.Location.GetOrientation(location))
            {
                var neighbor = new Vertex()
                {
                    Location = location
                };

                //     ◐
                //     |
                // ○---●---◐
                //     |
                //     ◐
                vertex.WestNeighbor = neighbor;
                //     ◐
                //     |
                // ○---●---◐
                // |   |
                // ◐---◐
                neighbor.SouthNeighbor = vertex.SouthNeighbor?.WestNeighbor;
                // ◐---◐
                // |   |
                // ○---●---◐
                //     |
                //     ◐
                neighbor.NorthNeighbor = vertex.NorthNeighbor?.WestNeighbor;

                this.SetPrev(neighbor);
                this.AddIntoOpenSet(neighbor);
            }
            else
                vertex.CouldWalkWest = false;
        }

        /// <summary>
        /// <para>遍历四个方位的相邻点：东、南、西、北</para>
        /// <para>●(实心圆)表示访问过的点</para>
        /// <para>◐(半实心圆)表示可能访问过的点</para>
        /// <para>○(空心圆)表示未访问过的点</para>
        /// </summary>
        /// <param name="vertex"></param>
        private void Walk(Vertex vertex)
        {
            //     ◐
            //     |
            // ◐---●---◐
            //     |
            //     ◐

            var count = 0;
            while (count++ < 4)
            {
                switch (this.m_orientation)
                {
                    case Orientation.East:
                        this.WalkEast(vertex);
                        this.m_orientation = Orientation.South;
                        break;
                    case Orientation.South:
                        this.WalkSouth(vertex);
                        this.m_orientation = Orientation.West;
                        break;
                    case Orientation.West:
                        this.WalkWest(vertex);
                        this.m_orientation = Orientation.North;
                        break;
                    case Orientation.North:
                        this.WalkNorth(vertex);
                        this.m_orientation = Orientation.East;
                        break;
                    default:
                        this.m_orientation = Orientation.East;
                        break;
                }
            }

            vertex.IsVisited = true;
            vertex.IsCurrent = false;
        }

        /// <summary>
        /// 遍历东边的相邻点
        /// </summary>
        /// <param name="vertex"></param>
        private void WalkEast(Vertex vertex)
        {
            if (vertex.CouldWalkEast && vertex.EastNeighbor is null)
                this.CreateEastNeighbor(vertex);
        }

        /// <summary>
        /// 遍历北边的相邻点
        /// </summary>
        /// <param name="vertex"></param>
        private void WalkNorth(Vertex vertex)
        {
            if (vertex.CouldWalkNorth && vertex.NorthNeighbor is null)
                this.CreateNorthNeighbor(vertex);
        }

        /// <summary>
        /// 遍历南边的相邻点
        /// </summary>
        /// <param name="vertex"></param>
        private void WalkSouth(Vertex vertex)
        {
            if (vertex.CouldWalkSouth && vertex.SouthNeighbor is null)
                this.CreateSouthNeighbor(vertex);
        }

        /// <summary>
        /// 遍历目标点
        /// </summary>
        private void WalkTarget()
        {
            // 遍历目标点及其相邻点
            this.Walk(this.m_goal);

            if (null != this.m_goal.EastNeighbor)
                this.Walk(this.m_goal.EastNeighbor);
            if (null != this.m_goal.SouthNeighbor)
                this.Walk(this.m_goal.SouthNeighbor);
            if (null != this.m_goal.WestNeighbor)
                this.Walk(this.m_goal.WestNeighbor);
            if (null != this.m_goal.NorthNeighbor)
                this.Walk(this.m_goal.NorthNeighbor);

            this.SetPrev(this.m_goal);
        }

        /// <summary>
        /// 遍历西边的相邻点
        /// </summary>
        /// <param name="vertex"></param>
        private void WalkWest(Vertex vertex)
        {
            if (vertex.CouldWalkWest && vertex.WestNeighbor is null)
                this.CreateWestNeighbor(vertex);
        }

        #endregion Traverse Neighbors
    }
}

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 &#8804; 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 &#8804; 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;
        }
    }
}