构图及Astar 算法寻路

枫叶棋语

1. using CsCad.Collections;
   
2. using CsCad.FlashDraw;
   
3. using static CsCad.Draw.DrawEntity;
   
4. namespace CsCad.PathSearch
   
5. {
   
6.     public class Edge2D
   
7.     {
   
8.         public Point2d StartPoint;
   
9.         public Point2d EndPoint;
   
10.         public double Dist;
    
11.         public Polyline Polyline;
    
12.         public Edge2D(Line line)
    
13.         {
    
14.             this.StartPoint = new Point2d(Math.Round( line.StartPoint.X,3),Math.Round( line.StartPoint.Y,3));
    
15.             this.EndPoint = new Point2d(Math.Round( line.EndPoint.X,3),Math.Round( line.EndPoint.Y,3));
    
16.             //this.Dist = Math.Abs(this.StartPoint.X - this.EndPoint.X) + Math.Abs(this.StartPoint.Y - this.EndPoint.Y);
    
17.             this.Dist = this.StartPoint.GetDistanceTo(this.EndPoint);
    
18.             this.Polyline = CreatePolyline(this.StartPoint, this.EndPoint);
    
19.             this.Polyline.ColorIndex = 8;
    
20.         }
    
21.         public Edge2D(Point2d start, Point2d end)
    
22.         {
    
23.             this.StartPoint = new Point2d(Math.Round(start.X, 3), Math.Round(start.Y, 3));
    
24.             this.EndPoint = new Point2d(Math.Round(end.X, 3), Math.Round(end.Y, 3));
    
25.             //this.Dist = Math.Abs(this.StartPoint.X - this.EndPoint.X) + Math.Abs(this.StartPoint.Y - this.EndPoint.Y);
    
26.             this.Dist = this.StartPoint.GetDistanceTo(this.EndPoint);
    
27.             this.Polyline = CreatePolyline(this.StartPoint, this.EndPoint);
    
28.             this.Polyline.ColorIndex = 8;
    
29.         }
    
30.         public Edge2D(Point3d start, Point3d end)
    
31.         {
    
32.             this.StartPoint = new Point2d(Math.Round(start.X, 3), Math.Round(start.Y, 3));
    
33.             this.EndPoint = new Point2d(Math.Round(end.X, 3), Math.Round(end.Y, 3));
    
34.             //this.Dist = Math.Abs(this.StartPoint.X - this.EndPoint.X) + Math.Abs(this.StartPoint.Y - this.EndPoint.Y);
    
35.             this.Dist = this.StartPoint.GetDistanceTo(this.EndPoint);
    
36.             this.Polyline = CreatePolyline(this.StartPoint, this.EndPoint);
    
37.             this.Polyline.ColorIndex = 8;
    
38.         }
    
39.         public Point2d? GetNeighbor(Point2d current)
    
40.         {
    
41.             if (current == StartPoint)
    
42.             {
    
43.                 return EndPoint;
    
44.             }
    
45.             else if (current == EndPoint)
    
46.             {
    
47.                 return StartPoint;
    
48.             }
    
49.             else
    
50.             {
    
51.                 return null;
    
52.             }
    
53.         }
    
54.         public override string ToString()
    
55.         {
    
56.             return $"[{this.StartPoint},{this.EndPoint}]";
    
57.         }
    
58.     }
    
59. 
    
60.     public class Graphs2D
    
61.     {
    
62.         public DefaultDict<Point2d, HashSet<Edge2D>> Pairs;
    
63.         public Flash Flash;
    
64.         public List<Polyline> pls = new List<Polyline>();
    
65.         public Graphs2D(HashSet<Line> lines)
    
66.         {
    
67.             this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();
    
68.             Flash = new Flash();
    
69.             var edges = lines.Select(line => new Edge2D(line)).ToList();
    
70.             foreach (Edge2D edge in edges)
    
71.             {
    
72.                 this.Pairs[edge.StartPoint].Add(edge);
    
73.                 this.Pairs[edge.EndPoint].Add(edge);
    
74.             }
    
75. 
    
76.         }
    
77.         public Graphs2D(HashSet<Edge2D> edges)
    
78.         {
    
79.             this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();
    
80.             Flash = new Flash();
    
81.             foreach (Edge2D edge in edges)
    
82.             {
    
83.                 this.Pairs[edge.StartPoint].Add(edge);
    
84.                 this.Pairs[edge.EndPoint].Add(edge);
    
85. 
    
86.             }
    
87.         }
    
88. 
    
89.         public Graphs2D()
    
90.         {
    
91.             this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();
    
92.             Flash = new Flash();
    
93.         }
    
94.         public void AddEdge(Edge2D edge)
    
95.         {
    
96.             this.Pairs[edge.StartPoint].Add(edge);
    
97.             this.Pairs[edge.EndPoint].Add(edge);
    
98.         }
    
99.         public void AddLine(Line line)
    
100.         {
     
101.             var edge = new Edge2D(line);
     
102.             AddEdge(edge);
     
103. 
     
104.         }
     
105. 
     
106. 
     
107.         public List<Point2d> AstarSearch(Point2d start, Point2d end)
     
108.         {
     
109.             start = new Point2d(Math.Round(start.X, 3), Math.Round(start.Y, 3));
     
110.             end = new Point2d(Math.Round(end.X, 3), Math.Round(end.Y, 3));
     
111.             List<Point2d> path = new List<Point2d>();
     
112.             PriorityQueue2D<Point2d> queue = new PriorityQueue2D<Point2d>();
     
113.             Dictionary<Point2d, Point2d> previous = new Dictionary<Point2d, Point2d>();
     
114.             Dictionary<Point2d, double> distance = new Dictionary<Point2d, double>();
     
115.             queue.Enqueue(start, 0);
     
116.             distance[start] = 0;
     
117.             var pl = new Polyline();
     
118.             pl.ColorIndex = 1;
     
119.             if (!Pairs.ContainsKey(start) || !Pairs.ContainsKey(end))
     
120.             {
     
121.                 // 起点或终点不在Pairs的key里面，返回空路径
     
122.                 Print(Pairs.ContainsKey(start), Pairs.ContainsKey(end));
     
123.                 Flash.Clear();
     
124.                 return path;
     
125.             }
     
126.             while (queue.Count > 0)
     
127.             {
     
128.                 Point2d current = queue.Dequeue();
     
129.                 int vertext = 0;
     
130.                 if (current.Equals(end))
     
131.                 {
     
132.                     Flash.Clear();
     
133.                     Flash.Update(pl);
     
134.                     path.Add(current);
     
135.                     pl.AddVertexAt(vertext, current, 0, 0, 0);
     
136.                     while (previous.ContainsKey(current))
     
137.                     {
     
138.                         current = previous[current];
     
139.                         vertext++;
     
140.                         pl.AddVertexAt(vertext, current, 0, 0, 0);
     
141.                         Flash.Update(pl, 1);
     
142.                         path.Insert(0, current);
     
143.                     }
     
144.                     Flash.Clear();
     
145.                     return path;
     
146.                 }
     
147.                 if (!this.Pairs.ContainsKey(current)) continue;
     
148.                 HashSet<Edge2D> neighborEdges = this.Pairs[current];
     
149.                 foreach (Edge2D neighborEdge in neighborEdges)
     
150.                 {
     
151.                     double dist = distance[current] + neighborEdge.Dist;
     
152.                     var nextPoint = neighborEdge.GetNeighbor(current);
     
153.                     switch (nextPoint)
     
154.                     {
     
155.                         case null:
     
156.                             neighborEdge.Polyline.ColorIndex = 2;
     
157.                             Flash.Update(neighborEdge.Polyline);
     
158.                             Print(current);
     
159.                             continue;
     
160.                         default:
     
161.                             if (!distance.ContainsKey(nextPoint.Value) || dist < distance[nextPoint.Value])
     
162.                             {
     
163.                                 neighborEdge.Polyline.ColorIndex = 6;
     
164.                                 Flash.Update(neighborEdge.Polyline);
     
165.                                 distance[nextPoint.Value] = dist;
     
166.                                 previous[nextPoint.Value] = current;
     
167.                                 double heuristic = CalculateHeuristic2D(nextPoint.Value, end);
     
168.                                 double priority = dist + heuristic;
     
169.                                 queue.Enqueue(nextPoint.Value, priority);
     
170.                             }
     
171.                             else if (distance.ContainsKey(nextPoint.Value))
     
172.                             {
     
173.                                 neighborEdge.Polyline.ColorIndex = 5;
     
174.                                 Flash.Update(neighborEdge.Polyline);
     
175.                             }
     
176. 
     
177.                             break;
     
178.                     }
     
179. 
     
180.                 }
     
181.             }
     
182.             Flash.Clear();
     
183.             return path;
     
184.         }
     
185. 
     
186.         private double CalculateHeuristic2D(Point2d current, Point2d end)
     
187.         {
     
188. 
     
189.             //return current.GetDistanceTo(end);
     
190.             return Math.Abs(current.X - end.X) + Math.Abs(current.Y - end.Y);
     
191.         }
     
192.         public class PriorityQueue2D<T>
     
193.         {
     
194.             private List<PriorityQueueElement<T>> elements = new List<PriorityQueueElement<T>>();
     
195.             private int counter = 0;
     
196. 
     
197.             public int Count => elements.Count;
     
198. 
     
199.             public void Enqueue(T item, double priority)
     
200.             {
     
201.                 elements.Add(new PriorityQueueElement<T>(item, priority, counter));
     
202.                 counter++;
     
203.                 elements.Sort();
     
204.             }
     
205. 
     
206.             public T Dequeue()
     
207.             {
     
208.                 var item = elements[0];
     
209.                 elements.RemoveAt(0);
     
210.                 return item.Value;
     
211.             }
     
212.         }
     
213. 
     
214.         public class PriorityQueueElement<T> : IComparable<PriorityQueueElement<T>>
     
215.         {
     
216.             public T Value { get; }
     
217.             public double Priority { get; }
     
218.             public int Counter { get; }
     
219. 
     
220.             public PriorityQueueElement(T value, double priority, int counter)
     
221.             {
     
222.                 Value = value;
     
223.                 Priority = priority;
     
224.                 Counter = counter;
     
225.             }
     
226. 
     
227.             public int CompareTo(PriorityQueueElement<T> other)
     
228.             {
     
229.                 int priorityComparison = Priority.CompareTo(other.Priority);
     
230.                 if (priorityComparison != 0)
     
231.                     return priorityComparison;
     
232.                 else
     
233.                     return Counter.CompareTo(other.Counter);
     
234.             }
     
235.         }
     
236.       
     
237.     }
     
238. }
     

