public IList<Vector2> FindPath (Vector2 start, Vector2 goal)
{
pathFound = true;
ResetNodes ();
var frontier = new MinHeap<T> ();
T startNode = new T ();
startNode.Pos = start;
T goalNode = new T ();
goalNode.Pos = goal;
startNode.Goal = goalNode;
frontier.Insert (startNode);
cameFrom.Add (startNode, startNode);
costSoFar.Add (startNode, 0);
T current = null;
while (frontier.Count > 0)
{
current = frontier.RemoveRoot ();
Console.WriteLine ("Current Node: " + current.Pos);