SampleVolumeTask_AVX.cpp

#if 0
#ifdef COGS_EXTENSIONS_AVX
#include <intrin.h>
#include "SampleVolumeTask.h"
 
using namespace Cogs::Core;
 
using glm::uvec3;
using glm::ivec3;
using glm::vec3;
using glm::clamp;
using glm::max;
using glm::min;
using glm::floor;
using glm::ceil;
using glm::inverse;
 
namespace {
  static const float piTwo = 1.5707963267948966f;
  static const float pi = 3.1415926535897931f;
  static const float signBit = -0.f;
  static const float one = 1.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 asin_deg7_C7 = -0.0012624911f;
  static const float asin_deg7_C6 = 0.0066700901f;
  static const float asin_deg7_C5 = -0.0170881256f;
  static const float asin_deg7_C4 = 0.0308918810f;
  static const float asin_deg7_C3 = -0.0501743046f;
  static const float asin_deg7_C2 = 0.0889789874f;
  static const float asin_deg7_C1 = -0.2145988016f;
  static const float asin_deg7_C0 = 1.5707963050f;
 
  __m256 asin_ps(__m256 x)
  {
    __m256 sign = _mm256_set1_ps(signBit);
    __m256 abs_x = _mm256_andnot_ps(sign, x);
 
#if 1
    // Max error < 2e-5
    __m256 C = _mm256_broadcast_ps(&asin_deg3_C);
    __m256 r = _mm256_mul_ps(_mm256_permute_ps(C, 0XFF), abs_x);
    r = _mm256_add_ps(r, _mm256_mul_ps(_mm256_permute_ps(C, 0XAA), abs_x));
    r = _mm256_add_ps(r, _mm256_mul_ps(_mm256_permute_ps(C, 0X55), abs_x));
    r = _mm256_add_ps(r, _mm256_permute_ps(C, 0X00));
#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
 
    __m256 q = _mm256_sub_ps(_mm256_set1_ps(one), abs_x);
#if 0
    q = _mm256_sqrt_ps(q);
#else
    q = _mm256_rcp_ps(_mm256_rsqrt_ps(q));
#endif
 
    r = _mm256_sub_ps(_mm256_set1_ps(piTwo), _mm256_mul_ps(q, r));
 
    // copy sign from x
    return _mm256_or_ps(r, _mm256_and_ps(x, sign));
  }
 
