UniformGridSystem_sample_sse41.cpp

#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
 
//#include "C:\utils\iaca-win64\iacaMarks.h"
 
namespace {
 
  template<int lane> inline __m128 broadcast_ps(__m128 x) {
    return _mm_shuffle_ps(x, x, _MM_SHUFFLE(lane, lane, lane, lane));
  }
 
  __forceinline void quat_times_vec3_ps(__m128& out_x, __m128& out_y, __m128& out_z,
                                        const __m128& q,
                                        const __m128& v_x, const __m128& v_y, const __m128& v_z)
  {
#if 0
    // Reference
    glm::quat rot;
    rot.x = q.m128_f32[0];
    rot.y = q.m128_f32[1];
    rot.z = q.m128_f32[2];
    rot.w = q.m128_f32[3];
    for (int i = 0; i < 4; i++) {
      glm::vec3 v;
      v.x = v_x.m128_f32[i];
      v.y = v_y.m128_f32[i];
      v.z = v_z.m128_f32[i];
 
      glm::vec3 w = rot * v;
      out_x.m128_f32[i] = w.x;
      out_y.m128_f32[i] = w.y;
      out_z.m128_f32[i] = w.z;
    }
    return;
#endif
 
    __m128 q_x = _mm_shuffle_ps(q, q, _MM_SHUFFLE(0, 0, 0, 0));
    __m128 q_y = _mm_shuffle_ps(q, q, _MM_SHUFFLE(1, 1, 1, 1));
    __m128 q_z = _mm_shuffle_ps(q, q, _MM_SHUFFLE(2, 2, 2, 2));
    __m128 q_w = _mm_shuffle_ps(q, q, _MM_SHUFFLE(3, 3, 3, 3));
 
    __m128 uv_x = _mm_sub_ps(_mm_mul_ps(q_y, v_z), _mm_mul_ps(v_y, q_z));
    __m128 uv_y = _mm_sub_ps(_mm_mul_ps(q_z, v_x), _mm_mul_ps(v_z, q_x));
    __m128 uv_z = _mm_sub_ps(_mm_mul_ps(q_x, v_y), _mm_mul_ps(v_x, q_y));
    __m128 uuv_x = _mm_sub_ps(_mm_mul_ps(q_y, uv_z), _mm_mul_ps(uv_y, q_z));
    __m128 uuv_y = _mm_sub_ps(_mm_mul_ps(q_z, uv_x), _mm_mul_ps(uv_z, q_x));
    __m128 uuv_z = _mm_sub_ps(_mm_mul_ps(q_x, uv_y), _mm_mul_ps(uv_x, q_y));
    __m128 t_x = _mm_add_ps(_mm_mul_ps(q_w, uv_x), uuv_x);
    __m128 t_y = _mm_add_ps(_mm_mul_ps(q_w, uv_y), uuv_y);
    __m128 t_z = _mm_add_ps(_mm_mul_ps(q_w, uv_z), uuv_z);
    out_x = _mm_add_ps(v_x, _mm_add_ps(t_x, t_x));
    out_y = _mm_add_ps(v_y, _mm_add_ps(t_y, t_y));
    out_z = _mm_add_ps(v_z, _mm_add_ps(t_z, t_z));
  }
 
  __forceinline __m128 atan_00155_ps(__m128 x)
  {
 
    static const float signBit = -0.f;
    //static const float C0 = float(3.14159265358979323846264338327950288 / 4.0);
    //static const float C1 = 0.2447f;
    //static const float C2 = 0.0663f;
    //static const float C3 = 1.f;
 
    static const __m128 C = _mm_setr_ps(float(3.14159265358979323846264338327950288 / 4.0),
                                        0.2447f,
                                        0.0663f,
                                        1.f);
    const auto c0 = _mm_shuffle_ps(C, C, _MM_SHUFFLE(0, 0, 0, 0));
    const auto c1 = _mm_shuffle_ps(C, C, _MM_SHUFFLE(1, 1, 1, 1));
    const auto c2 = _mm_shuffle_ps(C, C, _MM_SHUFFLE(2, 2, 2, 2));
    const auto c3 = _mm_shuffle_ps(C, C, _MM_SHUFFLE(3, 3, 3, 3));
 
    __m128 sign = _mm_load1_ps(&signBit);
    __m128 abs_x = _mm_andnot_ps(sign, x);
 
    __m128 t0 = _mm_mul_ps(c0, x);       // t0 = PI/4 * x
    __m128 t1 = _mm_sub_ps(abs_x, c3);   // t1 = |x|-1
    __m128 t2 = _mm_mul_ps(abs_x, c2);   // t2 = 0.0663*|x|
    __m128 t3 = _mm_add_ps(c1, t2);      // t3 = 0.2447 - t2
    __m128 t4 = _mm_mul_ps(x, _mm_mul_ps(t1, t3));                                  // r4 = x*t1*t3
    __m128 t5 = _mm_sub_ps(t0, t4);
 
 
    return t5;
  }
 
