public static float GetDistance(this Node A, Node B, Node[,] nodes)
{
var posA = A.GetLocation(nodes);
var posB = B.GetLocation(nodes);
var x = Math.Pow(posB[0] - posA[0], 2);
var y = Math.Pow(posB[1] - posA[1], 2);
return((float)Math.Sqrt(x + y));
}
public static void FindPath(Form1 window, Node[,] nodes, Node start, Node end)
{
List <Node> open = new List <Node>()
{
start
};
List <Node> closed = new List <Node>();
do
{
Node q = open.MinF();
open.Remove(q);
closed.Add(q);
int x = q.GetLocation(nodes)[0];
int y = q.GetLocation(nodes)[1];
if (nodes.IsValid(x - 1, y))
{
var s = nodes[x - 1, y];
if (s.Equals(end))
{
ShowPath(window, closed, nodes, end);
return;
}
if (!closed.Contains(s) && !s.blocked)
{
s.g = q.g + s.GetDistance(q, nodes);
s.h = s.GetDistance(end, nodes);
s.f = s.g + s.h;
open.Add(s);
}
}
if (nodes.IsValid(x + 1, y))
{
var s = nodes[x + 1, y];
if (s.Equals(end))
{
ShowPath(window, closed, nodes, end);
return;
}
if (!closed.Contains(s) && !s.blocked)
{
s.g = q.g + s.GetDistance(q, nodes);
s.h = s.GetDistance(end, nodes);
s.f = s.g + s.h;
open.Add(s);
}
}
if (nodes.IsValid(x, y + 1))
{
var s = nodes[x, y + 1];
if (s.Equals(end))
{
ShowPath(window, closed, nodes, end);
return;
}
if (!closed.Contains(s) && !s.blocked)
{
s.g = q.g + s.GetDistance(q, nodes);
s.h = s.GetDistance(end, nodes);
s.f = s.g + s.h;
open.Add(s);
}
}
if (nodes.IsValid(x, y - 1))
{
var s = nodes[x, y - 1];
if (s.Equals(end))
{
ShowPath(window, closed, nodes, end);
return;
}
if (!closed.Contains(s) && !s.blocked)
{
s.g = q.g + s.GetDistance(q, nodes);
s.h = s.GetDistance(end, nodes);
s.f = s.g + s.h;
open.Add(s);
}
}
if (nodes.IsValid(x - 1, y + 1))
{
var s = nodes[x - 1, y + 1];
if (s.Equals(end))
{
continue;
}
if (!closed.Contains(s) && !s.blocked)
{
s.g = q.g + s.GetDistance(q, nodes);
s.h = s.GetDistance(end, nodes);
s.f = s.g + s.h;
open.Add(s);
}
}
if (nodes.IsValid(x - 1, y - 1))
{
var s = nodes[x - 1, y - 1];
if (s.Equals(end))
{
ShowPath(window, closed, nodes, end);
return;
}
if (!closed.Contains(s) && !s.blocked)
{
s.g = q.g + s.GetDistance(q, nodes);
s.h = s.GetDistance(end, nodes);
s.f = s.g + s.h;
open.Add(s);
}
}
if (nodes.IsValid(x + 1, y + 1))
{
var s = nodes[x + 1, y + 1];
if (s.Equals(end))
{
ShowPath(window, closed, nodes, end);
return;
}
if (!closed.Contains(s) && !s.blocked)
{
s.g = q.g + s.GetDistance(q, nodes);
s.h = s.GetDistance(end, nodes);
s.f = s.g + s.h;
open.Add(s);
}
}
if (nodes.IsValid(x + 1, y - 1))
{
var s = nodes[x + 1, y - 1];
if (s.Equals(end))
{
ShowPath(window, closed, nodes, end);
return;
}
if (!closed.Contains(s) && !s.blocked)
{
s.g = q.g + s.GetDistance(q, nodes);
s.h = s.GetDistance(end, nodes);
s.f = s.g + s.h;
open.Add(s);
}
}
}while (open.Count != 0);
}