路徑查詢演算法應用之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;
        }
    }
}