Distance check when adding points to route