Blur_SSE.cpp

#ifndef EMSCRIPTEN
 
#ifdef __linux__
  #include <x86intrin.h>
#else
  #include <intrin.h>
#endif
#include <algorithm>
#include "Blur.h"
#include "Context.h"
 
using namespace Cogs::Core;
 
namespace {
 
  void boxFilterScanlineSSE(float* dst,
                            const float* src,
                            const uint32_t N,
                            const float radius)
  {
    // Assert assumptions on radius.
    auto r = std::max(0.f, std::min(static_cast<float>(N / 2) - 1, radius));
 
    auto s = _mm_set_ps1(1.f / (2.f*r + 1.f));
    auto n = static_cast<uint32_t>(r);
    auto f = _mm_set_ps1(r - n);
    auto q = _mm_sub_ps(_mm_set_ps1(1), f);
 
    __m128 sum = _mm_load_ps(src);
    for (uint32_t k = 1; k <= n; k++) {
      sum = _mm_add_ps(sum, _mm_load_ps(src + 4 * k));
    }
    sum = _mm_add_ps(sum, _mm_mul_ps(f, _mm_load_ps(src + 4 * (n + 1))));
 
#if 1
    uint32_t i = 0;
    for (; i <= n; i++) {
      _mm_store_ps(dst + 4 * i, _mm_mul_ps(s, sum));
      sum = _mm_add_ps(sum, _mm_mul_ps(q, _mm_load_ps(src + 4 * (i + n + 1))));
      sum = _mm_add_ps(sum, _mm_mul_ps(f, _mm_load_ps(src + 4 * (i + n + 2))));
      if (i - n < N) sum = _mm_sub_ps(sum, _mm_mul_ps(q, _mm_load_ps(src + 4 * (i - n))));              // if needed until n <= i.
      if (i - n - 1 < N) sum = _mm_sub_ps(sum, _mm_mul_ps(f, _mm_load_ps(src + 4 * (i - n - 1))));      // if needed until n < i.
    }
    for (; i < N - n - 2; i++) {                                                                        // run until second if in next loop can trigger.
      _mm_store_ps(dst + 4 * i, _mm_mul_ps(s, sum));
      sum = _mm_add_ps(sum, _mm_mul_ps(q, _mm_load_ps(src + 4 * (i + n + 1))));
      sum = _mm_add_ps(sum, _mm_mul_ps(f, _mm_load_ps(src + 4 * (i + n + 2))));                         // assume N - n - 1 + n + 1 <= N  <=> N <= N
      sum = _mm_sub_ps(sum, _mm_mul_ps(q, _mm_load_ps(src + 4 * (i - n))));
      sum = _mm_sub_ps(sum, _mm_mul_ps(f, _mm_load_ps(src + 4 * (i - n - 1))));
    }
    for (; i < N; i++) {
      _mm_store_ps(dst + 4 * i, _mm_mul_ps(s, sum));
      if (i + n + 1 < N) sum = _mm_add_ps(sum, _mm_mul_ps(q, _mm_load_ps(src + 4 * (i + n + 1))));      // if needed unless i < N - n - 1
      if (i + n + 2 < N) sum = _mm_add_ps(sum, _mm_mul_ps(f, _mm_load_ps(src + 4 * (i + n + 2))));      // if needed unless i < N - n - 2
      sum = _mm_sub_ps(sum, _mm_mul_ps(q, _mm_load_ps(src + 4 * (i - n))));
      sum = _mm_sub_ps(sum, _mm_mul_ps(f, _mm_load_ps(src + 4 * (i - n - 1))));
    }
 
#else
    for (uint32_t i = 0; i < N; i++) {
      _mm_store_ps(dst + 4 * i, _mm_mul_ps(s, sum));
      if (i + n + 1 < N) sum = _mm_add_ps(sum, _mm_mul_ps(q, _mm_load_ps(src + 4 * (i + n + 1))));
      if (i + n + 2 < N) sum = _mm_add_ps(sum, _mm_mul_ps(f, _mm_load_ps(src + 4 * (i + n + 2))));
      if (i - n < N) sum = _mm_sub_ps(sum, _mm_mul_ps(q, _mm_load_ps(src + 4 * (i - n))));
      if (i - n - 1 < N) sum = _mm_sub_ps(sum, _mm_mul_ps(f, _mm_load_ps(src + 4 * (i - n - 1))));
    }
#endif
  }
 
}
 
