Added an optimization step to Graph to speed up pathfinding for certain types of graphs.