using CsCad.Collections;<br /> using CsCad.FlashDraw;<br /> using static CsCad.Draw.DrawEntity;<br /> namespace CsCad.PathSearch<br /> {<br />     public class Edge2D<br />     {<br />         public Point2d StartPoint;<br />         public Point2d EndPoint;<br />         public double Dist;<br />         public Polyline Polyline;<br />         public Edge2D(Line line)<br />         {<br />             this.StartPoint = new Point2d(Math.Round( line.StartPoint.X,3),Math.Round( line.StartPoint.Y,3));<br />             this.EndPoint = new Point2d(Math.Round( line.EndPoint.X,3),Math.Round( line.EndPoint.Y,3));<br />             //this.Dist = Math.Abs(this.StartPoint.X - this.EndPoint.X) + Math.Abs(this.StartPoint.Y - this.EndPoint.Y);<br />             this.Dist = this.StartPoint.GetDistanceTo(this.EndPoint);<br />             this.Polyline = CreatePolyline(this.StartPoint, this.EndPoint);<br />             this.Polyline.ColorIndex = 8;<br />         }<br />         public Edge2D(Point2d start, Point2d end)<br />         {<br />             this.StartPoint = new Point2d(Math.Round(start.X, 3), Math.Round(start.Y, 3));<br />             this.EndPoint = new Point2d(Math.Round(end.X, 3), Math.Round(end.Y, 3));<br />             //this.Dist = Math.Abs(this.StartPoint.X - this.EndPoint.X) + Math.Abs(this.StartPoint.Y - this.EndPoint.Y);<br />             this.Dist = this.StartPoint.GetDistanceTo(this.EndPoint);<br />             this.Polyline = CreatePolyline(this.StartPoint, this.EndPoint);<br />             this.Polyline.ColorIndex = 8;<br />         }<br />         public Edge2D(Point3d start, Point3d end)<br />         {<br />             this.StartPoint = new Point2d(Math.Round(start.X, 3), Math.Round(start.Y, 3));<br />             this.EndPoint = new Point2d(Math.Round(end.X, 3), Math.Round(end.Y, 3));<br />             //this.Dist = Math.Abs(this.StartPoint.X - this.EndPoint.X) + Math.Abs(this.StartPoint.Y - this.EndPoint.Y);<br />             this.Dist = this.StartPoint.GetDistanceTo(this.EndPoint);<br />             this.Polyline = CreatePolyline(this.StartPoint, this.EndPoint);<br />             this.Polyline.ColorIndex = 8;<br />         }<br />         public Point2d? GetNeighbor(Point2d current)<br />         {<br />             if (current == StartPoint)<br />             {<br />                 return EndPoint;<br />             }<br />             else if (current == EndPoint)<br />             {<br />                 return StartPoint;<br />             }<br />             else<br />             {<br />                 return null;<br />             }<br />         }<br />         public override string ToString()<br />         {<br />             return $"[{this.StartPoint},{this.EndPoint}]";<br />         }<br />     }<br /> <br />     public class Graphs2D<br />     {<br />         public DefaultDict<Point2d, HashSet<Edge2D>> Pairs;<br />         public Flash Flash;<br />         public List<Polyline> pls = new List<Polyline>();<br />         public Graphs2D(HashSet<Line> lines)<br />         {<br />             this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();<br />             Flash = new Flash();<br />             var edges = lines.Select(line => new Edge2D(line)).ToList();<br />             foreach (Edge2D edge in edges)<br />             {<br />                 this.Pairs[edge.StartPoint].Add(edge);<br />                 this.Pairs[edge.EndPoint].Add(edge);<br />             }<br /> <br />         }<br />         public Graphs2D(HashSet<Edge2D> edges)<br />         {<br />             this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();<br />             Flash = new Flash();<br />             foreach (Edge2D edge in edges)<br />             {<br />                 this.Pairs[edge.StartPoint].Add(edge);<br />                 this.Pairs[edge.EndPoint].Add(edge);<br /> <br />             }<br />         }<br /> <br />         public Graphs2D()<br />         {<br />             this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();<br />             Flash = new Flash();<br />         }<br />         public void AddEdge(Edge2D edge)<br />         {<br />             this.Pairs[edge.StartPoint].Add(edge);<br />             this.Pairs[edge.EndPoint].Add(edge);<br />         }<br />         public void AddLine(Line line)<br />         {<br />             var edge = new Edge2D(line);<br />             AddEdge(edge);<br /> <br />         }<br /> <br /> <br />         public List<Point2d> AstarSearch(Point2d start, Point2d end)<br />         {<br />             start = new Point2d(Math.Round(start.X, 3), Math.Round(start.Y, 3));<br />             end = new Point2d(Math.Round(end.X, 3), Math.Round(end.Y, 3));<br />             List<Point2d> path = new List<Point2d>();<br />             PriorityQueue2D<Point2d> queue = new PriorityQueue2D<Point2d>();<br />             Dictionary<Point2d, Point2d> previous = new Dictionary<Point2d, Point2d>();<br />             Dictionary<Point2d, double> distance = new Dictionary<Point2d, double>();<br />             queue.Enqueue(start, 0);<br />             distance[start] = 0;<br />             var pl = new Polyline();<br />             pl.ColorIndex = 1;<br />             if (!Pairs.ContainsKey(start) || !Pairs.ContainsKey(end))<br />             {<br />                 // 起点或终点不在Pairs的key里面，返回空路径<br />                 Print(Pairs.ContainsKey(start), Pairs.ContainsKey(end));<br />                 Flash.Clear();<br />                 return path;<br />             }<br />             while (queue.Count > 0)<br />             {<br />                 Point2d current = queue.Dequeue();<br />                 int vertext = 0;<br />                 if (current.Equals(end))<br />                 {<br />                     Flash.Clear();<br />                     Flash.Update(pl);<br />                     path.Add(current);<br />                     pl.AddVertexAt(vertext, current, 0, 0, 0);<br />                     while (previous.ContainsKey(current))<br />                     {<br />                         current = previous[current];<br />                         vertext++;<br />                         pl.AddVertexAt(vertext, current, 0, 0, 0);<br />                         Flash.Update(pl, 1);<br />                         path.Insert(0, current);<br />                     }<br />                     Flash.Clear();<br />                     return path;<br />                 }<br />                 if (!this.Pairs.ContainsKey(current)) continue;<br />                 HashSet<Edge2D> neighborEdges = this.Pairs[current];<br />                 foreach (Edge2D neighborEdge in neighborEdges)<br />                 {<br />                     double dist = distance[current] + neighborEdge.Dist;<br />                     var nextPoint = neighborEdge.GetNeighbor(current);<br />                     switch (nextPoint)<br />                     {<br />                         case null:<br />                             neighborEdge.Polyline.ColorIndex = 2;<br />                             Flash.Update(neighborEdge.Polyline);<br />                             Print(current);<br />                             continue;<br />                         default:<br />                             if (!distance.ContainsKey(nextPoint.Value) || dist < distance[nextPoint.Value])<br />                             {<br />                                 neighborEdge.Polyline.ColorIndex = 6;<br />                                 Flash.Update(neighborEdge.Polyline);<br />                                 distance[nextPoint.Value] = dist;<br />                                 previous[nextPoint.Value] = current;<br />                                 double heuristic = CalculateHeuristic2D(nextPoint.Value, end);<br />                                 double priority = dist + heuristic;<br />                                 queue.Enqueue(nextPoint.Value, priority);<br />                             }<br />                             else if (distance.ContainsKey(nextPoint.Value))<br />                             {<br />                                 neighborEdge.Polyline.ColorIndex = 5;<br />                                 Flash.Update(neighborEdge.Polyline);<br />                             }<br /> <br />                             break;<br />                     }<br /> <br />                 }<br />             }<br />             Flash.Clear();<br />             return path;<br />         }<br /> <br />         private double CalculateHeuristic2D(Point2d current, Point2d end)<br />         {<br /> <br />             //return current.GetDistanceTo(end);<br />             return Math.Abs(current.X - end.X) + Math.Abs(current.Y - end.Y);<br />         }<br />         public class PriorityQueue2D<T><br />         {<br />             private List<PriorityQueueElement<T>> elements = new List<PriorityQueueElement<T>>();<br />             private int counter = 0;<br /> <br />             public int Count => elements.Count;<br /> <br />             public void Enqueue(T item, double priority)<br />             {<br />                 elements.Add(new PriorityQueueElement<T>(item, priority, counter));<br />                 counter++;<br />                 elements.Sort();<br />             }<br /> <br />             public T Dequeue()<br />             {<br />                 var item = elements[0];<br />                 elements.RemoveAt(0);<br />                 return item.Value;<br />             }<br />         }<br /> <br />         public class PriorityQueueElement<T> : IComparable<PriorityQueueElement<T>><br />         {<br />             public T Value { get; }<br />             public double Priority { get; }<br />             public int Counter { get; }<br /> <br />             public PriorityQueueElement(T value, double priority, int counter)<br />             {<br />                 Value = value;<br />                 Priority = priority;<br />                 Counter = counter;<br />             }<br /> <br />             public int CompareTo(PriorityQueueElement<T> other)<br />             {<br />                 int priorityComparison = Priority.CompareTo(other.Priority);<br />                 if (priorityComparison != 0)<br />                     return priorityComparison;<br />                 else<br />                     return Counter.CompareTo(other.Counter);<br />             }<br />         }<br />        <br />     }<br /> }<br />

qiyang

赞美枫神

你有种再说一遍

跟你提一个你可能一直没有意识到的问题,Edge2D这个类看起来就是一个Rect,而你插入了很多字段,使得这个信息熵特别大,我可以很明确告诉你,信息熵特别大的类跑起来会很慢.
所以这个类只需要留下四个double...但是你保留了3小数,所以用float就好了.如果还有旋转的话,就只需要加个angle.
而距离和PL类,这些不应该持有字段,而是方法To出来就好了.

枫叶棋语

你有种再说一遍 发表于 2024-1-20 18:58
跟你提一个你可能一直没有意识到的问题,Edge2D这个类看起来就是一个Rect,而你插入了很多字段,使得这个信息 ...

Edge2d 是边

你有种再说一遍

枫叶棋语 发表于 2024-1-22 08:32
Edge2d 是边

是边的话...这个类型的意义就更小了...
你想想看,一个类型的构造函数传入一个line,然后转为pline,然后...万一后续判断我又不想要这个边了,不就是白转换了吗...这就为什么很多c++类采取半构造的原因.
而且你都能写那么复杂的功能了,要考虑一下性能了.