应用A*路径搜索算法在画板上查找连接起始节点和终止节点之间的最短最直的折线。
环境：Visual Studio 2017 + .Net Framework 4.5
应用场景：在画板上查找起始点和目标点之间的最短最直路径，最后画出连接两个点之间的折线。
算法简介：A*算法是一种性能较高的最优路径搜索算法，由Stanford Research Institute(now SRI International)的Peter Hart，Nils Nilsson和Bertram Raphael于1968年发表。A*算法可看做是对Dijkstra算法的扩展和优化，其性能一般情况下比Dijkstra算法快得多。在本文的应用场景中，（根据测试）通常比Dijkstra算法快三倍以上，甚至可能比Dijkstra算法快十几倍甚至几十倍。
A*算法的应用范围也比较广泛，如机器人行走路径规划，游戏中的NPC移动计算等。
更详细的算法说明请参考维基百科A* search algorithm
实现思想：
1，通过Locator把起始点坐标和目标点坐标对齐到步长（step，默认为20,）的整数倍。这样，起始点和目标点就成了原来的起始点目标点的近似点。
2，把包含起始点和目标点的障碍物（如图中所示，为矩形框）排除掉，不然折线遇到障碍物无法通过。
下图中的矩形框的虚边为避障区域，为了防止折线和障碍物碰撞。
3，把起始点添加到待遍历点的集合中（本文中为SortedList<Vertex>）。
4，从待遍历点的集合中取出第一个点（当前的最优点），遍历其东、南、西、北四个方向的相邻节点。东西两个方向和当前点的Y坐标相同，南北两个方向和当前点的X坐标相同。
相邻点距当前点的距离为step参数设定的步长。step越大，搜索速度越快，然而，可能导致折线无法通过间距较小的障碍物。
如果某个方向的相邻点不存在，则创建新的相邻点（如果相邻点不在障碍物内部的话）；同时，设置新创建点的四个相邻点（也许新创建点的相邻点已经被创建了）。
把新创建的相邻点的Visited属性设置为false（当前实现中，Visited属性默认为false），然后对新创建点的所有相邻点排序，取最优点，设置为新创建点的前一个点（调用SetPrev方法）。
再把新创建的点添加到待遍历点的集合中（本文中为SortedList<Vertex>）。
当遍历完当前点的四个方向后，把当前点的Visited属性设置为true，并从带遍历点的集合中移除。
说明：当前算法的实现中仅考虑总距离（从起始点到当前点的距离加上猜测距离，起始点距离为0）、猜测距离（从当前点到目标点的距离，为从当前点到目标点的折线长度）和拐点个数（X或Y坐标变化时拐点个数加1）。
5，递归第4步。要么找到和目标点坐标相同的点（即，找到了目标点），要么待遍历点的集合为空（即，无法找到通往目标点的路径）。
6，当找到通往目标点的路径之后，通过Straightener（调直器）调直路径，减少拐点。
7，处理起始点和目标点。用原来的起始点和目标点替换坐标对齐到step整数倍的起始点和目标点，并调直其相邻拐点的X坐标或Y坐标。
8，返回最终路径。
如下两个图所示
第一张图为A*算法查找出来的最优路径（不一定是最短路径，依赖于算法的实现）
第二张图为调直后的最直路径（拐点最少）
代码
由于工程太大，仅上传必要的代码文件。
1 using System;
2 using System.Collections.Generic;
3 using System.Drawing;
4
5 namespace Pathfinding
6 {
7 /// <summary>
8 /// A*算法
9 /// </summary>
10 public class AStarAlgorithm
11 {
12 private Vertex m_goal;
13 private Locator m_locator;
14 private SortedList<Vertex> m_openSet;
15 private Orientation m_orientation;
16 private Vertex m_start;
17 /// <summary>
18 /// 查找最优路径
19 /// </summary>
20 /// <param name="canvas">画布</param>
21 /// <param name="obstacles">障碍物</param>
22 /// <param name="step">步长</param>
23 /// <param name="voDistance">避障距离</param>
24 /// <param name="initOrient">第一层查找的方向</param>
25 /// <param name="start">起始点</param>
26 /// <param name="goal">目标点</param>
27 /// <returns></returns>
28 public Point[] Find(Rectangle canvas, List<Rectangle> obstacles, int step, int voDistance, Orientation initOrient, Point start, Point goal)
29 {
30 if (start == goal)
31 return null;
32
33 if (start.GetDistanceTo(goal) < step)
34 return this.ProcessShortPath(start, goal);
35
36 this.Init(canvas, obstacles, step, voDistance, initOrient, start, goal);
37 this.AddIntoOpenSet(this.m_start);
38
39 Vertex optimal = null;
40 while (this.m_openSet.Count > 0)
41 {
42 optimal = this.GetOptimalVertex();
43
44 if (this.IsGoal(optimal))
45 {
46 this.WalkTarget();
47 var path = Straightener.Straighten(this.m_locator, this.m_goal.Lines);
48 this.ProcessEndpoint(start, 0, path);
49 this.ProcessEndpoint(goal, path.Length - 1, path);
50
51 return path;
52 }
53
54 this.Walk(optimal);
55 }
56
57 return null;
58 }
59
60 /// <summary>
61 /// 添加待遍历的点
62 /// </summary>
63 /// <param name="vertex"></param>
64 private void AddIntoOpenSet(Vertex vertex)
65 {
66 if (!vertex.IsVisited)
67 this.m_openSet.Add(vertex);
68 }
69
70 /// <summary>
71 /// 获取最优点
72 /// </summary>
73 /// <returns></returns>
74 private Vertex GetOptimalVertex()
75 {
76 var cheapest = this.m_openSet.TakeFirst();
77 cheapest.IsCurrent = true;
78
79 return cheapest;
80 }
81
82 /// <summary>
83 /// 估算到目标点的距离
84 /// </summary>
85 /// <param name="vertex"></param>
86 /// <returns></returns>
87 private int GuessDistanceToGoal(Vertex vertex)
88 {
89 return Math.Abs(vertex.X - this.m_goal.X) + Math.Abs(vertex.Y - this.m_goal.Y);
90 }
91
92 /// <summary>
93 /// 初始化数据
94 /// </summary>
95 /// <param name="canvas"></param>
96 /// <param name="obstacles"></param>
97 /// <param name="step"></param>
98 /// <param name="voDistance"></param>
99 /// <param name="initOrient"></param>
100 /// <param name="start"></param>
101 /// <param name="goal"></param>
102 private void Init(Rectangle canvas, List<Rectangle> obstacles, int step, int voDistance, Orientation initOrient, Point start, Point goal)
103 {
104 this.m_locator = new Locator(canvas, obstacles, step, voDistance);
105
106 this.m_locator.AlignPoint(ref start);
107 this.m_locator.AlignPoint(ref goal);
108 this.m_locator.ExcludeObstacles(start);
109 this.m_locator.ExcludeObstacles(goal);
110
111 this.m_start = new Vertex()
112 {
113 Location = start
114 };
115 this.m_goal = new Vertex()
116 {
117 Location = goal
118 };
119 this.m_openSet = new SortedList<Vertex>();
120 this.m_start.GuessDistance = this.GuessDistanceToGoal(this.m_start);
121 this.m_orientation = initOrient;
122 }
123
124 /// <summary>
125 /// 是否是目标点
126 /// </summary>
127 /// <param name="vertex"></param>
128 /// <returns></returns>
129 private bool IsGoal(Vertex vertex)
130 {
131 if (vertex.Location == this.m_goal.Location)
132 {
133 this.m_goal = vertex;
134 return true;
135 }
136
137 return false;
138 }
139
140 /// <summary>
141 /// 处理端点（起始点或目标点）
142 /// </summary>
143 /// <param name="endpoint"></param>
144 /// <param name="idx"></param>
145 /// <param name="path"></param>
146 private void ProcessEndpoint(Point endpoint, int idx, Point[] path)
147 {
148 Point approximatePoint = path[idx];
149 if (0 == idx)
150 {
151 path[0] = endpoint;
152 idx += 1;
153 }
154 else
155 {
156 path[idx] = endpoint;
157 idx -= 1;
158 }
159
160 if (approximatePoint.X == path[idx].X)
161 path[idx].X = endpoint.X;
162 else
163 path[idx].Y = endpoint.Y;
164 }
165
166 /// <summary>
167 /// 处理短路径
168 /// </summary>
169 /// <param name="start"></param>
170 /// <param name="goal"></param>
171 /// <returns></returns>
172 private Point[] ProcessShortPath(Point start, Point goal)
173 {
174 var dx = Math.Abs(goal.X - start.X);
175 var dy = Math.Abs(goal.Y - start.Y);
176 if (dx >= dy)
177 return new Point[] { start, new Point(goal.X, start.Y), goal };
178 else
179 return new Point[] { start, new Point(start.X, goal.Y), goal };
180 }
181
182 /// <summary>
183 /// 设置前一个点
184 /// </summary>
185 /// <param name="vertex"></param>
186 private void SetPrev(Vertex vertex)
187 {
188 var neighbors = vertex.Neighbors;
189 neighbors.Sort();
190 vertex.SetPrev(neighbors[0]);
191 vertex.GuessDistance = this.GuessDistanceToGoal(vertex);
192 }
193
194
195 #region Traverse Neighbors
196
197 /// <summary>
198 /// 创建东边的相邻点
199 /// </summary>
200 /// <param name="vertex"></param>
201 private void CreateEastNeighbor(Vertex vertex)
202 {
203 var location = new Point(vertex.X + this.m_locator.Step, vertex.Y);
204 if (this.m_locator.AlignPoint(ref location)
205 && Orientation.East == vertex.Location.GetOrientation(location))
206 {
207 var neighbor = new Vertex()
208 {
209 Location = location
210 };
211
212 // ◐
213 // |
214 // ◐---●---○
215 // |
216 // ◐
217 vertex.EastNeighbor = neighbor;
218 // ◐---◐
219 // | |
220 // ◐---●---○
221 // |
222 // ◐
223 neighbor.NorthNeighbor = vertex.NorthNeighbor?.EastNeighbor;
224 // ◐
225 // |
226 // ◐---●---○
227 // | |
228 // ◐---◐
229 neighbor.SouthNeighbor = vertex.SouthNeighbor?.EastNeighbor;
230
231 this.SetPrev(neighbor);
232 this.AddIntoOpenSet(neighbor);
233 }
234 else
235 vertex.CouldWalkEast = false;
236 }
237
238 /// <summary>
239 /// 创建北边的相邻点
240 /// </summary>
241 /// <param name="vertex"></param>
242 private void CreateNorthNeighbor(Vertex vertex)
243 {
244 var location = new Point(vertex.X, vertex.Y - this.m_locator.Step);
245 if (this.m_locator.AlignPoint(ref location)
246 && Orientation.North == vertex.Location.GetOrientation(location))
247 {
248 var neighbor = new Vertex()
249 {
250 Location = location
251 };
252
253 // ○
254 // |
255 // ◐---●---◐
256 // |
257 // ◐
258 vertex.NorthNeighbor = neighbor;
259 // ○---◐
260 // | |
261 // ◐---●---◐
262 // |
263 // ◐
264 neighbor.EastNeighbor = vertex.EastNeighbor?.NorthNeighbor;
265 // ◐---○
266 // | |
267 // ◐---●---◐
268 // |
269 // ◐
270 neighbor.WestNeighbor = vertex.WestNeighbor?.NorthNeighbor;
271
272 this.SetPrev(neighbor);
273 this.AddIntoOpenSet(neighbor);
274 }
275 else
276 vertex.CouldWalkNorth = false;
277 }
278
279 /// <summary>
280 /// 创建南边的相邻点
281 /// </summary>
282 /// <param name="vertex"></param>
283 private void CreateSouthNeighbor(Vertex vertex)
284 {
285 var location = new Point(vertex.X, vertex.Y + this.m_locator.Step);
286 if (this.m_locator.AlignPoint(ref location)
287 && Orientation.South == vertex.Location.GetOrientation(location))
288 {
289 var neighbor = new Vertex()
290 {
291 Location = location
292 };
293
294 // ◐
295 // |
296 // ◐---●---◐
297 // |
298 // ○
299 vertex.SouthNeighbor = neighbor;
300 // ◐
301 // |
302 // ◐---●---◐
303 // | |
304 // ○---◐
305 neighbor.EastNeighbor = vertex.EastNeighbor?.SouthNeighbor;
306 // ◐
307 // |
308 // ◐---●---◐
309 // | |
310 // ◐---○
311 neighbor.WestNeighbor = vertex.WestNeighbor?.SouthNeighbor;
312
313 this.SetPrev(neighbor);
314 this.AddIntoOpenSet(neighbor);
315 }
316 else
317 vertex.CouldWalkSouth = false;
318 }
319
320 /// <summary>
321 /// 创建西边的相邻点
322 /// </summary>
323 /// <param name="vertex"></param>
324 private void CreateWestNeighbor(Vertex vertex)
325 {
326 var location = new Point(vertex.X - this.m_locator.Step, vertex.Y);
327 if (this.m_locator.AlignPoint(ref location)
328 && Orientation.West == vertex.Location.GetOrientation(location))
329 {
330 var neighbor = new Vertex()
331 {
332 Location = location
333 };
334
335 // ◐
336 // |
337 // ○---●---◐
338 // |
339 // ◐
340 vertex.WestNeighbor = neighbor;
341 // ◐
342 // |
343 // ○---●---◐
344 // | |
345 // ◐---◐
346 neighbor.SouthNeighbor = vertex.SouthNeighbor?.WestNeighbor;
347 // ◐---◐
348 // | |
349 // ○---●---◐
350 // |
351 // ◐
352 neighbor.NorthNeighbor = vertex.NorthNeighbor?.WestNeighbor;
353
354 this.SetPrev(neighbor);
355 this.AddIntoOpenSet(neighbor);
356 }
357 else
358 vertex.CouldWalkWest = false;
359 }
360
361 /// <summary>
362 /// <para>遍历四个方位的相邻点：东、南、西、北</para>
363 /// <para>●(实心圆)表示访问过的点</para>
364 /// <para>◐(半实心圆)表示可能访问过的点</para>
365 /// <para>○(空心圆)表示未访问过的点</para>
366 /// </summary>
367 /// <param name="vertex"></param>
368 private void Walk(Vertex vertex)
369 {
370 // ◐
371 // |
372 // ◐---●---◐
373 // |
374 // ◐
375
376 var count = 0;
377 while (count++ < 4)
378 {
379 switch (this.m_orientation)
380 {
381 case Orientation.East:
382 this.WalkEast(vertex);
383 this.m_orientation = Orientation.South;
384 break;
385 case Orientation.South:
386 this.WalkSouth(vertex);
387 this.m_orientation = Orientation.West;
388 break;
389 case Orientation.West:
390 this.WalkWest(vertex);
391 this.m_orientation = Orientation.North;
392 break;
393 case Orientation.North:
394 this.WalkNorth(vertex);
395 this.m_orientation = Orientation.East;
396 break;
397 default:
398 this.m_orientation = Orientation.East;
399 break;
400 }
401 }
402
403 vertex.IsVisited = true;
404 vertex.IsCurrent = false;
405 }
406
407 /// <summary>
408 /// 遍历东边的相邻点
409 /// </summary>
410 /// <param name="vertex"></param>
411 private void WalkEast(Vertex vertex)
412 {
413 if (vertex.CouldWalkEast && vertex.EastNeighbor is null)
414 this.CreateEastNeighbor(vertex);
415 }
416
417 /// <summary>
418 /// 遍历北边的相邻点
419 /// </summary>
420 /// <param name="vertex"></param>
421 private void WalkNorth(Vertex vertex)
422 {
423 if (vertex.CouldWalkNorth && vertex.NorthNeighbor is null)
424 this.CreateNorthNeighbor(vertex);
425 }
426
427 /// <summary>
428 /// 遍历南边的相邻点
429 /// </summary>
430 /// <param name="vertex"></param>
431 private void WalkSouth(Vertex vertex)
432 {
433 if (vertex.CouldWalkSouth && vertex.SouthNeighbor is null)
434 this.CreateSouthNeighbor(vertex);
435 }
436
437 /// <summary>
438 /// 遍历目标点
439 /// </summary>
440 private void WalkTarget()
441 {
442 // 遍历目标点及其相邻点
443 this.Walk(this.m_goal);
444
445 if (null != this.m_goal.EastNeighbor)
446 this.Walk(this.m_goal.EastNeighbor);
447 if (null != this.m_goal.SouthNeighbor)
448 this.Walk(this.m_goal.SouthNeighbor);
449 if (null != this.m_goal.WestNeighbor)
450 this.Walk(this.m_goal.WestNeighbor);
451 if (null != this.m_goal.NorthNeighbor)
452 this.Walk(this.m_goal.NorthNeighbor);
453
454 this.SetPrev(this.m_goal);
455 }
456
457 /// <summary>
458 /// 遍历西边的相邻点
459 /// </summary>
460 /// <param name="vertex"></param>
461 private void WalkWest(Vertex vertex)
462 {
463 if (vertex.CouldWalkWest && vertex.WestNeighbor is null)
464 this.CreateWestNeighbor(vertex);
465 }
466
467 #endregion Traverse Neighbors
468 }
469 }AStarAlgorithm
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
namespace Pathfinding
{
/// <summary>
/// 定位器（用于避障，查找相邻点或者对齐坐标等）
/// </summary>
public class Locator
{
/// <summary>
/// 画布
/// </summary>
private readonly Rectangle m_canvas;
/// <summary>
/// 障碍物
/// </summary>
private readonly List<Rectangle> m_obstacles;
/// <summary>
/// 步长
/// </summary>
private readonly int m_step;
/// <summary>
/// 避障距离
/// </summary>
private readonly int m_voDistance;
public Locator(Rectangle canvas, List<Rectangle> obstacles, int step = 20, int voDistance = 10)
{
this.m_canvas = canvas;
if (step < 20)
step = 20;
this.m_step = (step >= 20) ? step : 20;
this.m_voDistance = (voDistance >= 10) ? voDistance : 10;
this.m_obstacles = this.BuildObstacles(obstacles);
}
/// <summary>
/// 画板
/// </summary>
public Rectangle Canvas => this.m_canvas;
/// <summary>
/// 步长
/// </summary>
public int Step => this.m_step;
/// <summary>
/// 避障距离
/// </summary>
public int VODistance => this.m_voDistance;
/// <summary>
/// 对齐坐标（把“点”的坐标值对齐到Step的整数倍）
/// </summary>
/// <param name="point"></param>
public Point AlignPoint(Point point)
{
return new Point(this.AlignCoord(point.X, 1), this.AlignCoord(point.Y, 1));
}
/// <summary>
/// <para>对齐坐标（把“点”的坐标值对齐到Step的整数倍，同时校验“点”是否在画布内或是否和障碍物冲突）</para>
/// <para>如果对齐前或对齐后的“点”坐标不在画布内或者和障碍物冲突，则返回false；否则，返回true。</para>
/// </summary>
/// <param name="point"></param>
/// <returns></returns>
public bool AlignPoint(ref Point point)
{
if (!this.m_canvas.Contains(point))
return false;
point = this.AlignPoint(point);
if (!this.m_canvas.Contains(point))
return false;
return !this.InObstacle(point);
}
/// <summary>
/// 排除包含“点”的障碍物
/// </summary>
/// <param name="point"></param>
public void ExcludeObstacles(Point point)
{
this.m_obstacles.RemoveAll(o => o.Contains(point));
}
/// <summary>
/// 判断点是否在障碍物内
/// </summary>
/// <param name="point"></param>
/// <returns></returns>
public bool InObstacle(Point point)
{
return this.m_obstacles.Exists(obst => obst.Contains(point));
}
/// <summary>
/// <para>判断水平线段或垂直线段（ab）是否和障碍物相交</para>
/// <para>a、b的顺序和结果无关</para>
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public bool IntersectWithObstacle(Point a, Point b)
{
if (a.X == b.X)
return this.IntersectVerticallyWithObstacle(a, b);
else // a.Y == b.Y
return this.IntersectHorizontallyWithObstacle(a, b);
}
/// <summary>
/// 判断水平线段(ab，其中a.X ≤ b.X)是否和障碍物相交
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public bool IntersectHorizontallyWithObstacle(Point a, Point b)
{
if (a.X > b.X)
{
var t = a;
a = b;
b = t;
}
return this.m_obstacles.Exists(obst =>
(obst.Top <= a.Y && a.Y <= obst.Bottom && ((a.X <= obst.Left && obst.Left <= b.X) || (a.X <= obst.Right && obst.Right <= b.X)))
|| obst.Contains(a)
|| obst.Contains(b));
}
/// <summary>
/// 判断垂直线段(ab，其中a.Y ≤ b.Y)是否和障碍物相交
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public bool IntersectVerticallyWithObstacle(Point a, Point b)
{
if (a.Y > b.Y)
{
var t = a;
a = b;
b = t;
}
return this.m_obstacles.Exists(obst =>
(obst.Left <= a.X && a.X <= obst.Right && ((a.Y <= obst.Top && obst.Top <= b.Y) || (a.Y <= obst.Bottom && obst.Bottom <= b.Y)))
|| obst.Contains(a)
|| obst.Contains(b));
}
/// <summary>
/// 对齐坐标的值
/// </summary>
/// <param name="val"></param>
/// <param name="direction"></param>
/// <returns></returns>
private int AlignCoord(int val, int direction)
{
int md = val % this.m_step;
if (0 == md)
return val;
else if (md <= this.m_step / 2)
return val - md;
else
return val - md + (direction * this.m_step);
}
/// <summary>
/// 构造障碍物（用于调试）
/// </summary>
/// <param name="obstacles"></param>
/// <returns></returns>
private List<Rectangle> BuildDebugObstacles(List<Rectangle> obstacles)
{
if (obstacles is null || obstacles.Count <= 0)
return new List<Rectangle>();
else
return obstacles.Select(o => new Rectangle(o.X - this.m_voDistance,
o.Y - this.m_voDistance,
o.Width + this.m_voDistance * 2,
o.Height + m_voDistance * 2)).ToList();
}
/// <summary>
/// 构造障碍物
/// </summary>
/// <param name="obstacles"></param>
/// <returns></returns>
private List<Rectangle> BuildObstacles(List<Rectangle> obstacles)
{
if (obstacles is null || obstacles.Count <= 0)
return new List<Rectangle>();
else
return obstacles.Select(o => new Rectangle(o.X - this.m_voDistance + 1,
o.Y - this.m_voDistance + 1,
o.Width + this.m_voDistance * 2 - 2,
o.Height + m_voDistance * 2 - 2)).ToList();
}
}
}Locator
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
namespace Pathfinding
{
/// <summary>
/// 路径调制器
/// </summary>
public class Straightener
{
/// <summary>
/// 满足调直的前提条件（路径最少包含4个点）
/// </summary>
private const int MIN_COUNT_POINTS = 4;
/// <summary>
/// 定位器
/// </summary>
private Locator m_locator;
/// <summary>
/// 原始路径
/// </summary>
private LinkedList<Point> m_originalPath;
/// <summary>
/// 调直后的路径
/// </summary>
private LinkedList<Point> m_straightenedPath;
private Straightener()
{
this.Reset();
}
/// <summary>
/// 调直路径，减少拐点（参数为空或小于4个点不做任何处理）
/// </summary>
/// <param name="path"></param>
/// <returns></returns>
public static Point[] Straighten(Locator locator, Point[] path)
{
if (locator is null || path is null || path.Length < MIN_COUNT_POINTS)
return path;
var worker = new Straightener();
worker.m_locator = locator;
worker.m_originalPath = new LinkedList<Point>(path);
worker.Straighten();
return worker.m_straightenedPath.ToArray();
}
/// <summary>
/// 创建假设的拐点
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
private static Point CreateHypotheticalInflection(LinkedListNode<Point> node)
{
if (node.Value.X == node.Next.Value.X)
return new Point(node.Next.Next.Value.X, node.Value.Y);
else
return new Point(node.Value.X, node.Next.Next.Value.Y);
}
/// <summary>
/// 计算线段（abcd）上拐点的个数
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="c"></param>
/// <param name="d"></param>
/// <returns></returns>
private static int GetCountInflections(Point a, Point b, Point c, Point d)
{
var inflections = 0;
if (c.X != a.X && c.Y != a.Y)
inflections++;
if (d.X != b.X && d.Y != b.Y)
inflections++;
return inflections;
}
private static int GetDistance(Point a, Point b)
{
if (a.X == b.X)
return Math.Abs(a.Y - b.Y);
else
return Math.Abs(a.X - b.X);
}
/// <summary>
/// 添加拐点
/// </summary>
/// <param name="inflection"></param>
private void AddInflection(Point inflection)
{
if (null != this.m_straightenedPath.Last
&& this.m_straightenedPath.Last.Value == inflection)
return;
var last = this.m_straightenedPath.AddLast(inflection);
if (null != last.Previous?.Previous
&& (last.Value.X == last.Previous.Previous.Value.X
|| last.Value.Y == last.Previous.Previous.Value.Y))
this.m_straightenedPath.Remove(last.Previous);
}
private void DoStraighten()
{
this.Reset();
var current = this.m_originalPath.First;
while (null != current.Next?.Next)
{
this.AddInflection(current.Value);
this.RemoveRedundantInflections(current);
if (current.Next?.Next is null)
break;
var inflection = CreateHypotheticalInflection(current);
if (!this.IntersectWithObstacle(current.Value, inflection, current.Next.Next.Value))
{
var success = false;
if (null != current.Previous)
{
var i1 = GetCountInflections(current.Previous.Value, current.Value, current.Next.Value, current.Next.Next.Value);
var i2 = GetCountInflections(current.Previous.Value, current.Value, inflection, current.Next.Next.Value);
if (i2 < i1)
{
current.Next.Value = inflection;
success = true;
}
}
else if (null != current.Next?.Next?.Next)
{
var i3 = GetCountInflections(current.Value, current.Next.Value, current.Next.Next.Value, current.Next.Next.Next.Value);
var i4 = GetCountInflections(current.Value, inflection, current.Next.Next.Value, current.Next.Next.Next.Value);
if (i4 < i3)
{
current.Next.Value = inflection;
success = true;
}
}
if (success)
{
this.RemoveRedundantInflections(current.Next);
if (current.Next?.Next is null)
break;
}
}
this.RemoveTurnBackInflections(current);
current = current.Next;
}
this.AddInflection(this.m_originalPath.Last.Previous.Value);
this.AddInflection(this.m_originalPath.Last.Value);
}
private int GetDistance()
{
var dist = 0;
var current = this.m_straightenedPath.First;
do
{
if (null != current.Next)
{
dist += GetDistance(current.Value, current.Next.Value);
current = current.Next;
}
else
break;
} while (true);
return dist;
}
/// <summary>
/// 判断线段（abc）是否和障碍物相交
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="c"></param>
/// <returns></returns>
private bool IntersectWithObstacle(Point a, Point b, Point c)
{
return this.m_locator.IntersectWithObstacle(a, b)
|| this.m_locator.IntersectWithObstacle(b, c);
}
/// <summary>
/// 删除冗余拐点
/// </summary>
/// <param name="node"></param>
private void RemoveRedundantInflections(LinkedListNode<Point> node)
{
while (true)
{
if (node.Next?.Next is null)
break;
if (node.Value.X == node.Next.Next.Value.X
|| node.Value.Y == node.Next.Next.Value.Y)
this.m_originalPath.Remove(node.Next);
else
break;
}
}
/// <summary>
/// 删除回转拐点
/// </summary>
/// <param name="node"></param>
private void RemoveTurnBackInflections(LinkedListNode<Point> node)
{
if (node.Next?.Next?.Next is null)
return;
var point = node.Value;
var nPoint = node.Next.Value;
var nnPoint = node.Next.Next.Value;
var nnnPoint = node.Next.Next.Next.Value;
// ●为已知拐点；○为假设拐点
// 消除如下形式的拐点
// ●
// |
// ○---●
// | |
// ●---●
if (point.X == nPoint.X
&& nPoint.Y == nnPoint.Y
&& nnPoint.X == nnnPoint.X)
{
var dy1 = point.Y - nPoint.Y;
var dy2 = nnnPoint.Y - nnPoint.Y;
var p1 = new Point(nnnPoint.X, point.Y);
if (Math.Abs(dy2) >= Math.Abs(dy1)
&& Math.Abs(dy1) / dy1 == Math.Abs(dy2) / dy2
&& !this.m_locator.IntersectHorizontallyWithObstacle(node.Value, p1))
{
this.m_originalPath.Remove(node.Next);
this.m_originalPath.Remove(node.Next);
this.m_originalPath.AddAfter(node, p1);
}
}
// ●为已知拐点；○为假设拐点
// 消除如下形式的拐点
// ●---○---●
// | |
// ●---●
else if (point.Y == nPoint.Y
&& nPoint.X == nnPoint.X
&& nnPoint.Y == nnnPoint.Y)
{
var dx1 = point.X - nPoint.X;
var dx2 = nnnPoint.X - nnPoint.X;
var p2 = new Point(point.X, nnnPoint.Y);
if (Math.Abs(dx2) >= Math.Abs(dx1)
&& Math.Abs(dx1) / dx1 == Math.Abs(dx2) / dx2
&& !this.m_locator.IntersectVerticallyWithObstacle(node.Value, p2))
{
this.m_originalPath.Remove(node.Next);
this.m_originalPath.Remove(node.Next);
this.m_originalPath.AddAfter(node, p2);
}
}
}
private void Reset()
{
this.m_straightenedPath = new LinkedList<Point>();
}
private void Straighten()
{
int prevDistance = 0;
int prevInflections = 0;
int distance = 0;
int inflections = 0;
while (true)
{
this.DoStraighten();
this.m_originalPath = this.m_straightenedPath;
distance = this.GetDistance();
inflections = this.m_originalPath.Count;
if (distance == prevDistance
&& inflections == prevInflections)
break;
prevDistance = distance;
prevInflections = inflections;
}
}
}
}Straightener
using System;
using System.Collections;
using System.Collections.Generic;
namespace Pathfinding
{
/// <summary>
/// <para>有序链表</para>
/// <para>此类不是线程安全的</para>
/// </summary>
/// <typeparam name="T"></typeparam>
public class SortedList<T> : IEnumerable<T> where T : IComparable<T>
{
private int m_count = 0;
private Node m_first;
private Node m_last;
public SortedList()
{
// do nothing
}
public SortedList(IEnumerable<T> collection)
{
this.AddRange(collection);
}
/// <summary>
/// 链表中的元素个数
/// </summary>
public int Count => this.m_count;
/// <summary>
/// 第一个元素
/// </summary>
public T First
{
get
{
if (null != this.m_first)
return this.m_first.Value;
else
return default(T);
}
}
public bool IsEmpty => this.m_count <= 0;
/// <summary>
/// 最后一个元素
/// </summary>
public T Last
{
get
{
if (null != this.m_last)
return this.m_last.Value;
else
return default(T);
}
}
public void Add(T value)
{
var node = new Node(value);
if (this.IsEmpty)
{
this.m_first = node;
this.m_last = node;
}
else if (value.CompareTo(this.m_first.Value) < 0)
{
node.Next = this.m_first;
this.m_first.Prev = node;
this.m_first = node;
}
else if (this.m_last.Value.CompareTo(value) <= 0)
{
node.Prev = this.m_last;
this.m_last.Next = node;
this.m_last = node;
}
else
{
Node current = this.m_first;
do
{
if (value.CompareTo(current.Value) < 0)
break;
current = current.Next;
} while (current != null);
var prev = current.Prev;
prev.Next = node;
node.Prev = prev;
node.Next = current;
current.Prev = node;
}
this.m_count++;
}
public void AddRange(IEnumerable<T> collection)
{
if (collection is null)
return;
foreach (var item in collection)
this.Add(item);
}
/// <summary>
/// 清除所有元素
/// </summary>
public void Clear()
{
this.m_first = null;
this.m_last = null;
this.m_count = 0;
}
/// <summary>
/// 判断链表是否包含指定的元素
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public bool Contains(T value)
{
if (this.IsEmpty)
return false;
var current = this.m_first;
while (null != current)
{
if (value.CompareTo(current.Value) == 0)
return true;
current = current.Next;
}
return false;
}
public int IndexOf(T value)
{
if (this.IsEmpty)
return -1;
var current = this.m_first;
var idx = 0;
while (null != current)
{
if (value.CompareTo(current.Value) == 0)
return idx;
idx++;
current = current.Next;
}
return -1;
}
/// <summary>
/// 获取IEnumerator<T>
/// </summary>
/// <returns></returns>
public IEnumerator<T> GetEnumerator()
{
return new Enumerator(this);
}
IEnumerator IEnumerable.GetEnumerator()
{
return this.GetEnumerator();
}
/// <summary>
/// 删除指定的元素
/// </summary>
/// <param name="value"></param>
public void Remove(T value)
{
if (this.IsEmpty)
return;
var current = this.m_first;
while (null != current)
{
if (value.CompareTo(current.Value) == 0)
break;
current = current.Next;
}
if (null != current)
{
var prev = current.Prev;
var next = current.Next;
if (null != prev && null != next)
{
prev.Next = next;
next.Prev = prev;
}
else if (null != prev)
this.SetLast(prev);
else
this.SetFirst(next);
this.m_count--;
}
}
/// <summary>
/// 返回并删除第一个元素
/// </summary>
/// <returns></returns>
public T TakeFirst()
{
if (this.IsEmpty)
return default(T);
var value = this.m_first.Value;
this.SetFirst(this.m_first.Next);
this.m_count--;
return value;
}
/// <summary>
/// 返回并删除最后一个元素
/// </summary>
/// <returns></returns>
public T TakeLast()
{
if (this.IsEmpty)
return default(T);
var value = this.m_last.Value;
this.SetLast(this.m_last.Prev);
this.m_count--;
return value;
}
public T[] ToArray()
{
if (this.IsEmpty)
return null;
var a = new T[this.m_count];
var current = this.m_first;
var idx = 0;
while (null != current)
{
a[idx++] = current.Value;
current = current.Next;
}
return a;
}
public List<T> ToList()
{
if (this.IsEmpty)
return null;
var l = new List<T>(this.m_count);
var current = this.m_first;
while (null != current)
{
l.Add(current.Value);
}
return l;
}
private void SetFirst(Node first)
{
this.m_first = first;
if (this.m_first is null)
this.m_last = null;
else
this.m_first.Prev = null;
}
private void SetLast(Node last)
{
this.m_last = last;
if (this.m_last is null)
this.m_first = null;
else
this.m_last.Next = null;
}
/// <summary>
/// 枚举器
/// </summary>
private class Enumerator : IEnumerator<T>
{
private readonly SortedList<T> m_list;
private readonly Node m_prevFirst = new Node(default(T));
private Node m_current;
public Enumerator(SortedList<T> list)
{
this.m_list = list;
this.Reset();
}
public T Current
{
get
{
if (null != this.m_current)
return this.m_current.Value;
else
return default(T);
}
}
object IEnumerator.Current
{
get
{
if (null != this.m_current)
return this.m_current.Value;
else
return default(T);
}
}
public void Dispose()
{
// do nothing
}
public bool MoveNext()
{
if (object.ReferenceEquals(this.m_current, this.m_prevFirst))
this.m_current = this.m_list.m_first;
else
this.m_current = this.m_current?.Next;
return null != this.m_current;
}
public void Reset()
{
this.m_current = this.m_prevFirst;
}
}
/// <summary>
/// 链表节点
/// </summary>
private class Node
{
public Node(T data)
{
this.Value = data;
}
public Node Next { get; set; }
public Node Prev { get; set; }
public T Value { get; }
public override string ToString()
{
if (null != Value)
return Value.ToString();
return null;
}
}
}
}SortedList
namespace Pathfinding
{
/// <summary>
/// 方向
/// </summary>
public enum Orientation
{
/// <summary>
/// 无方向
/// </summary>
None = 0,
/// <summary>
/// 东
/// </summary>
East = 0x1,
/// <summary>
/// 南
/// </summary>
South = 0x10,
/// <summary>
/// 西
/// </summary>
West = 0x100,
/// <summary>
/// 北
/// </summary>
North = 0x1000,
/// <summary>
/// 东西
/// </summary>
EastWest = East | West,
/// <summary>
/// 南北
/// </summary>
NorthSouth = South | North,
/// <summary>
/// 东南
/// </summary>
SouthEast = East | South,
/// <summary>
/// 西南
/// </summary>
SouthWest = South | West,
/// <summary>
/// 西北
/// </summary>
NorthWest = West | North,
/// <summary>
/// 东北
/// </summary>
NorthEast = North | East,
}
}Orientation
namespace Pathfinding
{
public static class OrientationExtension
{
/// <summary>
/// 是否为东西方向
/// </summary>
/// <param name="orient"></param>
/// <returns></returns>
public static bool IsEastWest(this Orientation orient)
{
return orient == Orientation.East
|| orient == Orientation.West
|| orient == Orientation.EastWest;
}
/// <summary>
/// 是否为南北方向
/// </summary>
/// <param name="orient"></param>
/// <returns></returns>
public static bool IsNorthSouth(this Orientation orient)
{
return orient == Orientation.South
|| orient == Orientation.North
|| orient == Orientation.NorthSouth;
}
/// <summary>
/// <para>把方向转换为EastWest或NorthSouth</para>
/// <para>如果方向不是东西方向或南北方向，则返回None</para>
/// </summary>
/// <param name="orient"></param>
/// <returns></returns>
public static Orientation ConvertToEWOrNS(this Orientation orient)
{
if (orient.IsEastWest())
return Orientation.EastWest;
else if (orient.IsNorthSouth())
return Orientation.NorthSouth;
else
return Orientation.None;
}
}
}OrientationExtension
using System;
using System.Drawing;
namespace Pathfinding
{
public static class PointExtension
{
/// <summary>
/// <para>获取第二个点相对于第一个点的方位</para>
/// <para>此方法只判断是否为正南，正北，正东或正西四个方向。</para>
/// <para>如果两个点坐标一样，则返回Orientation.None。</para>
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns>East、South、West、North</returns>
public static Orientation GetOrientation(this Point from, Point to)
{
if (from.X == to.X)
{
if (to.Y > from.Y)
return Orientation.South;
else if (to.Y < from.Y)
return Orientation.North;
}
else if (from.Y == to.Y)
{
if (to.X > from.X)
return Orientation.East;
else if (to.X < from.X)
return Orientation.West;
}
return Orientation.None;
}
/// <summary>
/// <para>判断两点之间的相对位置：东西方向或南北方向</para>
/// <para>如果两个点坐标一样或不是东西或南北方向，则返回Orientation.None。</para>
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns>EastWest或NorthSouth</returns>
public static Orientation GetOrientationEWOrNS(this Point from, Point to)
{
if (from.X == to.X && to.Y != from.Y)
{
return Orientation.NorthSouth;
}
else if (from.Y == to.Y && to.X != from.X)
{
return Orientation.EastWest;
}
return Orientation.None;
}
/// <summary>
/// 两点的位置是否为东西方向：Y坐标相等，且X坐标不相等
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static bool IsEastWest(this Point from, Point to)
{
return from.Y == to.Y && to.X != from.X;
}
/// <summary>
/// 两点的位置是否为南北方向：X坐标相等，且Y坐标不相等
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static bool IsNorthSouth(this Point from, Point to)
{
return from.X == to.X && to.Y != from.Y;
}
/// <summary>
/// 计算两点之间的距离（仅计算东西和南北方向的距离）
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static int GetAlignedDistanceTo(this Point a, Point b)
{
if (a.IsEastWest(b))
return Math.Abs(a.X - b.X);
else
return Math.Abs(a.Y - b.Y);
}
/// <summary>
/// 计算两点之间的距离
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static double GetDistanceTo(this Point from, Point to)
{
return Math.Sqrt(Math.Pow(from.X - to.X, 2.0d) + Math.Pow(from.Y - to.Y, 2.0d));
}
/// <summary>
/// 判断两个点是否在东西方向或南北方向的同一条直线上
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static bool InStraightLine(this Point a, Point b)
{
return a.X == b.X || a.Y == b.Y;
}
}
}PointExtension
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