void Blur::BoxFilterPassXTaskSSE::operator()()
{
  for (uint32_t k = a; k < b; k++) {
    for (uint32_t j = 0; j < size.y; j += 4) {
      for (uint32_t i = 0; i < size.x; i += 4) {
        auto A = _mm_load_ps(src + (k*size.y + j + 0)*size.x + i);
        auto B = _mm_load_ps(src + (k*size.y + j + 1)*size.x + i);
        auto C = _mm_load_ps(src + (k*size.y + j + 2)*size.x + i);
        auto D = _mm_load_ps(src + (k*size.y + j + 3)*size.x + i);
        _MM_TRANSPOSE4_PS(A, B, C, D);
        _mm_store_ps(ptrA + 4 * (i + 0), A);
        _mm_store_ps(ptrA + 4 * (i + 1), B);
        _mm_store_ps(ptrA + 4 * (i + 2), C);
        _mm_store_ps(ptrA + 4 * (i + 3), D);
      }
      for (uint32_t l = 0; l < iterations; l++) {
        boxFilterScanlineSSE(ptrB, ptrA, size.x, radius);
        std::swap(ptrA, ptrB);
      }
      for (uint32_t i = 0; i < size.x; i += 4) {
        auto A = _mm_load_ps(ptrA + 4 * (i + 0));
        auto B = _mm_load_ps(ptrA + 4 * (i + 1));
        auto C = _mm_load_ps(ptrA + 4 * (i + 2));
        auto D = _mm_load_ps(ptrA + 4 * (i + 3));
        _MM_TRANSPOSE4_PS(A, B, C, D);
        _mm_store_ps(dst + (k*size.y + j + 0)*size.x + i, A);
        _mm_store_ps(dst + (k*size.y + j + 1)*size.x + i, B);
        _mm_store_ps(dst + (k*size.y + j + 2)*size.x + i, C);
        _mm_store_ps(dst + (k*size.y + j + 3)*size.x + i, D);
      }
    }
  }
}
 