  void quat_times_vec3_ps(__m256& out_x, __m256& out_y, __m256& out_z,
                          const __m128* q,
                          const __m256& v_x, const __m256& v_y, const __m256& v_z)
  {
    __m256 q2 = _mm256_broadcast_ps(q);
    __m256 q_x = _mm256_permute_ps(q2, 0x00);
    __m256 q_y = _mm256_permute_ps(q2, 0x55);
    __m256 q_z = _mm256_permute_ps(q2, 0xAA);
    __m256 q_w = _mm256_permute_ps(q2, 0xFF);
 
    __m256 uv_x = _mm256_sub_ps(_mm256_mul_ps(q_y, v_z), _mm256_mul_ps(v_y, q_z));
    __m256 uv_y = _mm256_sub_ps(_mm256_mul_ps(q_z, v_x), _mm256_mul_ps(v_z, q_x));
    __m256 uv_z = _mm256_sub_ps(_mm256_mul_ps(q_x, v_y), _mm256_mul_ps(v_x, q_y));
    __m256 uuv_x = _mm256_sub_ps(_mm256_mul_ps(q_y, uv_z), _mm256_mul_ps(uv_y, q_z));
    __m256 uuv_y = _mm256_sub_ps(_mm256_mul_ps(q_z, uv_x), _mm256_mul_ps(uv_z, q_x));
    __m256 uuv_z = _mm256_sub_ps(_mm256_mul_ps(q_x, uv_y), _mm256_mul_ps(uv_x, q_y));
    __m256 t_x = _mm256_add_ps(_mm256_mul_ps(q_w, uv_x), uuv_x);
    __m256 t_y = _mm256_add_ps(_mm256_mul_ps(q_w, uv_y), uuv_y);
    __m256 t_z = _mm256_add_ps(_mm256_mul_ps(q_w, uv_z), uuv_z);
    out_x = _mm256_add_ps(v_x, _mm256_add_ps(t_x, t_x));
    out_y = _mm256_add_ps(v_y, _mm256_add_ps(t_y, t_y));
    out_z = _mm256_add_ps(v_z, _mm256_add_ps(t_z, t_z));
  }
 
}
 
 
void EchoSounder::SampleVolumeTask2::runAVX2(TileValue * dstValues,
                                             uint16_t * timeValues,
                                             const EchoSounder::PingInfo* pinf,
                                             const uint32_t upperFansToRemove,
                                             const uint32_t numPings,
                                             const uint32_t numSamples,
                                             const uint32_t gridSizeX,
                                             const uint32_t gridSizeY,
                                             const float depthOffset,
                                             const float depthStep,
                                             const float sampleSpacing,
                                             const float decay,
                                             const std::vector<float>& beamAngleAlongship,
                                             const std::vector<float>& beamAngleAthwartship,
                                             const glm::ivec3& this_minIndex,
                                             const glm::ivec3& this_maxIndex)
{
  const auto time = pinf->time;
  const auto weight = 1.f;// / data.numPings;
  const auto numBeamsMajor = static_cast<uint32_t>(beamAngleAlongship.size());
  const auto numBeamsMinor = static_cast<uint32_t>(beamAngleAthwartship.size());
  const auto s = 1.f / sampleSpacing;
  //const auto width = data.cellWidth;
 
  const vec3 minPolar(beamAngleAthwartship.front(),
                      beamAngleAlongship.front(),
                      depthOffset);
  const vec3 maxPolar(beamAngleAthwartship.back(),
                      beamAngleAlongship.back(),
                      depthOffset + (numSamples - 1)*depthStep);
 
 
  const uvec3 maxTau(max(2u, numBeamsMinor) - 2,
                     max(2u + upperFansToRemove, numBeamsMajor) - 2 - upperFansToRemove,
                     max(2u, numSamples) - 2);
 
  // map minPolar to 0 and maxPolar to N-1.
  const vec3 polarToIndex = vec3(max(2u, numBeamsMinor) - 2,
                                 max(2u, numBeamsMajor) - 2,
                                 max(2u, numSamples) - 2) / (maxPolar - minPolar);
 
  const auto * minorMu = beamAngleAthwartship.data();
  const auto * majorMu = beamAngleAlongship.data();
 
  for (uint32_t p = 0; p < numPings; p++) {
 
    const auto minIndex = this_minIndex;// max(this_minIndex, ivec3(floor(s * pinf[p].boundingBoxTile[0])));
    const auto maxIndex = this_maxIndex;// min(this_maxIndex, ivec3(ceil(s * pinf[p].boundingBoxTile[1])));
    const auto * srcValues = pinf[p].field;
 
    const auto rot = inverse(pinf[p].metaPing.arrayOrientationGlobal);
    const __m128 rot_ = _mm_set_ps(rot.w, rot.z, rot.y, rot.x);
    const auto shift = pinf[p].arrayPositionTile;
 
    const __m128 n_x = _mm_set_ps(
      pinf[p].boundingFrustumNormals[3].x,
      pinf[p].boundingFrustumNormals[2].x,
      pinf[p].boundingFrustumNormals[1].x,
      pinf[p].boundingFrustumNormals[0].x
    );
    for (int k = minIndex.z; k < maxIndex.z; k++) {
 
      float samplePosGrid_z = sampleSpacing*k;
      if (samplePosGrid_z < pinf[p].depthRestriction) continue;
 
      for (int j = minIndex.y; j < maxIndex.y; j++) {
        float samplePosGrid_y = sampleSpacing*j;
 
        const auto l_z = sampleSpacing*k - shift.z;
        const auto l_y = sampleSpacing*j - shift.y;
        const __m128 eq_a = _mm_set_ps(
          pinf[p].boundingFrustumNormals[3].z * l_z + pinf[p].boundingFrustumNormals[3].y*l_y,
          pinf[p].boundingFrustumNormals[2].z * l_z + pinf[p].boundingFrustumNormals[2].y*l_y,
          pinf[p].boundingFrustumNormals[1].z * l_z + pinf[p].boundingFrustumNormals[1].y*l_y,
          pinf[p].boundingFrustumNormals[0].z * l_z + pinf[p].boundingFrustumNormals[0].y*l_y
        );
        const auto l_zy_2 = l_z*l_z + l_y*l_y;
 
        for (int i = minIndex.x; i < maxIndex.x; i += 8) {
          __m256 ii = _mm256_set_ps(static_cast<float>(i + 7),
                                    static_cast<float>(i + 6),
                                    static_cast<float>(i + 5),
                                    static_cast<float>(i + 4),
                                    static_cast<float>(i + 3),
                                    static_cast<float>(i + 2),
                                    static_cast<float>(i + 1),
                                    static_cast<float>(i + 0));
 
          __m256 mask = _mm256_cmp_ps(ii, _mm256_set1_ps(static_cast<float>(maxIndex.x)), _CMP_LE_OQ);
          __m256 l_x = _mm256_sub_ps(_mm256_mul_ps(_mm256_set1_ps(sampleSpacing), ii), _mm256_set1_ps(shift.x));
          __m256 r2_ = _mm256_add_ps(_mm256_set1_ps(l_zy_2), _mm256_mul_ps(l_x, l_x));
 
          // Early exit: check against frustum planes
          __m256 eq_a2 = _mm256_broadcast_ps(&eq_a);  // double copies of eq_a
          __m256 n_x2 = _mm256_broadcast_ps(&n_x);  // double copies of n_x
 
          __m256 mask_0 = _mm256_cmp_ps(_mm256_setzero_ps(), _mm256_add_ps(_mm256_permute_ps(eq_a2, 0x00), _mm256_mul_ps(_mm256_permute_ps(n_x2, 0x00), l_x)), _CMP_LE_OQ);
          __m256 mask_1 = _mm256_cmp_ps(_mm256_setzero_ps(), _mm256_add_ps(_mm256_permute_ps(eq_a2, 0x55), _mm256_mul_ps(_mm256_permute_ps(n_x2, 0x55), l_x)), _CMP_LE_OQ);
          __m256 mask_2 = _mm256_cmp_ps(_mm256_setzero_ps(), _mm256_add_ps(_mm256_permute_ps(eq_a2, 0xAA), _mm256_mul_ps(_mm256_permute_ps(n_x2, 0xAA), l_x)), _CMP_LE_OQ);
          __m256 mask_3 = _mm256_cmp_ps(_mm256_setzero_ps(), _mm256_add_ps(_mm256_permute_ps(eq_a2, 0xFF), _mm256_mul_ps(_mm256_permute_ps(n_x2, 0xFF), l_x)), _CMP_LE_OQ);
          __m256 mask_4 = _mm256_cmp_ps(_mm256_set1_ps(pinf[p].minDepthSquared), r2_, _CMP_LE_OQ);
          __m256 mask_5 = _mm256_cmp_ps(r2_, _mm256_set1_ps(pinf[p].maxDepthSquared), _CMP_LE_OQ);
          mask = _mm256_and_ps(_mm256_and_ps(_mm256_and_ps(mask_0, mask_1),
                                             _mm256_and_ps(mask_2, mask_3)),
                               _mm256_and_ps(_mm256_and_ps(mask_4, mask_5),
                                             mask));
          if (_mm256_movemask_ps(mask) == 0) {
            continue;
          }
 
          __m256 cartesian_x, cartesian_y, cartesian_z;
          quat_times_vec3_ps(cartesian_x, cartesian_y, cartesian_z,
                             &rot_,
                             l_x, _mm256_set1_ps(l_y), _mm256_set1_ps(l_z));
          __m256 rep_r = _mm256_rsqrt_ps(r2_);
          __m256 r_ = _mm256_rcp_ps(rep_r);
 
          __m256 dirX = asin_ps(_mm256_mul_ps(cartesian_x, rep_r)); //std::asin(cartesianPos.x / r);
          __m256 dirY = asin_ps(_mm256_mul_ps(cartesian_y, rep_r)); //std::asin(cartesianPos.y / r);
 
 
                                                                 // calculate xi (source indices with fractional part).
          __m256 xi_x = _mm256_mul_ps(_mm256_set1_ps(polarToIndex.x),
                                      _mm256_sub_ps(dirY, _mm256_set1_ps(minPolar.x)));
          __m256 xi_y = _mm256_mul_ps(_mm256_set1_ps(polarToIndex.y),
                                      _mm256_sub_ps(dirX, _mm256_set1_ps(minPolar.y)));
          __m256 xi_z = _mm256_mul_ps(_mm256_set1_ps(polarToIndex.z),
                                      _mm256_sub_ps(r_, _mm256_set1_ps(minPolar.z)));
 
          __m256 tau_x = _mm256_floor_ps(xi_x);
          __m256 tau_y = _mm256_floor_ps(xi_y);
          __m256 tau_z = _mm256_floor_ps(xi_z);
 
 
          __m256i tau_x_vui = _mm256_cvtps_epi32(tau_x);
          __m256i tau_y_vui = _mm256_cvtps_epi32(tau_y);
          __m256i tau_z_vui = _mm256_cvtps_epi32(tau_z);
          __m256 bx_ = _mm256_sub_ps(xi_x, tau_x);
          __m256 by_ = _mm256_sub_ps(xi_y, tau_y);
 
 
 
          __m256i upper_x = _mm256_set1_epi32(maxTau.x);
          __m256i inrange_x = _mm256_cmpeq_epi32(_mm256_max_epu32(tau_x_vui, upper_x), upper_x);
 
          __m256i upper_y = _mm256_set1_epi32(maxTau.y);
          __m256i inrange_y = _mm256_cmpeq_epi32(_mm256_max_epu32(tau_y_vui, upper_y), upper_y);
 
          __m256i upper_z = _mm256_set1_epi32(maxTau.z);
          __m256i inrange_z = _mm256_cmpeq_epi32(_mm256_max_epu32(tau_z_vui, upper_z), upper_z);
 
          mask = _mm256_and_ps(_mm256_and_ps(mask, _mm256_castsi256_ps(inrange_x)),
                               _mm256_and_ps(_mm256_castsi256_ps(inrange_y), _mm256_castsi256_ps(inrange_z)));
 
          int movemask = _mm256_movemask_ps(mask);
          if (movemask == 0) continue;
 
          for (uint32_t lane = 0; lane < 8; lane++) {
            if (((movemask >> lane) & 1) == 0) continue;
 
            uint32_t ix_x = tau_x_vui.m256i_u32[lane];
            uint32_t ix_y = tau_y_vui.m256i_u32[lane];
            uint32_t ix_z = tau_z_vui.m256i_u32[lane];
 
            // Bi-linear interpolation, nearest nb along depth.
            float bx = bx_.m256_f32[lane];//xi.x - tau.x;
            float by = by_.m256_f32[lane];//xi.y - tau.y;
 
            const auto val00 = srcValues[((ix_y + 0)*numBeamsMinor + (ix_x + 0))*numSamples + ix_z];
            const auto val01 = srcValues[((ix_y + 1)*numBeamsMinor + (ix_x + 0))*numSamples + ix_z];
            const auto val0 = (1.f - by)*PingValue_Decode(val00) + by*PingValue_Decode(val01);
 
            const auto val10 = srcValues[((ix_y + 0)*numBeamsMinor + (ix_x + 1))*numSamples + ix_z];
            const auto val11 = srcValues[((ix_y + 1)*numBeamsMinor + (ix_x + 1))*numSamples + ix_z];
            const auto val1 = (1.f - by)*PingValue_Decode(val10) + by*PingValue_Decode(val11);
 
            //const auto val = 10000000.f;//val11;// (1.f - bx)*val0 + bx*val1;
            const auto val = (1.f - bx)*val0 + bx*val1;
 
            auto dstOffset = (k*gridSizeY + j)*gridSizeX + i + lane;
 
 
            auto oldValue = TileValue_Decode(dstValues[dstOffset]);
            //if (oldValue == 0.0) oldValue = val;
 
            auto age = static_cast<float>(time - timeValues[dstOffset]);
            auto w = std::pow(decay, age);
 
            dstValues[dstOffset] = TileValue_Encode(w*oldValue + (1.f-decay)*val);
            //dstValues[dstOffset] = max(oldValue, val);
            timeValues[dstOffset] = time;
          }
        }
      }
    }
  }
}
#endif
#endif