Added slight optimization to corner waypointing