Move function for SAD calculation to math namespace