Replace SAD function with assembly version ~ 25% faster