Pathfinding now consider unit movement costs