  __forceinline __m128 asin_ps(__m128 x)
  {
    static const float signBit = -0.f;
    static const float asin_deg3_C3 = -0.0187293f;
    static const float asin_deg3_C2 = 0.0742610f;
    static const float asin_deg3_C1 = -0.2121144f;
    static const float asin_deg3_C0 = 1.5707288f;
    static const __m128 asin_deg3_C = _mm_set_ps(asin_deg3_C3, asin_deg3_C2, asin_deg3_C1, asin_deg3_C0);
    static const float one = 1.f;
    static const float piTwo = 1.5707963267948966f;
 
#if 0
    // Reference
    __m128 rv;
    for (int i = 0; i < 4; i++) {
      rv.m128_f32[i] = std::asin(x.m128_f32[i]);
    }
    return rv;
#endif
 
    __m128 sign = _mm_load1_ps(&signBit);
    __m128 abs_x = _mm_andnot_ps(sign, x);
 
#if 1
    // Max error < 2e-5
    __m128 C = _mm_load_ps((float*)(&asin_deg3_C));
    __m128 r = _mm_mul_ps(_mm_shuffle_ps(C, C, _MM_SHUFFLE(3, 3, 3, 3)), abs_x);
    r = _mm_add_ps(r, _mm_mul_ps(_mm_shuffle_ps(C, C, _MM_SHUFFLE(2, 2, 2, 2)), abs_x));
    r = _mm_add_ps(r, _mm_mul_ps(_mm_shuffle_ps(C, C, _MM_SHUFFLE(1, 1, 1, 1)), abs_x));
    r = _mm_add_ps(r, _mm_shuffle_ps(C, C, _MM_SHUFFLE(0, 0, 0, 0)));
#elif 1
    // Max error < 2e-8
    __m128 r = _mm_mul_ps(_mm_load1_ps(&asin_deg7_C7), abs_x);
    r = _mm_add_ps(r, _mm_mul_ps(_mm_load1_ps(&asin_deg7_C6), abs_x));
    r = _mm_add_ps(r, _mm_mul_ps(_mm_load1_ps(&asin_deg7_C5), abs_x));
    r = _mm_add_ps(r, _mm_mul_ps(_mm_load1_ps(&asin_deg7_C4), abs_x));
    r = _mm_add_ps(r, _mm_mul_ps(_mm_load1_ps(&asin_deg7_C3), abs_x));
    r = _mm_add_ps(r, _mm_mul_ps(_mm_load1_ps(&asin_deg7_C2), abs_x));
    r = _mm_add_ps(r, _mm_mul_ps(_mm_load1_ps(&asin_deg7_C1), abs_x));
    r = _mm_add_ps(r, _mm_load1_ps(&asin_deg7_C0));
#endif
 
    __m128 q = _mm_sub_ps(_mm_load1_ps(&one), abs_x);
#if 1
    q = _mm_sqrt_ps(q);
#else
    q = _mm_rcp_ps(_mm_rsqrt_ps(q));
#endif
 
    r = _mm_sub_ps(_mm_load1_ps(&piTwo), _mm_mul_ps(q, r));
 
    // copy sign from x
    return _mm_or_ps(r, _mm_and_ps(x, sign));
  }
 
 
  __forceinline  __m128 asin_v2_ps(__m128 x)
  {
    const __m128 sign_ps = _mm_set1_ps(-0.f);
    const __m128 one_ps = _mm_set1_ps(1.f);
    const __m128 halfpi_ps = _mm_set1_ps(1.5707963267948966f);
    const __m128 C3_ps = _mm_set1_ps(-0.0187293f);
    const __m128 C2_ps = _mm_set1_ps(0.0742610f);
    const __m128 C1_ps = _mm_set1_ps(-0.2121144f);
    const __m128 C0_ps = _mm_set1_ps(1.5707288f);
 
    __m128 abs_x = _mm_andnot_ps(sign_ps, x);
 
    __m128 a = _mm_sub_ps(one_ps, abs_x);
    a = _mm_sqrt_ps(a);
 
    __m128 b = C3_ps;
    b = _mm_add_ps(_mm_mul_ps(b, abs_x), C2_ps);
    b = _mm_add_ps(_mm_mul_ps(b, abs_x), C1_ps);
    b = _mm_add_ps(_mm_mul_ps(b, abs_x), C0_ps);
 
    __m128 rv = _mm_sub_ps(halfpi_ps, _mm_mul_ps(a, b));
 
    rv = _mm_or_ps(rv, _mm_and_ps(x, sign_ps));
 
    return rv;
  }
 
#if 0 //static unittest is no good on systems which does not have sse41 support, but extensions does not have proper unittest support
#pragma optimize( "", off )
  static struct UnitTests
  {
    UnitTests()
    {
      const unsigned N = 1000;
      for (unsigned i = 0; i < N; i++) {
        __m128 x, y;
        x.m128_f32[0] = (2.f / (N - 1))*i - 1.f;
        y = atan_00155_ps(x);
        auto e = std::abs(atan(x.m128_f32[0]) - y.m128_f32[0]);
        assert(e < 0.00155f);
      }
 
      for (unsigned i = 0; i < N; i++) {
        __m128 x, y;
        x.m128_f32[0] = (2.f / (N - 1))*i - 1.f;
        y = asin_v2_ps(x);
        auto e = std::abs(std::asin(x.m128_f32[0]) - y.m128_f32[0]);
        assert(e < 7e-4f);
      }
    }
  } unitTests;
#pragma optimize( "", on )
#endif
 
}
 
