fix astar diagonal algorithm