环境：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///
8/// A*算法
9///
10public class AStarAlgorithm
11{
12private Vertex m_goal;
13private Locator m_locator;
14private SortedList m_openSet;
15private Orientation m_orientation;
16private Vertex m_start;
17///
18/// 查找最优路径
19///
20/// 画布
21/// 障碍物
22/// 步长
23/// 避障距离
24/// 第一层查找的方向
25/// 起始点
26/// 目标点
27///
28public Point[] Find(Rectangle canvas, List obstacles, int step, int voDistance, Orientation initOrient, Point start, Point goal)
29{
30if (start == goal)
31return null;
32
33if (start.GetDistanceTo(goal) < step)
34return this.ProcessShortPath(start, goal);
35
36this.Init(canvas, obstacles, step, voDistance, initOrient, start, goal);
37this.AddIntoOpenSet(this.m_start);
38
39Vertex optimal = null;
40while (this.m_openSet.Count > 0)
41{
42optimal = this.GetOptimalVertex();
43
44if (this.IsGoal(optimal))
45{
46this.WalkTarget();
47var path = Straightener.Straighten(this.m_locator, this.m_goal.Lines);
48this.ProcessEndpoint(start, 0, path);
49this.ProcessEndpoint(goal, path.Length - 1, path);
50
51return path;
52}
53
54this.Walk(optimal);
55}
56
57return null;
58}
59
60///
61/// 添加待遍历的点
62///
63///
64private void AddIntoOpenSet(Vertex vertex)
65{
66if (!vertex.IsVisited)
67this.m_openSet.Add(vertex);
68}
69
70///
71/// 获取最优点
72///
73///
74private Vertex GetOptimalVertex()
75{
76var cheapest = this.m_openSet.TakeFirst();
77cheapest.IsCurrent = true;
78
79return cheapest;
80}
81
82///
83/// 估算到目标点的距离
84///
85///
86///
87private int GuessDistanceToGoal(Vertex vertex)
88{
89return Math.Abs(vertex.X - this.m_goal.X) + Math.Abs(vertex.Y - this.m_goal.Y);
90}
91
92///
93/// 初始化数据
94///
95///
96///
97///
98///
99///
100///
101///
102private void Init(Rectangle canvas, List obstacles, int step, int voDistance, Orientation initOrient, Point start, Point goal)
103{
104this.m_locator = new Locator(canvas, obstacles, step, voDistance);
105
106this.m_locator.AlignPoint(ref start);
107this.m_locator.AlignPoint(ref goal);
108this.m_locator.ExcludeObstacles(start);
109this.m_locator.ExcludeObstacles(goal);
110
111this.m_start = new Vertex()
112{
113Location = start
114};
115this.m_goal = new Vertex()
116{
117Location = goal
118};
119this.m_openSet = new SortedList();
120this.m_start.GuessDistance = this.GuessDistanceToGoal(this.m_start);
121this.m_orientation = initOrient;
122}
123
124///
125/// 是否是目标点
126///
127///
128///
129private bool IsGoal(Vertex vertex)
130{
131if (vertex.Location == this.m_goal.Location)
132{
133this.m_goal = vertex;
134return true;
135}
136
137return false;
138}
139
140///
141/// 处理端点（起始点或目标点）
142///
143///
144///
145///
146private void ProcessEndpoint(Point endpoint, int idx, Point[] path)
147{
148Point approximatePoint = path[idx];
149if (0 == idx)
150{
151path[0] = endpoint;
152idx += 1;
153}
154else
155{
156path[idx] = endpoint;
157idx -= 1;
158}
159
160if (approximatePoint.X == path[idx].X)
161path[idx].X = endpoint.X;
162else
163path[idx].Y = endpoint.Y;
164}
165
166///
167/// 处理短路径
168///
169///
170///
171///
172private Point[] ProcessShortPath(Point start, Point goal)
173{
174var dx = Math.Abs(goal.X - start.X);
175var dy = Math.Abs(goal.Y - start.Y);
176if (dx >= dy)
177return new Point[] { start, new Point(goal.X, start.Y), goal };
178else
179return new Point[] { start, new Point(start.X, goal.Y), goal };
180}
181
182///
183/// 设置前一个点
184///
185///
186private void SetPrev(Vertex vertex)
187{
188var neighbors = vertex.Neighbors;
189neighbors.Sort();
190vertex.SetPrev(neighbors[0]);
191vertex.GuessDistance = this.GuessDistanceToGoal(vertex);
192}
193
194
195#region Traverse Neighbors
196
197///
198/// 创建东边的相邻点
199///
200///
201private void CreateEastNeighbor(Vertex vertex)
202{
203var location = new Point(vertex.X + this.m_locator.Step, vertex.Y);
204if (this.m_locator.AlignPoint(ref location)
205&& Orientation.East == vertex.Location.GetOrientation(location))
206{
207var neighbor = new Vertex()
208{
209Location = location
210};
211
212//?
213//|
214// ?---●---○
215//|
216//?
217vertex.EastNeighbor = neighbor;
218//?---?
219//||
220// ?---●---○
221//|
222//?
223neighbor.NorthNeighbor = vertex.NorthNeighbor?.EastNeighbor;
224//?
225//|
226// ?---●---○
227//||
228//?---?
229neighbor.SouthNeighbor = vertex.SouthNeighbor?.EastNeighbor;
230
231this.SetPrev(neighbor);
232this.AddIntoOpenSet(neighbor);
233}
234else
235vertex.CouldWalkEast = false;
236}
237
238///
239/// 创建北边的相邻点
240///
241///
242private void CreateNorthNeighbor(Vertex vertex)
243{
244var location = new Point(vertex.X, vertex.Y - this.m_locator.Step);
245if (this.m_locator.AlignPoint(ref location)
246&& Orientation.North == vertex.Location.GetOrientation(location))
247{
248var neighbor = new Vertex()
249{
250Location = location
251};
252
253//○
254//|
255// ?---●---?
256//|
257//?
258vertex.NorthNeighbor = neighbor;
259//○---?
260//||
261// ?---●---?
262//|
263//?
264neighbor.EastNeighbor = vertex.EastNeighbor?.NorthNeighbor;
265// ?---○
266// ||
267// ?---●---?
268//|
269//?
270neighbor.WestNeighbor = vertex.WestNeighbor?.NorthNeighbor;
271
272this.SetPrev(neighbor);
273this.AddIntoOpenSet(neighbor);
274}
275else
276vertex.CouldWalkNorth = false;
277}
278
279///
280/// 创建南边的相邻点
281///
282///
283private void CreateSouthNeighbor(Vertex vertex)
284{
285var location = new Point(vertex.X, vertex.Y + this.m_locator.Step);
286if (this.m_locator.AlignPoint(ref location)
287&& Orientation.South == vertex.Location.GetOrientation(location))
288{
289var neighbor = new Vertex()
290{
291Location = location
292};
293
294//?
295//|
296// ?---●---?
297//|
298//○
299vertex.SouthNeighbor = neighbor;
300//?
301//|
302// ?---●---?
303//||
304//○---?
305neighbor.EastNeighbor = vertex.EastNeighbor?.SouthNeighbor;
306//?
307//|
308// ?---●---?
309// ||
310// ?---○
311neighbor.WestNeighbor = vertex.WestNeighbor?.SouthNeighbor;
312
313this.SetPrev(neighbor);
314this.AddIntoOpenSet(neighbor);
315}
316else
317vertex.CouldWalkSouth = false;
318}
319
320///
321/// 创建西边的相邻点
322///
323///
324private void CreateWestNeighbor(Vertex vertex)
325{
326var location = new Point(vertex.X - this.m_locator.Step, vertex.Y);
327if (this.m_locator.AlignPoint(ref location)
328&& Orientation.West == vertex.Location.GetOrientation(location))
329{
330var neighbor = new Vertex()
331{
332Location = location
333};
334
335//?
336//|
337// ○---●---?
338//|
339//?
340vertex.WestNeighbor = neighbor;
341//?
342//|
343// ○---●---?
344// ||
345// ?---?
346neighbor.SouthNeighbor = vertex.SouthNeighbor?.WestNeighbor;
347// ?---?
348// ||
349// ○---●---?
350//|
351//?
352neighbor.NorthNeighbor = vertex.NorthNeighbor?.WestNeighbor;
353
354this.SetPrev(neighbor);
355this.AddIntoOpenSet(neighbor);
356}
357else
358vertex.CouldWalkWest = false;
359}
360
361///
362/// 遍历四个方位的相邻点：东、南、西、北
363/// ●(实心圆)表示访问过的点
364/// ?(半实心圆)表示可能访问过的点
365/// ○(空心圆)表示未访问过的点
366///
367///
368private void Walk(Vertex vertex)
369{
370//?
371//|
372// ?---●---?
373//|
374//?
375
376var count = 0;
377while (count++ < 4)
378{
379switch (this.m_orientation)
380{
381case Orientation.East:
382this.WalkEast(vertex);
383this.m_orientation = Orientation.South;
384break;
385case Orientation.South:
386this.WalkSouth(vertex);
387this.m_orientation = Orientation.West;
388break;
389case Orientation.West:
390this.WalkWest(vertex);
391this.m_orientation = Orientation.North;
392break;
393case Orientation.North:
394this.WalkNorth(vertex);
395this.m_orientation = Orientation.East;
396break;
397default:
398this.m_orientation = Orientation.East;
399break;
400}
401}
402
403vertex.IsVisited = true;
404vertex.IsCurrent = false;
405}
406
407///
408/// 遍历东边的相邻点
409///
410///
411private void WalkEast(Vertex vertex)
412{
413if (vertex.CouldWalkEast && vertex.EastNeighbor is null)
414this.CreateEastNeighbor(vertex);
415}
416
417///
418/// 遍历北边的相邻点
419///
420///
421private void WalkNorth(Vertex vertex)
422{
423if (vertex.CouldWalkNorth && vertex.NorthNeighbor is null)
424this.CreateNorthNeighbor(vertex);
425}
426
427///
428/// 遍历南边的相邻点
429///
430///
431private void WalkSouth(Vertex vertex)
432{
433if (vertex.CouldWalkSouth && vertex.SouthNeighbor is null)
434this.CreateSouthNeighbor(vertex);
435}
436
437///
438/// 遍历目标点
439///
440private void WalkTarget()
441{
442// 遍历目标点及其相邻点
443this.Walk(this.m_goal);
444
445if (null != this.m_goal.EastNeighbor)
446this.Walk(this.m_goal.EastNeighbor);
447if (null != this.m_goal.SouthNeighbor)
448this.Walk(this.m_goal.SouthNeighbor);
449if (null != this.m_goal.WestNeighbor)
450this.Walk(this.m_goal.WestNeighbor);
451if (null != this.m_goal.NorthNeighbor)
452this.Walk(this.m_goal.NorthNeighbor);
453
454this.SetPrev(this.m_goal);
455}
456
457///
458/// 遍历西边的相邻点
459///
460///
461private void WalkWest(Vertex vertex)
462{
463if (vertex.CouldWalkWest && vertex.WestNeighbor is null)
464this.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;
///
/// 障碍物
///
private readonly List m_obstacles;
///
/// 步长
///
private readonly int m_step;
///
/// 避障距离
///
private readonly int m_voDistance;
public Locator(Rectangle canvas, List 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);
}///
/// 画板
///
public Rectangle Canvas => this.m_canvas;
///
/// 步长
///
public int Step => this.m_step;
///
/// 避障距离
///
public int VODistance => this.m_voDistance;
///
/// 对齐坐标（把“点”的坐标值对齐到Step的整数倍）
///
///
public Point AlignPoint(Point point)
{
return new Point(this.AlignCoord(point.X, 1), this.AlignCoord(point.Y, 1));
}///
/// 对齐坐标（把“点”的坐标值对齐到Step的整数倍，同时校验“点”是否在画布内或是否和障碍物冲突）
/// 如果对齐前或对齐后的“点”坐标不在画布内或者和障碍物冲突，则返回false；否则，返回true。
///
///
///
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);
}///
/// 排除包含“点”的障碍物
///
///
public void ExcludeObstacles(Point point)
{
this.m_obstacles.RemoveAll(o => o.Contains(point));
}///
/// 判断点是否在障碍物内
///
///
///
public bool InObstacle(Point point)
{
return this.m_obstacles.Exists(obst => obst.Contains(point));
}///
/// 判断水平线段或垂直线段（ab）是否和障碍物相交
/// a、b的顺序和结果无关
///
///
///
///
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);
}///
/// 判断水平线段(ab，其中a.X ≤
b.X)是否和障碍物相交
///
///
///
///
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));
}///
/// 判断垂直线段(ab，其中a.Y ≤
b.Y)是否和障碍物相交
///
///
///
///
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));
}///
/// 对齐坐标的值
///
///
///
///
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);
}///
/// 构造障碍物（用于调试）
///
///
///
private List BuildDebugObstacles(List obstacles)
{
if (obstacles is null || obstacles.Count <= 0)
return new List();
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();
}///
/// 构造障碍物
///
///
///
private List BuildObstacles(List obstacles)
{
if (obstacles is null || obstacles.Count <= 0)
return new List();
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
{
///
/// 满足调直的前提条件（路径最少包含4个点）
///
private const int MIN_COUNT_POINTS = 4;
///
/// 定位器
///
private Locator m_locator;
///
/// 原始路径
///
private LinkedList m_originalPath;
///
/// 调直后的路径
///
private LinkedList m_straightenedPath;
private Straightener()
{
this.Reset();
}///
/// 调直路径，减少拐点（参数为空或小于4个点不做任何处理）
///
///
///
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(path);
worker.Straighten();
return worker.m_straightenedPath.ToArray();
}///
/// 创建假设的拐点
///
///
///
private static Point CreateHypotheticalInflection(LinkedListNode 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);
}///
/// 计算线段（abcd）上拐点的个数
///
///
///
///
///
///
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);
}
///
/// 添加拐点
///
///
private void AddInflection(Point inflection)
{
if (null != this.m_straightenedPath.Last
&& this.m_straightenedPath.Last.Value =https://www.it610.com/article/= 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;
}///
/// 判断线段（abc）是否和障碍物相交
///
///
///
///
///
private bool IntersectWithObstacle(Point a, Point b, Point c)
{
return this.m_locator.IntersectWithObstacle(a, b)
|| this.m_locator.IntersectWithObstacle(b, c);
}///
/// 删除冗余拐点
///
///
private void RemoveRedundantInflections(LinkedListNode 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;
}
}///
/// 删除回转拐点
///
///
private void RemoveTurnBackInflections(LinkedListNode 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();
}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 : IEnumerable where T : IComparable
{
private int m_count = 0;
private Node m_first;
private Node m_last;
public SortedList()
{
// do nothing
}public SortedList(IEnumerable collection)
{
this.AddRange(collection);
}///
/// 链表中的元素个数
///
public int Count => this.m_count;
///
/// 第一个元素
///
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;
///
/// 最后一个元素
///
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 collection)
{
if (collection is null)
return;
foreach (var item in collection)
this.Add(item);
}///
/// 清除所有元素
///
public void Clear()
{
this.m_first = null;
this.m_last = null;
this.m_count = 0;
}///
/// 判断链表是否包含指定的元素
///
///
///
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;
}///
/// 获取IEnumerator
///
///
public IEnumerator GetEnumerator()
{
return new Enumerator(this);
}IEnumerator IEnumerable.GetEnumerator()
{
return this.GetEnumerator();
}///
/// 删除指定的元素
///
///
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--;
}
}///
/// 返回并删除第一个元素
///
///
public T TakeFirst()
{
if (this.IsEmpty)
return default(T);
var value = https://www.it610.com/article/this.m_first.Value;
this.SetFirst(this.m_first.Next);
this.m_count--;
return value;
}///
/// 返回并删除最后一个元素
///
///
public T TakeLast()
{
if (this.IsEmpty)
return default(T);
var value = https://www.it610.com/article/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 ToList()
{
if (this.IsEmpty)
return null;
var l = new List(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;
}///
/// 枚举器
///
private class Enumerator : IEnumerator
{
private readonly SortedList m_list;
private readonly Node m_prevFirst = new Node(default(T));
private Node m_current;
public Enumerator(SortedList 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;
}
}///
/// 链表节点
///
private class Node
{
public Node(T data)
{
this.Value = https://www.it610.com/article/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;
}///
/// 是否为南北方向
///
///
///
public static bool IsNorthSouth(this Orientation orient)
{
return orient == Orientation.South
|| orient == Orientation.North
|| orient == Orientation.NorthSouth;
}///
/// 把方向转换为EastWest或NorthSouth
/// 如果方向不是东西方向或南北方向，则返回None
///
///
///
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
{
///
/// 获取第二个点相对于第一个点的方位
/// 此方法只判断是否为正南，正北，正东或正西四个方向。
/// 如果两个点坐标一样，则返回Orientation.None。
///
///
///
/// East、South、West、North
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;
}///
/// 判断两点之间的相对位置：东西方向或南北方向
/// 如果两个点坐标一样或不是东西或南北方向，则返回Orientation.None。
///
///
///
/// EastWest或NorthSouth
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;
}///
/// 两点的位置是否为东西方向：Y坐标相等，且X坐标不相等
///
///
///
///
public static bool IsEastWest(this Point from, Point to)
{
return from.Y == to.Y && to.X != from.X;
}///
/// 两点的位置是否为南北方向：X坐标相等，且Y坐标不相等
///
///
///
///
public static bool IsNorthSouth(this Point from, Point to)
{
return from.X == to.X && to.Y != from.Y;
}///
/// 计算两点之间的距离（仅计算东西和南北方向的距离）
///
///
///
///
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);
}///
/// 计算两点之间的距离
///
///
///
///
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));
}///
/// 判断两个点是否在东西方向或南北方向的同一条直线上
///
///
///
///
public static bool InStraightLine(this Point a, Point b)
{
return a.X == b.X || a.Y == b.Y;
}
}
}
PointExtension 【路径查找算法应用之A*算法】
推荐阅读
