Add tolerance check to speed things up to avoid unneded optimization