public static List<Node> findPath(Node start, Node end)
{
var open = new List<Node>();
var closed = new List<Node>();
var current = start;
Node test;
List<Node> connected;
var cost = 1.0f;
float g, h, f;
current.g = 0;
current.h = Pathfinder.heuristic(current, end, cost);
current.f = current.g + current.h;
int l, i;
while(current != end)
{
connected = current.connected;
l = connected.Count;
for(i=0; i< l; ++i)
{
test = connected[i];
if (test == current || test.obstacle) continue;
g = current.g + cost;
h = heuristic(test, end, cost);
f = g + h;
if(open.Contains(test) || closed.Contains(test))
{
if(test.f > f)
{
test.f = f;
test.g = g;
test.h = h;
test.parent = current;
}
}
else
{
test.f = f;
test.g = g;
test.h = h;
test.parent = current;
open.Add(test);
}
}
closed.Add(current);
if (open.Count == 0) // no path found
return new List<Node>();
open = open.OrderBy(n => n.f).ToList();
current = open[0];
open.RemoveAt(0);
}
//build waypoint list
return buildPath(end, start);
}
public static List<Node> buildPath(Node end, Node start)
{
var path = new List<Node>();
var node = end;
path.Add(node);
while(node != start)
{
node = node.parent;
path.Insert(0, node);
}
return path;
}
public static void Disconnect(Node first, Node second)
{
first.connected.Remove(second);
second.connected.Remove(first);
}
public static void Connect(Node first, Node second)
{
first.connected.Add(second);
second.connected.Add(first);
}
public static float heuristic(Node start, Node end, float cost = 1.0f)
{
//basic euclidian distance
var dx = start.x - end.x;
var dy = start.y - end.y;
return (float)Math.Sqrt(dx*dx + dy *dy) * cost;
}
