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); }