Fix "Pathfinding sometimes walks into staircases"