环境: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
4,从待遍历点的集合中取出第一个点(当前的最优点),遍历其东、南、西、北四个方向的相邻节点。东西两个方向和当前点的Y坐标相同,南北两个方向和当前点的X坐标相同。
相邻点距当前点的距离为step参数设定的步长。step越大,搜索速度越快,然而,可能导致折线无法通过间距较小的障碍物。
如果某个方向的相邻点不存在,则创建新的相邻点(如果相邻点不在障碍物内部的话);同时,设置新创建点的四个相邻点(也许新创建点的相邻点已经被创建了)。
把新创建的相邻点的Visited属性设置为false(当前实现中,Visited属性默认为false),然后对新创建点的所有相邻点排序,取最优点,设置为新创建点的前一个点(调用SetPrev方法)。
再把新创建的点添加到待遍历点的集合中(本文中为SortedList
当遍历完当前点的四个方向后,把当前点的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 ///
10 public class AStarAlgorithm
11 {
12 private Vertex m_goal;
13 private Locator m_locator;
14 private SortedList
15 private Orientation m_orientation;
16 private Vertex m_start;
17 ///
20 /// 画布
21 /// 障碍物
22 /// 步长
23 /// 避障距离
24 /// 第一层查找的方向
25 /// 起始点
26 /// 目标点
27 ///
28 public Point[] Find(Rectangle canvas, List
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 ///
63 ///
64 private void AddIntoOpenSet(Vertex vertex)
65 {
66 if (!vertex.IsVisited)
67 this.m_openSet.Add(vertex);
68 }
69
70 ///
73 ///
74 private Vertex GetOptimalVertex()
75 {
76 var cheapest = this.m_openSet.TakeFirst();
77 cheapest.IsCurrent = true;
78
79 return cheapest;
80 }
81
82 ///
85 ///
86 ///
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 ///
95 ///
96 ///
97 ///
98 ///
99 ///
100 ///
101 ///
102 private void Init(Rectangle canvas, List
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
120 this.m_start.GuessDistance = this.GuessDistanceToGoal(this.m_start);
121 this.m_orientation = initOrient;
122 }
123
124 ///
127 ///
128 ///
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 ///
143 ///
144 ///
145 ///
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 ///
169 ///
170 ///
171 ///
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 ///
185 ///
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 ///
200 ///
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 ///
241 ///
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 ///
282 ///
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 ///
323 ///
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 ///
367 ///
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 ///
410 ///
411 private void WalkEast(Vertex vertex)
412 {
413 if (vertex.CouldWalkEast && vertex.EastNeighbor is null)
414 this.CreateEastNeighbor(vertex);
415 }
416
417 ///
420 ///
421 private void WalkNorth(Vertex vertex)
422 {
423 if (vertex.CouldWalkNorth && vertex.NorthNeighbor is null)
424 this.CreateNorthNeighbor(vertex);
425 }
426
427 ///
430 ///
431 private void WalkSouth(Vertex vertex)
432 {
433 if (vertex.CouldWalkSouth && vertex.SouthNeighbor is null)
434 this.CreateSouthNeighbor(vertex);
435 }
436
437 ///
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 ///
460 ///
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
{
///
public class Locator
{
///
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
{
///
public class Straightener
{
///
private const int MIN_COUNT_POINTS = 4;
///
private Locator m_locator;
///
private LinkedList
///
private LinkedList
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
{
///
///
public class SortedList
{
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
{
///
public enum Orientation
{
///
None = 0,
///
East = 0x1,
///
South = 0x10,
///
West = 0x100,
///
North = 0x1000,
///
EastWest = East | West,
///
NorthSouth = South | North,
///
SouthEast = East | South,
///
SouthWest = South | West,
///
NorthWest = West | North,
///
NorthEast = North | East,
}
}
Orientation
namespace Pathfinding
{
public static class OrientationExtension
{
///
///
///
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
{
///
///
///
///
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
手机扫一扫
移动阅读更方便
你可能感兴趣的文章