namespace Cogs::Core::EchoSounder {
 
  void sampleTile_border_sse41(float * data,
                               const float *v,
                               const glm::vec3 /*tileIndex*/,
                               const glm::uvec3 /*tilePos*/,
                               const glm::uvec3 dataSize,
                               const glm::uvec3 maxIndices,
                               const glm::vec3 tp,
                               const glm::vec3 scale,
                               const glm::vec3 arrayPositionGlobal,
                               const glm::vec4* frustum,
                               const float minDistanceSquared,
                               const float maxDistanceSquared,
                               const glm::quat inverseOrientation,
                               const uint32_t coordSys,
                               const uint32_t minorCount,
                               const uint32_t sampleCount,
                               const glm::vec3 polarScale,
                               const glm::vec3 polarShift)
  {
    const __m128 rot_ = _mm_set_ps(inverseOrientation.w, inverseOrientation.z, inverseOrientation.y, inverseOrientation.x);
    assert((dataSize.x & 3) == 0);
    assert(coordSys == 1);
 
    glm::vec3 ban = arrayPositionGlobal - tp;
 
    for (uint32_t z = 0; z < dataSize.z; z++) {
      __m128 pz = _mm_mul_ss(_mm_set1_ps(scale.z), _mm_set1_ps((float)z));
      __m128 qz = _mm_sub_ss(pz, _mm_set1_ps(ban.z));
      for (uint32_t y = 0; y < dataSize.y; y++) {
        __m128 py = _mm_mul_ss(_mm_set1_ps(scale.y), _mm_set1_ps((float)y));
        __m128 qy = _mm_sub_ss(py, _mm_set1_ps(ban.y));
        __m128 r2_yz = _mm_add_ss(_mm_mul_ss(qy, qy), _mm_mul_ss(qz, qz));
        __m128 in0_dot_yz = _mm_add_ss(_mm_mul_ss(_mm_set1_ps(frustum[0].y), qy), _mm_mul_ss(_mm_set1_ps(frustum[0].z), qz));
        __m128 in1_dot_yz = _mm_add_ss(_mm_mul_ss(_mm_set1_ps(frustum[1].y), qy), _mm_mul_ss(_mm_set1_ps(frustum[1].z), qz));
        __m128 in2_dot_yz = _mm_add_ss(_mm_mul_ss(_mm_set1_ps(frustum[2].y), qy), _mm_mul_ss(_mm_set1_ps(frustum[2].z), qz));
        __m128 in3_dot_yz = _mm_add_ss(_mm_mul_ss(_mm_set1_ps(frustum[3].y), qy), _mm_mul_ss(_mm_set1_ps(frustum[3].z), qz));
 
        in0_dot_yz = _mm_shuffle_ps(in0_dot_yz, in0_dot_yz, _MM_SHUFFLE(0, 0, 0, 0));
        in1_dot_yz = _mm_shuffle_ps(in1_dot_yz, in1_dot_yz, _MM_SHUFFLE(0, 0, 0, 0));
        in2_dot_yz = _mm_shuffle_ps(in2_dot_yz, in2_dot_yz, _MM_SHUFFLE(0, 0, 0, 0));
        in3_dot_yz = _mm_shuffle_ps(in3_dot_yz, in3_dot_yz, _MM_SHUFFLE(0, 0, 0, 0));
        r2_yz = _mm_shuffle_ps(r2_yz, r2_yz, _MM_SHUFFLE(0, 0, 0, 0));
 
        for (uint32_t x = 0; x < dataSize.x; x += 4) {
 
          // sample position relative to ping origin
          __m128 i = _mm_setr_ps((float)(x + 0), (float)(x + 1), (float)(x + 2), (float)(x + 3));
 
          __m128 qx = _mm_sub_ps(_mm_mul_ps(_mm_set1_ps(scale.x), i), _mm_set1_ps(ban.x));
 
          // squared radius
          __m128 r2 = _mm_add_ps(_mm_mul_ps(qx, qx), r2_yz);
 
          // compare q against frustum planes
          __m128 mask0 = _mm_cmple_ps(_mm_setzero_ps(), _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[0].x), qx), in0_dot_yz));
          __m128 mask1 = _mm_cmple_ps(_mm_setzero_ps(), _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[1].x), qx), in1_dot_yz));
          __m128 mask2 = _mm_cmple_ps(_mm_setzero_ps(), _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[2].x), qx), in2_dot_yz));
          __m128 mask3 = _mm_cmple_ps(_mm_setzero_ps(), _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[3].x), qx), in3_dot_yz));
          __m128 mask4 = _mm_cmple_ps(_mm_set1_ps(minDistanceSquared), r2);
          __m128 mask5 = _mm_cmple_ps(r2, _mm_set1_ps(maxDistanceSquared));
          __m128 mask = _mm_and_ps(_mm_and_ps(_mm_and_ps(mask0, mask1),
                                              _mm_and_ps(mask2, mask3)),
                                   _mm_and_ps(mask4, mask5));
          int movemask = _mm_movemask_ps(mask);
          if (movemask == 0) {
            continue;
          }
          // rotate into ping's orientation
          __m128 ax, ay, az;
          quat_times_vec3_ps(ax, ay, az,
                             rot_,
                             qx, broadcast_ps<0>(qy), broadcast_ps<0>(qz));
 
          __m128 r_inv = _mm_rsqrt_ps(r2);
          __m128 r = _mm_rcp_ps(r_inv);
 
          // dirX = asin(a.x/r)
           // dirY = atan(y/z)
          __m128 dirx = asin_ps(_mm_mul_ps(ax, r_inv));
          __m128 diry = atan_00155_ps(_mm_mul_ps(ay, _mm_rcp_ps(az)));
 
          // Figure out interpolation parameters
          __m128 xi_i = _mm_max_ps(_mm_setzero_ps(), _mm_mul_ps(_mm_set1_ps(polarScale.x), _mm_sub_ps(diry, _mm_set1_ps(polarShift.x))));
          __m128 xi_j = _mm_max_ps(_mm_setzero_ps(), _mm_mul_ps(_mm_set1_ps(polarScale.y), _mm_sub_ps(dirx, _mm_set1_ps(polarShift.y))));
          __m128 xi_k = _mm_max_ps(_mm_setzero_ps(), _mm_mul_ps(_mm_set1_ps(polarScale.z), _mm_sub_ps(r, _mm_set1_ps(polarShift.z))));
          __m128 tau_i = _mm_floor_ps(xi_i);
          __m128 tau_j = _mm_floor_ps(xi_j);
          __m128 tau_k = _mm_floor_ps(xi_k);
          __m128 t_i = _mm_sub_ps(xi_i, tau_i);
          __m128 t_j = _mm_sub_ps(xi_j, tau_j);
          __m128 t_k = _mm_sub_ps(xi_k, tau_k);
 
          __m128 i_i = (_mm_min_ps(_mm_set1_ps((float)maxIndices.x), tau_i));
          __m128 i_j = (_mm_min_ps(_mm_set1_ps((float)maxIndices.y), tau_j));
          __m128 i_k = (_mm_min_ps(_mm_set1_ps((float)maxIndices.z), tau_k));
          __m128 j_i = (_mm_min_ps(_mm_set1_ps((float)maxIndices.x), _mm_add_ps(tau_i, _mm_set1_ps(1.f))));
          __m128 j_j = (_mm_min_ps(_mm_set1_ps((float)maxIndices.y), _mm_add_ps(tau_j, _mm_set1_ps(1.f))));
 
          for (unsigned lane = 0; lane < 4; lane++) {
            if (((movemask >> lane) & 1) == 0) continue;
#if 0
            // Same as reference
            glm::vec3 a(ax.m128_f32[lane],
                        ay.m128_f32[lane],
                        az.m128_f32[lane]);
 
            float r = std::sqrt(r2.m128_f32[lane]);
 
            float dirX = std::asin(a.x / r);
            float dirY = std::atan(a.y / a.z);
            auto polarPos = glm::vec3(dirY, dirX, r);
 
            // Initial float index:
            glm::vec3 xi = glm::max(glm::vec3(0.f), polarScale*(polarPos - polarShift));
 
            // Figure out interpolation parameters.
            glm::vec3 tau = glm::floor(xi);
 
            glm::vec3 t = xi - tau;
            glm::uvec3 i = glm::min(maxIndices, glm::uvec3(tau));
            glm::uvec2 j = glm::min(glm::uvec2(maxIndices), glm::uvec2(i) + glm::uvec2(1));
 
            float val00 = v[(i.y*minorCount + i.x)*sampleCount + i.z];
            float val01 = v[(j.y*minorCount + i.x)*sampleCount + i.z];
            float val0 = (1.f - t.y)*val00 + t.y*val01;
 
            float val10 = v[(i.y*minorCount + j.x)*sampleCount + i.z];
            float val11 = v[(j.y*minorCount + j.x)*sampleCount + i.z];
            float val1 = (1.f - t.y)*val10 + t.y*val11;
 
            float val = (1.f - t.x)*val0 + t.x*val1;
            uint32_t index = z * dataSize.y*dataSize.x + y * dataSize.x + x + lane;
            data[index] = val;
#elif 0
            // approximate trigonometry
            glm::vec3 a(ax.m128_f32[lane],
                        ay.m128_f32[lane],
                        az.m128_f32[lane]);
 
            float r = std::sqrt(r2.m128_f32[lane]);
 
            float dirX = dirx.m128_f32[lane];
            float dirY = diry.m128_f32[lane];
            auto polarPos = glm::vec3(dirY, dirX, r);
 
            // Initial float index:
            glm::vec3 xi = glm::max(glm::vec3(0.f), polarScale*(polarPos - polarShift));
 
            // Figure out interpolation parameters.
            glm::vec3 tau = glm::floor(xi);
 
            glm::vec3 t = xi - tau;
            glm::uvec3 i = glm::min(maxIndices, glm::uvec3(tau));
            glm::uvec2 j = glm::min(glm::uvec2(maxIndices), glm::uvec2(i) + glm::uvec2(1));
 
            float val00 = v[(i.y*minorCount + i.x)*sampleCount + i.z];
            float val01 = v[(j.y*minorCount + i.x)*sampleCount + i.z];
            float val0 = (1.f - t.y)*val00 + t.y*val01;
 
            float val10 = v[(i.y*minorCount + j.x)*sampleCount + i.z];
            float val11 = v[(j.y*minorCount + j.x)*sampleCount + i.z];
            float val1 = (1.f - t.y)*val10 + t.y*val11;
 
            float val = (1.f - t.x)*val0 + t.x*val1;
            uint32_t index = z * dataSize.y*dataSize.x + y * dataSize.x + x + lane;
            data[index] = val;
#elif 0
            glm::vec3 a(ax.m128_f32[lane],
                        ay.m128_f32[lane],
                        az.m128_f32[lane]);
 
            float r_ = r.m128_f32[lane];
 
            float dirX = dirx.m128_f32[lane];
            float dirY = diry.m128_f32[lane];
            auto polarPos = glm::vec3(dirY, dirX, r_);
 
            // Initial float index:
            glm::vec3 xi = glm::max(glm::vec3(0.f), polarScale*(polarPos - polarShift));
            xi.x = xi_i.m128_f32[lane];
            xi.y = xi_j.m128_f32[lane];
            xi.z = xi_k.m128_f32[lane];
 
            // Figure out interpolation parameters.
            glm::vec3 tau = glm::floor(xi);
            //tau.x = tau_i.m128_f32[lane];
            //tau.y = tau_j.m128_f32[lane];
            //tau.z = tau_k.m128_f32[lane];
 
            glm::vec3 t = xi - tau;
            glm::uvec3 i = glm::min(maxIndices, glm::uvec3(tau));
            glm::uvec2 j = glm::min(glm::uvec2(maxIndices), glm::uvec2(i) + glm::uvec2(1));
 
            float val00 = v[(i.y*minorCount + i.x)*sampleCount + i.z];
            float val01 = v[(j.y*minorCount + i.x)*sampleCount + i.z];
            float val0 = (1.f - t.y)*val00 + t.y*val01;
 
            float val10 = v[(i.y*minorCount + j.x)*sampleCount + i.z];
            float val11 = v[(j.y*minorCount + j.x)*sampleCount + i.z];
            float val1 = (1.f - t.y)*val10 + t.y*val11;
 
            float val = (1.f - t.x)*val0 + t.x*val1;
            uint32_t index = z * dataSize.y*dataSize.x + y * dataSize.x + x + lane;
            data[index] = val;
 
#else
 
            // Figure out interpolation parameters.
            glm::vec3 tau;// = glm::floor(xi);
            tau.x = tau_i.m128_f32[lane];
            tau.y = tau_j.m128_f32[lane];
            tau.z = tau_k.m128_f32[lane];
 
            glm::vec3 t; //= xi - tau;
            t.x = t_i.m128_f32[lane];
            t.x = t_j.m128_f32[lane];
            t.x = t_k.m128_f32[lane];
 
            glm::uvec3 ii;// = glm::min(maxIndices, glm::uvec3(tau));
            ii.x = unsigned(i_i.m128_f32[lane]);
            ii.y = unsigned(i_j.m128_f32[lane]);
            ii.z = unsigned(i_k.m128_f32[lane]);
 
            glm::uvec2 j;// = glm::min(glm::uvec2(maxIndices), glm::uvec2(i) + glm::uvec2(1));
            j.x = unsigned(j_i.m128_f32[lane]);
            j.y = unsigned(j_j.m128_f32[lane]);
 
 
 
            float val00 = v[(ii.y*minorCount + ii.x)*sampleCount + ii.z];
            float val01 = v[(j.y*minorCount + ii.x)*sampleCount + ii.z];
            float val0 = (1.f - t.y)*val00 + t.y*val01;
 
            float val10 = v[(ii.y*minorCount + j.x)*sampleCount + ii.z];
            float val11 = v[(j.y*minorCount + j.x)*sampleCount + ii.z];
            float val1 = (1.f - t.y)*val10 + t.y*val11;
 
            float val = (1.f - t.x)*val0 + t.x*val1;
 
            uint32_t index = z * dataSize.y*dataSize.x + y * dataSize.x + x + lane;
            data[index] = val;
            //tile.age[index] = float(10e-7*((*m)[0].ping->timestamp - misc.ping->timestamp));
#endif
          }
        }
      }
    }
  }
 
 
 
  //#pragma optimize( "", off )
  void sampleTile_inner_sse41(float * data,
                              const float *v,
                              const glm::vec3 /*tileIndex*/,
                              const glm::uvec3 /*tilePos*/,
                              const glm::uvec3 dataSize,
                              const glm::uvec3 maxIndices,
                              const glm::vec3 tp,
                              const glm::vec3 scale,
                              const glm::vec3 arrayPositionGlobal,
                              const glm::vec4* frustum,
                              const float minDistanceSquared,
                              const float maxDistanceSquared,
                              const glm::quat inverseOrientation,
                              const uint32_t coordSys,
                              const uint32_t minorCount,
                              const uint32_t sampleCount,
                              const glm::vec3 polarScale,
                              const glm::vec3 polarShift)
  {
    const __m128 rot_ = _mm_set_ps(inverseOrientation.w, inverseOrientation.z, inverseOrientation.y, inverseOrientation.x);
    assert((dataSize.x & 3) == 0);
    assert(coordSys == 1);
 
    static const __m128 c0123 = _mm_setr_ps(0, 1, 2, 3);
    static const __m128 one_ps = _mm_setr_ps(1.f, 1.f, 1.f, 1.f);
    glm::vec3 ban = arrayPositionGlobal - tp;
 
    for (uint32_t z = 0; z < dataSize.z; z++) {
      __m128 pz = _mm_mul_ss(_mm_set1_ps(scale.z), _mm_set1_ps((float)z));
      __m128 qz = _mm_sub_ss(pz, _mm_set1_ps(ban.z));
      for (uint32_t y = 0; y < dataSize.y; y++) {
        __m128 py = _mm_mul_ss(_mm_set1_ps(scale.y), _mm_set1_ps((float)y));
        __m128 qy = _mm_sub_ss(py, _mm_set1_ps(ban.y));
        __m128 r2_yz = _mm_add_ss(_mm_mul_ss(qy, qy), _mm_mul_ss(qz, qz));
        __m128 in0_dot_yz = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[0].y), qy), _mm_mul_ps(_mm_set1_ps(frustum[0].z), qz));
        __m128 in1_dot_yz = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[1].y), qy), _mm_mul_ps(_mm_set1_ps(frustum[1].z), qz));
        __m128 in2_dot_yz = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[2].y), qy), _mm_mul_ps(_mm_set1_ps(frustum[2].z), qz));
        __m128 in3_dot_yz = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[3].y), qy), _mm_mul_ps(_mm_set1_ps(frustum[3].z), qz));
 
        in0_dot_yz = _mm_shuffle_ps(in0_dot_yz, in0_dot_yz, _MM_SHUFFLE(0, 0, 0, 0));
        in1_dot_yz = _mm_shuffle_ps(in1_dot_yz, in1_dot_yz, _MM_SHUFFLE(0, 0, 0, 0));
        in2_dot_yz = _mm_shuffle_ps(in2_dot_yz, in2_dot_yz, _MM_SHUFFLE(0, 0, 0, 0));
        in3_dot_yz = _mm_shuffle_ps(in3_dot_yz, in3_dot_yz, _MM_SHUFFLE(0, 0, 0, 0));
        r2_yz = _mm_shuffle_ps(r2_yz, r2_yz, _MM_SHUFFLE(0, 0, 0, 0));
 
        for (uint32_t x = 0; x < dataSize.x; x += 4) {
 
          // sample position relative to ping origin
          __m128 i = _mm_add_ps(_mm_set1_ps((float)x), c0123);
          __m128 qx = _mm_sub_ps(_mm_mul_ps(_mm_set1_ps(scale.x), i), _mm_set1_ps(ban.x));
 
          // squared radius
          __m128 r2 = _mm_add_ps(_mm_mul_ps(qx, qx), r2_yz);
 
          // compare q against frustum planes
          __m128 gmask0 = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[0].x), qx), in0_dot_yz);
          __m128 gmask1 = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[1].x), qx), in1_dot_yz);
          __m128 gmask2 = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[2].x), qx), in2_dot_yz);
          __m128 gmask3 = _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[3].x), qx), in3_dot_yz);
 
 
          __m128 mask0 = _mm_cmple_ps(_mm_setzero_ps(), _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[0].x), qx), in0_dot_yz));
          __m128 mask1 = _mm_cmple_ps(_mm_setzero_ps(), _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[1].x), qx), in1_dot_yz));
          __m128 mask2 = _mm_cmple_ps(_mm_setzero_ps(), _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[2].x), qx), in2_dot_yz));
          __m128 mask3 = _mm_cmple_ps(_mm_setzero_ps(), _mm_add_ps(_mm_mul_ps(_mm_set1_ps(frustum[3].x), qx), in3_dot_yz));
          __m128 mask4 = _mm_cmple_ps(_mm_set1_ps(minDistanceSquared), r2);
          __m128 mask5 = _mm_cmple_ps(r2, _mm_set1_ps(maxDistanceSquared));
          __m128 mask = _mm_and_ps(_mm_and_ps(_mm_and_ps(mask0, mask1),
                                              _mm_and_ps(mask2, mask3)),
                                   _mm_and_ps(mask4, mask5));
          //int movemask = _mm_movemask_ps(mask);
          //if (movemask == 0) {
          //  assert(false);
          //  continue;
          //}
          // rotate into ping's orientation
          __m128 ax, ay, az;
          quat_times_vec3_ps(ax, ay, az,
                             rot_,
                             qx, broadcast_ps<0>(qy), broadcast_ps<0>(qz));
 
          __m128 r_inv = _mm_rsqrt_ps(r2);
          __m128 r = _mm_rcp_ps(r_inv);
 
          // dirX = asin(a.x/r)
           // dirY = atan(y/z)
          __m128 dirx = asin_ps(_mm_mul_ps(ax, r_inv));
          __m128 diry = atan_00155_ps(_mm_mul_ps(ay, _mm_rcp_ps(az)));
 
          // Figure out interpolation parameters
          __m128 xi_i = _mm_max_ps(_mm_setzero_ps(), _mm_mul_ps(_mm_set1_ps(polarScale.x), _mm_sub_ps(diry, _mm_set1_ps(polarShift.x))));
          __m128 xi_j = _mm_max_ps(_mm_setzero_ps(), _mm_mul_ps(_mm_set1_ps(polarScale.y), _mm_sub_ps(dirx, _mm_set1_ps(polarShift.y))));
          __m128 xi_k = _mm_max_ps(_mm_setzero_ps(), _mm_mul_ps(_mm_set1_ps(polarScale.z), _mm_sub_ps(r, _mm_set1_ps(polarShift.z))));
          __m128 tau_i = _mm_floor_ps(xi_i);
          __m128 tau_j = _mm_floor_ps(xi_j);
          __m128 tau_k = _mm_floor_ps(xi_k);
          __m128 t_i = _mm_sub_ps(xi_i, tau_i);
          __m128 t_j = _mm_sub_ps(xi_j, tau_j);
          __m128 t_k = _mm_sub_ps(xi_k, tau_k);
 
          __m128 i_i = (_mm_min_ps(_mm_set1_ps((float)maxIndices.x), tau_i));
          __m128 i_j = (_mm_min_ps(_mm_set1_ps((float)maxIndices.y), tau_j));
          __m128 i_k = (_mm_min_ps(_mm_set1_ps((float)maxIndices.z), tau_k));
          __m128 j_i = (_mm_min_ps(_mm_set1_ps((float)maxIndices.x), _mm_add_ps(tau_i, _mm_set1_ps(1.f))));
          __m128 j_j = (_mm_min_ps(_mm_set1_ps((float)maxIndices.y), _mm_add_ps(tau_j, _mm_set1_ps(1.f))));
 
 
          __m128i i_i_ = _mm_cvtps_epi32(i_i);
          __m128i i_j_ = _mm_cvtps_epi32(i_j);
          __m128i i_k_ = _mm_cvtps_epi32(i_k);
 
          __m128i j_i_ = _mm_cvtps_epi32(j_i);
          __m128i j_j_ = _mm_cvtps_epi32(j_j);
 
          __m128i ix00 = _mm_add_epi32(_mm_mullo_epi32(_mm_set1_epi32(sampleCount), _mm_add_epi32(_mm_mullo_epi32(_mm_set1_epi32(minorCount), i_j_), i_i_)), i_k_);
          __m128 val00 = _mm_setr_ps(v[ix00.m128i_u32[0]],
                                     v[ix00.m128i_u32[1]],
                                     v[ix00.m128i_u32[2]],
                                     v[ix00.m128i_u32[3]]);
 
          __m128i ix01 = _mm_add_epi32(_mm_mullo_epi32(_mm_set1_epi32(sampleCount), _mm_add_epi32(_mm_mullo_epi32(_mm_set1_epi32(minorCount), j_j_), i_i_)), i_k_);
          __m128 val01 = _mm_setr_ps(v[ix01.m128i_u32[0]],
                                     v[ix01.m128i_u32[1]],
                                     v[ix01.m128i_u32[2]],
                                     v[ix01.m128i_u32[3]]);
          __m128 val0 = _mm_add_ps(_mm_mul_ps(_mm_sub_ps(one_ps, t_j), val00), _mm_mul_ps(t_j, val01));
 
 
          __m128i ix10 = _mm_add_epi32(_mm_mullo_epi32(_mm_set1_epi32(sampleCount), _mm_add_epi32(_mm_mullo_epi32(_mm_set1_epi32(minorCount), i_j_), j_i_)), i_k_);
          __m128 val10 = _mm_setr_ps(v[ix10.m128i_u32[0]],
                                     v[ix10.m128i_u32[1]],
                                     v[ix10.m128i_u32[2]],
                                     v[ix10.m128i_u32[3]]);
 
          __m128i ix11 = _mm_add_epi32(_mm_mullo_epi32(_mm_set1_epi32(sampleCount), _mm_add_epi32(_mm_mullo_epi32(_mm_set1_epi32(minorCount), j_j_), j_i_)), i_k_);
          __m128 val11 = _mm_setr_ps(v[ix01.m128i_u32[0]],
                                     v[ix01.m128i_u32[1]],
                                     v[ix01.m128i_u32[2]],
                                     v[ix01.m128i_u32[3]]);
 
          __m128 val1 = _mm_add_ps(_mm_mul_ps(_mm_sub_ps(one_ps, t_j), val10), _mm_mul_ps(t_j, val11));
 
          __m128 val_ = _mm_add_ps(_mm_mul_ps(_mm_sub_ps(one_ps, t_i), val0), _mm_mul_ps(t_i, val1));
 
          uint32_t index = z * dataSize.y*dataSize.x + y * dataSize.x + x;
          _mm_store_ps(data + index, val_);
 
 
