Optimal Pathfinding

Most likely as fast as we can get it with the current code we are using.