SampleVolumeTask.cpp

#if 0
#include <emmintrin.h>  // SSE2
#include "SampleVolumeTask.h"
#include "RemapTileTask.h"
#include "../Utils.h"
#include "Platform/Timer.h"
 
#include "Context.h"
#include "Services/Features.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 atan_deg5_C5 = 8.7292946518897740e-02f;
  static const float atan_deg5_C3 = -3.0189478312144946e-01f;
  static const float atan_deg5_C1 = 1.0f;
 
  static const float atan_deg7_C7 = -4.2330209451053591e-02f;
  static const float atan_deg7_C5 = +1.5342994884206673e-01f;
  static const float atan_deg7_C3 = -3.2570157599356531e-01f;
  static const float atan_deg7_C1 = 1.0f;
  static const __m128 atan_deg7_C = _mm_set_ps(atan_deg7_C7, atan_deg7_C5, atan_deg7_C3, atan_deg7_C1);
 
  static const float atan_deg9_C9 = 0.0208351f;
  static const float atan_deg9_C7 = -0.0851330f;
  static const float atan_deg9_C5 = 0.1801410f;
  static const float atan_deg9_C3 = -0.3302995f;
  static const float atan_deg9_C1 = 0.9998660f;
 
 
 
  inline bool insideRange(const vec3& x, const vec3& a, const vec3& b)
  {
    return
      (a.x <= x.x) && (x.x <= b.x) &&
      (a.y <= x.y) && (x.y <= b.y) &&
      (a.z <= x.z) && (x.z <= b.z);
  }
 
  inline void vec3_cross_vec3_ps(__m128& c_x, __m128& c_y, __m128& c_z,
                                 const __m128& a_x, const __m128& a_y, const __m128& a_z,
                                 const __m128& b_x, const __m128& b_y, const __m128& b_z)
  {
    c_x = _mm_sub_ps(_mm_mul_ps(a_y, b_z), _mm_mul_ps(b_y, a_z));
    c_y = _mm_sub_ps(_mm_mul_ps(a_z, b_x), _mm_mul_ps(b_z, a_x));
    c_z = _mm_sub_ps(_mm_mul_ps(a_x, b_y), _mm_mul_ps(b_x, a_y));
  }
 
 
 
 
 
  __m128 atan2_ps(__m128 y, __m128 x)
  {
#if 0
    // Reference
    __m128 rv;
    for (int i = 0; i < 4; i++) {
      rv.m128_f32[i] = std::atan2(y.m128_f32[i], x.m128_f32[i]);
    }
    return rv;
#endif
 
    __m128 sign = _mm_load_ps1(&signBit);
    __m128 abs_x = _mm_andnot_ps(sign, x);
    __m128 abs_y = _mm_andnot_ps(sign, y);
    __m128 pq = _mm_cmplt_ps(abs_y, abs_x);
 
    __m128 num = _mm_or_ps(_mm_and_ps(pq, y), _mm_andnot_ps(pq, x));
    __m128 den = _mm_or_ps(_mm_and_ps(pq, x), _mm_andnot_ps(pq, y));
    __m128 t = _mm_div_ps(num, den);
 
    __m128 t_t = _mm_mul_ps(t, t);
 
#if 0
    // Degree 5 polynomial approximation, max error < 1.4e-3
    __m128 r = _mm_mul_ps(_mm_load_ps1(&atan_deg5_C5), t_t);
    r = _mm_mul_ps(_mm_add_ps(r, _mm_load_ps1(&atan_deg5_C3)), t_t);
    r = _mm_mul_ps(_mm_add_ps(r, _mm_load_ps1(&atan_deg5_C1)), t);
#elif 1
    // Degree 7 polynomial approximation, max error < 1.5e-4
    __m128 C = _mm_load_ps((float*)(&atan_deg7_C));
    __m128 r = _mm_mul_ps(_mm_shuffle_ps(C, C, _MM_SHUFFLE(3, 3, 3, 3)), t_t);
    r = _mm_mul_ps(_mm_add_ps(r, _mm_shuffle_ps(C, C, _MM_SHUFFLE(2, 2, 2, 2))), t_t);
    r = _mm_mul_ps(_mm_add_ps(r, _mm_shuffle_ps(C, C, _MM_SHUFFLE(1, 1, 1, 1))), t_t);
    r = _mm_add_ps(_mm_mul_ps(r, t), t);
#else
    // Degree 9 polynomial approximation, max error < 1e-5
    __m128 r = _mm_mul_ps(_mm_load_ps1(&atan_deg9_C9), t_t);
    r = _mm_mul_ps(_mm_add_ps(r, _mm_load_ps1(&atan_deg9_C7)), t_t);
    r = _mm_mul_ps(_mm_add_ps(r, _mm_load_ps1(&atan_deg9_C5)), t_t);
    r = _mm_mul_ps(_mm_add_ps(r, _mm_load_ps1(&atan_deg9_C3)), t_t);
    r = _mm_mul_ps(_mm_add_ps(r, _mm_load_ps1(&atan_deg9_C1)), t);
#endif
 
    r = _mm_xor_ps(r, _mm_andnot_ps(pq, sign));
 
    __m128 t2 = _mm_andnot_ps(_mm_cmple_ps(_mm_setzero_ps(), x), _mm_set1_ps(pi));     // t2    = 0 <= x ? 0  : pi
    __m128 t3 = _mm_or_ps(_mm_and_ps(pq, t2), _mm_andnot_ps(pq, _mm_set1_ps(piTwo)));  // t3    =   pq   ? t2 : pi/2
    __m128 shift = _mm_xor_ps(t3, _mm_and_ps(y, sign));                               // shift = 0 <= y ? t3 : - t3
 
    return _mm_add_ps(r, shift);
  }
 
}
 
 
 
 
 
void EchoSounder::SampleVolumeTask2::operator()()
{
  auto timer = Timer::startNew();
 
#ifdef COGS_EXTENSIONS_AVX
  if (data.context->features->supported(CPUFeature::AVX2)) {
    runAVX2(field,
            times,
            pinf,
            data.upperFansToRemove,
            numPings,
            data.numSamples,
            data.samplesInTile, data.samplesInTile,
            data.depthOffset, data.depthStep, data.sampleSpacing, data.decay,
            data.beamAngleAlongship, data.beamAngleAthwartship,
            minIndex, maxIndex);
  } else
#endif
  if (data.context->features->supported(CPUFeature::SSE41)) {
    runSSE4_1(field,
              times,
              pinf,
              data.upperFansToRemove,
              numPings,
              data.numSamples,
              data.samplesInTile, data.samplesInTile,
              data.depthOffset, data.depthStep, data.sampleSpacing, data.decay,
              data.beamAngleAlongship, data.beamAngleAthwartship,
              minIndex, maxIndex);
  }
  else {
    runVanilla(field,
               times,
               pinf,
               data.upperFansToRemove,
               numPings,
               data.numSamples,
               data.samplesInTile, data.samplesInTile,
               data.depthOffset, data.depthStep, data.sampleSpacing, data.decay,
               data.beamAngleAlongship, data.beamAngleAthwartship,
               minIndex, maxIndex);
  }
 
  if (elapsed_us != nullptr) {
    elapsed_us->fetch_add(timer.elapsedMicroseconds());
  }
}
#endif