public static CNode Get_Best()
{
double Max = 0.0;
CNode MaxNode = null;
foreach (CNode N in Open)
{
if (MaxNode == null)
{
MaxNode = N;
}
if (N == Var.Goal)
{
return(Var.Goal); // Don't want to divide by 0!
}
double E = Get_Cost(N);
if (E > Max)
{
Max = E;
MaxNode = N;
}
}
return(MaxNode);
}
public static void Clear_End_Edges(CNode N)
{
HashSet <Polyline> H;
if (N == Var.Goal)
{
H = Objects.G_Edges;
}
else
{
H = Objects.S_Edges;
}
foreach (Polyline P in H.ToList())
{
Box.Edges.Children.Remove(P);
PointCollection PC = P.Points;
Objects.Edges.Remove(PC);
}
H.Clear();
foreach (CNode C in N.Neighbors.ToList())
{
N.Remove_Neighbor(N);
}
N.Neighbors.Clear();
Update_Data();
}
private static void Create_Shapes(PointCollection[] Outlines)
{
foreach (PointCollection Points in Outlines)
{
Polygon Poly = new Polygon();
CNode First = null, Last = null, Prev = null;
foreach (Point P in Points)
{
CNode Node = new CNode(P, Poly, Var.Goal);
if (P == Points.First <Point>())
{
First = Node;
}
else if (P == Points.Last <Point>())
{
Last = Node;
}
if (Prev != null)
{
Connect_Nodes(Node, Prev);
}
Prev = Node;
Objects.Nodes.Add(Node);
}
Connect_Nodes(First, Last);
Poly.Points = Points;
Poly.Fill = new SolidColorBrush(Var.ShapeColor);
Poly.HorizontalAlignment = HorizontalAlignment.Left;
Poly.VerticalAlignment = VerticalAlignment.Top;
Box.Shapes.Children.Add(Poly);
}
}
public CNode(Point P, Shape S, CNode Goal = null)
{
Location = P;
Poly = S;
Neighbors = new HashSet <CNode>();
H = (Goal == null ? 0.0 : this.Get_Distance_To(Goal));
F = H;
}
public double Get_Distance_To(CNode N)
{ // Euclidean Distance
if (this == N)
{
return(0.0);
}
double X = this.Location.X - N.Location.X;
double Y = this.Location.Y - N.Location.Y;
return(Math.Sqrt(Math.Pow(X, 2) + Math.Pow(Y, 2)));
}
public static void Rebuild_End_Edges(CNode N)
{
foreach (CNode A in Objects.Nodes.ToList())
{
LineGeometry L = new LineGeometry(N.Location, A.Location);
if (!Util.CollisionExists(Objects.Shapes, L))
{
Connect_Nodes(N, A);
}
}
Update_Data();
}
private static async Task <int> Draw_Solution(double Timing)
{
CNode Next = Var.Goal;
PointCollection Path = new PointCollection();
Polyline Solution = new Polyline();
while (Next != null)
{
Path.Add(new Point(Next.Location.X, Next.Location.Y));
Next = Next.Parent;
await Task.Delay(Var.Speed);
}
Solution.Points = Path;
Graph.Add_Solution(Solution, Timing);
return(0);
}
private static bool Improve_Solution()
{ // Should be good
CNode Current = null;
double newE;
while (Open.Count > 0)
{
Current = Get_Best();
if (Current == null)
{
return(false);
}
Open.Remove(Current);
newE = Get_Cost(Current);
if (newE < E)
{
E = newE;
}
if (Current == Var.Goal)
{
G = Current.G;
return(true);
}
foreach (CNode N in Current.Get_Neighbors())
{
double newG = Current.G + Current.Get_Distance_To(N);
if (newG < N.G)
{
N.G = newG;
N.Parent = Current;
if ((N.G + N.H) < G)
{
N.E = Get_Cost(N);
Open.Add(N);
}
}
}
}
return(false);
}
// Create Edges and Neighbors
private static void Connect_Nodes(CNode A, CNode B)
{
PointCollection Edge = new PointCollection();
Polyline Line = new Polyline();
Line.Stroke = System.Windows.Media.Brushes.Chartreuse;
Line.StrokeThickness = 1;
A.Add_Neighbor(B);
B.Add_Neighbor(A);
Edge.Add(A.Location);
Edge.Add(B.Location);
Objects.Edges.Add(Edge);
Line.Points = Edge;
Box.Edges.Children.Add(Line);
if (A == Var.Goal || B == Var.Goal)
{
Objects.G_Edges.Add(Line);
}
if (A == Var.Start || B == Var.Start)
{
Objects.S_Edges.Add(Line);
}
}
public void Remove_Neighbor(CNode N)
{
Neighbors.Remove(N);
}
public void Add_Neighbor(CNode N)
{
Neighbors.Add(N);
}
private static void Set_Goals() // TO DO - SET RANDOM GOALS
{
Point PS = new Point();
Shape SS = new Ellipse();
Point PG = new Point();
Shape SG = new Ellipse();
Random Chance = new Random();
bool Colliding = true;
PathGeometry G = new PathGeometry();
PathFigure F = new PathFigure();
do
{
G = new PathGeometry();
F = new PathFigure();
PS.X = Chance.Next(Const.MIN_X, Const.MAX_X);
PS.Y = Chance.Next(Const.MIN_Y, Const.MAX_Y);
F.StartPoint = PS;
F.Segments.Add(new LineSegment(new Point(PS.X + 1, PS.Y), false));
G.Figures.Add(F);
Colliding = Util.CollisionExists(Objects.Shapes, G);
} while (Colliding);
Colliding = true;
do
{
G = new PathGeometry();
F = new PathFigure();
PG.X = Chance.Next(Const.MIN_X, Const.MAX_X);
PG.Y = Chance.Next(Const.MIN_Y, Const.MAX_Y);
F.StartPoint = PG;
F.Segments.Add(new LineSegment(new Point(PG.X + 1, PG.Y), false));
G.Figures.Add(F);
Colliding = Util.CollisionExists(Objects.Shapes, G);
} while (Colliding);
SS.Fill = new SolidColorBrush(Colors.Red);
SG.Fill = new SolidColorBrush(Colors.Chartreuse);
SS.Margin = new Thickness(PS.X - 5, PS.Y - 5, 0, 0);
SS.Width = 10;
SS.Height = 10;
SS.HorizontalAlignment = HorizontalAlignment.Left;
SS.VerticalAlignment = VerticalAlignment.Top;
SG.Margin = new Thickness(PG.X - 5, PG.Y - 5, 0, 0);
SG.Width = 10;
SG.Height = 10;
SG.HorizontalAlignment = HorizontalAlignment.Left;
SG.VerticalAlignment = VerticalAlignment.Top;
Box.Shapes.Children.Add(SS);
Box.Shapes.Children.Add(SG);
CNode NG = new CNode(PG, SG);
CNode NS = new CNode(PS, SS, NG);
NS.G = 0.0; // Starting node initializes with g(s) = 0 | All others get max value
Objects.Nodes.Add(NG);
Objects.Nodes.Add(NS);
Var.Start = NS; Var.Goal = NG;
}
private static double Get_Cost(CNode N)
{
return((G - N.G) / N.H);
}
