构图及Astar 算法寻路

枫叶棋语 · 发表于 2024-1-17 13:56:05

本帖最后由枫叶棋语于 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[edge.StartPoint].Add(edge);
this.Pairs[edge.EndPoint].Add(edge);
}
}
public Graphs2D(HashSet<Edge2D> edges)
{
this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();
Flash = new Flash();
foreach (Edge2D edge in edges)
{
this.Pairs[edge.StartPoint].Add(edge);
this.Pairs[edge.EndPoint].Add(edge);
}
}
public Graphs2D()
{
this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>();
Flash = new Flash();
}
public void AddEdge(Edge2D edge)
{
this.Pairs[edge.StartPoint].Add(edge);
this.Pairs[edge.EndPoint].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[start] = 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[current];
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[current];
foreach (Edge2D neighborEdge in neighborEdges)
{
double dist = distance[current] + 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[nextPoint.Value])
{
neighborEdge.Polyline.ColorIndex = 6;
Flash.Update(neighborEdge.Polyline);
distance[nextPoint.Value] = dist;
previous[nextPoint.Value] = 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[0];
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);
}
}
}
}

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[edge.StartPoint].Add(edge); 
                this.Pairs[edge.EndPoint].Add(edge); 
            } 
 
        } 
        public Graphs2D(HashSet<Edge2D> edges) 
        { 
            this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>(); 
            Flash = new Flash(); 
            foreach (Edge2D edge in edges) 
            { 
                this.Pairs[edge.StartPoint].Add(edge); 
                this.Pairs[edge.EndPoint].Add(edge); 
 
            } 
        } 
 
        public Graphs2D() 
        { 
            this.Pairs = new DefaultDict<Point2d, HashSet<Edge2D>>(); 
            Flash = new Flash(); 
        } 
        public void AddEdge(Edge2D edge) 
        { 
            this.Pairs[edge.StartPoint].Add(edge); 
            this.Pairs[edge.EndPoint].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[start] = 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[current]; 
                        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[current]; 
                foreach (Edge2D neighborEdge in neighborEdges) 
                { 
                    double dist = distance[current] + 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[nextPoint.Value]) 
                            { 
                                neighborEdge.Polyline.ColorIndex = 6; 
                                Flash.Update(neighborEdge.Polyline); 
                                distance[nextPoint.Value] = dist; 
                                previous[nextPoint.Value] = 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[0]; 
                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); 
            } 
        } 
       
    } 
}

qiyang · 发表于 2024-1-17 16:21:14

赞美枫神

你有种再说一遍 · 发表于 2024-1-20 18:58:59

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

枫叶棋语 · 发表于 2024-1-22 08:32:58

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

Edge2d 是边

你有种再说一遍 · 发表于 2024-1-22 22:34:06

本帖最后由你有种再说一遍于 2024-1-22 22:35 编辑

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

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

账号		自动登录	找回密码
密码			注册

[【PyCAD】] 构图及Astar 算法寻路