Avoid checking for destination twice in CYapfRoadT