void Blur::BoxFilterPassYTaskSSE::operator()()
{
  for (uint32_t k = a; k < b; k++) {
    for (uint32_t i = 0; i < size.x; i += 4) {
      for (uint32_t j = 0; j < size.y; j++) {
        _mm_store_ps(ptrA + 4 * j, _mm_load_ps(src + (k*size.y + j)*size.x + i));
      }
      for (uint32_t l = 0; l < iterations; l++) {
        boxFilterScanlineSSE(ptrB, ptrA, size.y, radius);
        std::swap(ptrA, ptrB);
      }
      for (uint32_t j = 0; j < size.y; j++) {
        _mm_store_ps(dst + (k*size.y + j)*size.x + i, _mm_load_ps(ptrA + 4 * j));
      }
    }
  }
}
 
 
void Blur::BoxFilterPassZTaskSSE::operator()()
{
#if 1
  for (uint32_t j = a; j < b; j++) {
 
    uint32_t i = 0;
    for (; i + 12 < size.x; i += 16) {
      for (uint32_t k = 0; k < size.z; k++) {
        _mm_store_ps(ptrA + 4 * (k + 0 * size.y), _mm_load_ps(src + (k*size.y + j)*size.x + i + 0));
        _mm_store_ps(ptrA + 4 * (k + 1 * size.y), _mm_load_ps(src + (k*size.y + j)*size.x + i + 4));
        _mm_store_ps(ptrA + 4 * (k + 2 * size.y), _mm_load_ps(src + (k*size.y + j)*size.x + i + 8));
        _mm_store_ps(ptrA + 4 * (k + 3 * size.y), _mm_load_ps(src + (k*size.y + j)*size.x + i + 12));
      }
      for (uint32_t l = 0; l < iterations; l++) {
        boxFilterScanlineSSE(ptrB + 4 * 0 * size.y, ptrA + 4 * 0 * size.y, size.z, radius);
        boxFilterScanlineSSE(ptrB + 4 * 1 * size.y, ptrA + 4 * 1 * size.y, size.z, radius);
        boxFilterScanlineSSE(ptrB + 4 * 2 * size.y, ptrA + 4 * 2 * size.y, size.z, radius);
        boxFilterScanlineSSE(ptrB + 4 * 3 * size.y, ptrA + 4 * 3 * size.y, size.z, radius);
        std::swap(ptrA, ptrB);
      }
      for (uint32_t k = 0; k < size.z; k++) {
        _mm_store_ps(dst + (k*size.y + j)*size.x + i + 0, _mm_load_ps(ptrA + 4 * (k + 0 * size.y)));
        _mm_store_ps(dst + (k*size.y + j)*size.x + i + 4, _mm_load_ps(ptrA + 4 * (k + 1 * size.y)));
        _mm_store_ps(dst + (k*size.y + j)*size.x + i + 8, _mm_load_ps(ptrA + 4 * (k + 2 * size.y)));
        _mm_store_ps(dst + (k*size.y + j)*size.x + i + 12, _mm_load_ps(ptrA + 4 * (k + 3 * size.y)));
      }
    }
    if (i < size.x) {
      for (uint32_t k = 0; k < size.z; k++) {
        _mm_store_ps(ptrA + 4 * (k + 0 * size.y), _mm_load_ps(src + (k*size.y + j)*size.x + i + 0));
        _mm_store_ps(ptrA + 4 * (k + 1 * size.y), i + 4 < size.x ? _mm_load_ps(src + (k*size.y + j)*size.x + i + 4) : _mm_setzero_ps());
        _mm_store_ps(ptrA + 4 * (k + 2 * size.y), i + 8 < size.x ? _mm_load_ps(src + (k*size.y + j)*size.x + i + 8) : _mm_setzero_ps());
        _mm_store_ps(ptrA + 4 * (k + 3 * size.y), i + 12 < size.x ? _mm_load_ps(src + (k*size.y + j)*size.x + i + 12) : _mm_setzero_ps());
      }
      for (uint32_t l = 0; l < iterations; l++) {
        boxFilterScanlineSSE(ptrB + 4 * 0 * size.y, ptrA + 4 * 0 * size.y, size.z, radius);
        boxFilterScanlineSSE(ptrB + 4 * 1 * size.y, ptrA + 4 * 1 * size.y, size.z, radius);
        boxFilterScanlineSSE(ptrB + 4 * 2 * size.y, ptrA + 4 * 2 * size.y, size.z, radius);
        boxFilterScanlineSSE(ptrB + 4 * 3 * size.y, ptrA + 4 * 3 * size.y, size.z, radius);
        std::swap(ptrA, ptrB);
      }
      for (uint32_t k = 0; k < size.z; k++) {
        _mm_store_ps(dst + (k*size.y + j)*size.x + i + 0, _mm_load_ps(ptrA + 4 * (k + 0 * size.y)));
        if (i + 4 < size.x) _mm_store_ps(dst + (k*size.y + j)*size.x + i + 4, _mm_load_ps(ptrA + 4 * (k + 1 * size.y)));
        if (i + 8 < size.x)_mm_store_ps(dst + (k*size.y + j)*size.x + i + 8, _mm_load_ps(ptrA + 4 * (k + 2 * size.y)));
        if (i + 12 < size.x)_mm_store_ps(dst + (k*size.y + j)*size.x + i + 12, _mm_load_ps(ptrA + 4 * (k + 3 * size.y)));
      }
    }
  }
 
#elif 1
  for (uint32_t j = a; j < b; j++) {
    for (uint32_t i = 0; i < size.x; i += 16) {
      for (uint32_t k = 0; k < size.z; k++) {
        _mm_store_ps(ptrA + 4 * (k + 0 * size.y), _mm_load_ps(src + (k*size.y + j)*size.x + i + 0));
        _mm_store_ps(ptrA + 4 * (k + 1 * size.y), i + 4 < size.x ? _mm_load_ps(src + (k*size.y + j)*size.x + i + 4) : _mm_setzero_ps());
        _mm_store_ps(ptrA + 4 * (k + 2 * size.y), i + 8 < size.x ? _mm_load_ps(src + (k*size.y + j)*size.x + i + 8) : _mm_setzero_ps());
        _mm_store_ps(ptrA + 4 * (k + 3 * size.y), i + 12 < size.x ? _mm_load_ps(src + (k*size.y + j)*size.x + i + 12) : _mm_setzero_ps());
      }
      for (uint32_t l = 0; l < iterations; l++) {
        boxFilterScanlineSSE(ptrB + 4 * 0 * size.y, ptrA + 4 * 0 * size.y, size.z, radius);
        boxFilterScanlineSSE(ptrB + 4 * 1 * size.y, ptrA + 4 * 1 * size.y, size.z, radius);
        boxFilterScanlineSSE(ptrB + 4 * 2 * size.y, ptrA + 4 * 2 * size.y, size.z, radius);
        boxFilterScanlineSSE(ptrB + 4 * 3 * size.y, ptrA + 4 * 3 * size.y, size.z, radius);
        std::swap(ptrA, ptrB);
      }
      for (uint32_t k = 0; k < size.z; k++) {
        _mm_store_ps(dst + (k*size.y + j)*size.x + i + 0, _mm_load_ps(ptrA + 4 * (k + 0 * size.y)));
        if (i + 4 < size.x) _mm_store_ps(dst + (k*size.y + j)*size.x + i + 4, _mm_load_ps(ptrA + 4 * (k + 1 * size.y)));
        if (i + 8 < size.x)_mm_store_ps(dst + (k*size.y + j)*size.x + i + 8, _mm_load_ps(ptrA + 4 * (k + 2 * size.y)));
        if (i + 12 < size.x)_mm_store_ps(dst + (k*size.y + j)*size.x + i + 12, _mm_load_ps(ptrA + 4 * (k + 3 * size.y)));
      }
    }
  }
#else
  for (uint32_t j = a; j < b; j++) {
    for (uint32_t i = 0; i < size.x; i += 4) {
      for (uint32_t k = 0; k < size.z; k++) {
        _mm_store_ps(ptrA + 4 * k, _mm_load_ps(src + (k*size.y + j)*size.x + i));
      }
      for (uint32_t l = 0; l < iterations; l++) {
        boxFilterScanlineSSE(ptrB, ptrA, size.z, radius);
        std::swap(ptrA, ptrB);
      }
      for (uint32_t k = 0; k < size.z; k++) {
        _mm_store_ps(dst + (k*size.y + j)*size.x + i, _mm_load_ps(ptrA + 4 * k));
      }
    }
  }
#endif
}
 
#endif