Bugfixes to A* pathfinding