Minor optimization of new algorithm