构图及Astar 算法寻路
本帖最后由 枫叶棋语 于 2024-1-17 23:08 编辑using CsCad.Collections;
using CsCad.FlashDraw;
using static CsCad.Draw.DrawEntity;
namespace CsCad.PathSearch
{
public class Edge2D
{
public Point2d StartPoint;
public Point2d EndPoint;
public double Dist;
public Polyline Polyline;
public Edge2D(Line line)
{
this.StartPoint = new Point2d(Math.Round( line.StartPoint.X,3),Math.Round( line.StartPoint.Y,3));
this.EndPoint = new Point2d(Math.Round( line.EndPoint.X,3),Math.Round( line.EndPoint.Y,3));
//this.Dist = Math.Abs(this.StartPoint.X - this.EndPoint.X) + Math.Abs(this.StartPoint.Y - this.EndPoint.Y);
this.Dist = this.StartPoint.GetDistanceTo(this.EndPoint);
this.Polyline = CreatePolyline(this.StartPoint, this.EndPoint);
this.Polyline.ColorIndex = 8;
}
public Edge2D(Point2d start, Point2d end)
{
this.StartPoint = new Point2d(Math.Round(start.X, 3), Math.Round(start.Y, 3));
this.EndPoint = new Point2d(Math.Round(end.X, 3), Math.Round(end.Y, 3));
//this.Dist = Math.Abs(this.StartPoint.X - this.EndPoint.X) + Math.Abs(this.StartPoint.Y - this.EndPoint.Y);
this.Dist = this.StartPoint.GetDistanceTo(this.EndPoint);
this.Polyline = CreatePolyline(this.StartPoint, this.EndPoint);
this.Polyline.ColorIndex = 8;
}
public Edge2D(Point3d start, Point3d end)
{
this.StartPoint = new Point2d(Math.Round(start.X, 3), Math.Round(start.Y, 3));
this.EndPoint = new Point2d(Math.Round(end.X, 3), Math.Round(end.Y, 3));
//this.Dist = Math.Abs(this.StartPoint.X - this.EndPoint.X) + Math.Abs(this.StartPoint.Y - this.EndPoint.Y);
this.Dist = this.StartPoint.GetDistanceTo(this.EndPoint);
this.Polyline = CreatePolyline(this.StartPoint, this.EndPoint);
this.Polyline.ColorIndex = 8;
}
public Point2d? GetNeighbor(Point2d current)
{
if (current == StartPoint)
{
return EndPoint;
}
else if (current == EndPoint)
{
return StartPoint;
}
else
{
return null;
}
}
public override string ToString()
{
return $"[{this.StartPoint},{this.EndPoint}]";
}
}
public class Graphs2D
{
public DefaultDict<Point2d, HashSet<Edge2D>> Pairs;
public Flash Flash;
public List<Polyline> pls = new List<Polyline>();
public Graphs2D(HashSet<Line> lines)
{
this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();
Flash = new Flash();
var edges = lines.Select(line => new Edge2D(line)).ToList();
foreach (Edge2D edge in edges)
{
this.Pairs.Add(edge);
this.Pairs.Add(edge);
}
}
public Graphs2D(HashSet<Edge2D> edges)
{
this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();
Flash = new Flash();
foreach (Edge2D edge in edges)
{
this.Pairs.Add(edge);
this.Pairs.Add(edge);
}
}
public Graphs2D()
{
this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();
Flash = new Flash();
}
public void AddEdge(Edge2D edge)
{
this.Pairs.Add(edge);
this.Pairs.Add(edge);
}
public void AddLine(Line line)
{
var edge = new Edge2D(line);
AddEdge(edge);
}
public List<Point2d> AstarSearch(Point2d start, Point2d end)
{
start = new Point2d(Math.Round(start.X, 3), Math.Round(start.Y, 3));
end = new Point2d(Math.Round(end.X, 3), Math.Round(end.Y, 3));
List<Point2d> path = new List<Point2d>();
PriorityQueue2D<Point2d> queue = new PriorityQueue2D<Point2d>();
Dictionary<Point2d, Point2d> previous = new Dictionary<Point2d, Point2d>();
Dictionary<Point2d, double> distance = new Dictionary<Point2d, double>();
queue.Enqueue(start, 0);
distance = 0;
var pl = new Polyline();
pl.ColorIndex = 1;
if (!Pairs.ContainsKey(start) || !Pairs.ContainsKey(end))
{
// 起点或终点不在Pairs的key里面，返回空路径
Print(Pairs.ContainsKey(start), Pairs.ContainsKey(end));
Flash.Clear();
return path;
}
while (queue.Count > 0)
{
Point2d current = queue.Dequeue();
int vertext = 0;
if (current.Equals(end))
{
Flash.Clear();
Flash.Update(pl);
path.Add(current);
pl.AddVertexAt(vertext, current, 0, 0, 0);
while (previous.ContainsKey(current))
{
current = previous;
vertext++;
pl.AddVertexAt(vertext, current, 0, 0, 0);
Flash.Update(pl, 1);
path.Insert(0, current);
}
Flash.Clear();
return path;
}
if (!this.Pairs.ContainsKey(current)) continue;
HashSet<Edge2D> neighborEdges = this.Pairs;
foreach (Edge2D neighborEdge in neighborEdges)
{
double dist = distance + neighborEdge.Dist;
var nextPoint = neighborEdge.GetNeighbor(current);
switch (nextPoint)
{
case null:
neighborEdge.Polyline.ColorIndex = 2;
Flash.Update(neighborEdge.Polyline);
Print(current);
continue;
default:
if (!distance.ContainsKey(nextPoint.Value) || dist < distance)
{
neighborEdge.Polyline.ColorIndex = 6;
Flash.Update(neighborEdge.Polyline);
distance = dist;
previous = current;
double heuristic = CalculateHeuristic2D(nextPoint.Value, end);
double priority = dist + heuristic;
queue.Enqueue(nextPoint.Value, priority);
}
else if (distance.ContainsKey(nextPoint.Value))
{
neighborEdge.Polyline.ColorIndex = 5;
Flash.Update(neighborEdge.Polyline);
}
break;
}
}
}
Flash.Clear();
return path;
}
private double CalculateHeuristic2D(Point2d current, Point2d end)
{
//return current.GetDistanceTo(end);
return Math.Abs(current.X - end.X) + Math.Abs(current.Y - end.Y);
}
public class PriorityQueue2D<T>
{
private List<PriorityQueueElement<T>> elements = new List<PriorityQueueElement<T>>();
private int counter = 0;
public int Count => elements.Count;
public void Enqueue(T item, double priority)
{
elements.Add(new PriorityQueueElement<T>(item, priority, counter));
counter++;
elements.Sort();
}
public T Dequeue()
{
var item = elements;
elements.RemoveAt(0);
return item.Value;
}
}
public class PriorityQueueElement<T> : IComparable<PriorityQueueElement<T>>
{
public T Value { get; }
public double Priority { get; }
public int Counter { get; }
public PriorityQueueElement(T value, double priority, int counter)
{
Value = value;
Priority = priority;
Counter = counter;
}
public int CompareTo(PriorityQueueElement<T> other)
{
int priorityComparison = Priority.CompareTo(other.Priority);
if (priorityComparison != 0)
return priorityComparison;
else
return Counter.CompareTo(other.Counter);
}
}
}
}
赞美枫神:lol:lol 跟你提一个你可能一直没有意识到的问题,Edge2D这个类看起来就是一个Rect,而你插入了很多字段,使得这个信息熵特别大,我可以很明确告诉你,信息熵特别大的类跑起来会很慢.
所以这个类只需要留下四个double...但是你保留了3小数,所以用float就好了.如果还有旋转的话,就只需要加个angle.
而距离和PL类,这些不应该持有字段,而是方法To出来就好了. 你有种再说一遍 发表于 2024-1-20 18:58
跟你提一个你可能一直没有意识到的问题,Edge2D这个类看起来就是一个Rect,而你插入了很多字段,使得这个信息 ...
Edge2d 是边 本帖最后由 你有种再说一遍 于 2024-1-22 22:35 编辑
枫叶棋语 发表于 2024-1-22 08:32
Edge2d 是边
是边的话...这个类型的意义就更小了...
你想想看,一个类型的构造函数传入一个line,然后转为pline,然后...万一后续判断我又不想要这个边了,不就是白转换了吗...这就为什么很多c++类采取半构造的原因.
而且你都能写那么复杂的功能了,要考虑一下性能了.
页:
[1]