路径查找算法应用之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 }

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();
}
}
}

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;
}
}
}