#if 0
          for (unsigned lane = 0; lane < 4; lane++) {
            //if (((movemask >> lane) & 1) == 0) {
            //  continue;
            //}
#if 0
                // Same as reference
            glm::vec3 a(ax.m128_f32[lane],
                        ay.m128_f32[lane],
                        az.m128_f32[lane]);
 
            float r = std::sqrt(r2.m128_f32[lane]);
 
            float dirX = std::asin(a.x / r);
            float dirY = std::atan(a.y / a.z);
            auto polarPos = glm::vec3(dirY, dirX, r);
 
            // Initial float index:
            glm::vec3 xi = glm::max(glm::vec3(0.f), polarScale*(polarPos - polarShift));
 
            // Figure out interpolation parameters.
            glm::vec3 tau = glm::floor(xi);
 
            glm::vec3 t = xi - tau;
            glm::uvec3 i = glm::min(maxIndices, glm::uvec3(tau));
            glm::uvec2 j = glm::min(glm::uvec2(maxIndices), glm::uvec2(i) + glm::uvec2(1));
 
            float val00 = v[(i.y*minorCount + i.x)*sampleCount + i.z];
            float val01 = v[(j.y*minorCount + i.x)*sampleCount + i.z];
            float val0 = (1.f - t.y)*val00 + t.y*val01;
 
            float val10 = v[(i.y*minorCount + j.x)*sampleCount + i.z];
            float val11 = v[(j.y*minorCount + j.x)*sampleCount + i.z];
            float val1 = (1.f - t.y)*val10 + t.y*val11;
 
            float val = (1.f - t.x)*val0 + t.x*val1;
            uint32_t index = z * dataSize.y*dataSize.x + y * dataSize.x + x + lane;
            data[index] = val;
#else
                // Figure out interpolation parameters.
            glm::vec3 tau;// = glm::floor(xi);
            tau.x = tau_i.m128_f32[lane];
            tau.y = tau_j.m128_f32[lane];
            tau.z = tau_k.m128_f32[lane];
 
            glm::vec3 t; //= xi - tau;
            t.x = t_i.m128_f32[lane];
            t.x = t_j.m128_f32[lane];
            t.x = t_k.m128_f32[lane];
 
            glm::uvec3 i;// = glm::min(maxIndices, glm::uvec3(tau));
            i.x = unsigned(i_i.m128_f32[lane]);
            i.y = unsigned(i_j.m128_f32[lane]);
            i.z = unsigned(i_k.m128_f32[lane]);
 
            glm::uvec2 j;// = glm::min(glm::uvec2(maxIndices), glm::uvec2(i) + glm::uvec2(1));
            j.x = unsigned(j_i.m128_f32[lane]);
            j.y = unsigned(j_j.m128_f32[lane]);
 
 
            float val00 = v[ix00.m128i_u32[lane]];
            float val01 = v[ix01.m128i_u32[lane]];
            //float val00 = v[(i.y*minorCount + i.x)*sampleCount + i.z];
            //float val01 = v[(j.y*minorCount + i.x)*sampleCount + i.z];
            float val0 = (1.f - t.y)*val00 + t.y*val01;
 
            float val10 = v[ix10.m128i_u32[lane]];
            float val11 = v[ix11.m128i_u32[lane]];
            //float val10 = v[(i.y*minorCount + j.x)*sampleCount + i.z];
            //float val11 = v[(j.y*minorCount + j.x)*sampleCount + i.z];
            float val1 = (1.f - t.y)*val10 + t.y*val11;
 
            float val = (1.f - t.x)*val0 + t.x*val1;
 
            val = val_.m128_f32[lane];
 
            uint32_t index = z * dataSize.y*dataSize.x + y * dataSize.x + x + lane;
            data[index] = val;
#endif
 
          }
#endif
        }
      }
    }
  }
#pragma optimize( "", on )
 
}