A* path finding efficiency improvements + benchmark test.