BeamResampleTask.cpp

#include <algorithm>
#include <glm/gtc/constants.hpp>
#include <glm/gtc/quaternion.hpp>
 
#include "BeamResampleTask.h"
#include "../BeamUtils.h"
#include "Platform/Instrumentation.h"
 
using namespace Cogs::Core::EchoSounder;
 
 
void Cogs::Core::EchoSounder::SetupBeamFilterUnivariate(std::vector<InterpolationSamplePos>& samplesOut,
                                                        const std::vector<float>& angles,
                                                        const float angularStep,
                                                        const uint32_t N)
{
  samplesOut.clear();
  if (N < 2) return;
 
  const float a0 = angles.front();
  const float a1 = angles.back();
  const auto sampleCountOut = static_cast<uint32_t>(std::max(2.f, std::abs(a1 - a0) / angularStep));
  samplesOut.resize(sampleCountOut);
 
  uint32_t ix = 0;
  for (uint32_t i = 0; i < sampleCountOut; i++) {
    const float t = i / (sampleCountOut - 1.f);
    const float a = (1.f - t)*a0 + t*a1;
 
    while ((ix + 1u < N) && (angles[ix + 1] < a)) ix++;
    samplesOut[i].i = ix;
 
    const auto delta = angles[ix + 1] - angles[ix];
    samplesOut[i].t = delta < std::numeric_limits<float>::epsilon() ? 0.5f : (a - angles[ix]) / delta;
  }
}
 
 
void Cogs::Core::EchoSounder::SetupBeamFilter(std::vector<InterpolationSamplePos>& iSamplesOut,
                                              std::vector<InterpolationSamplePos>& jSamplesOut,
                                              const std::vector<glm::vec3>& rayDir,
                                              const float angularStep,
                                              const float maxAngularUpsample,
                                              const uint32_t minorCount,
                                              const uint32_t majorCount)
  //const int yIndexLo, const int yIndexHi,
  //const int xIndexLo, const int xIndexHi)
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "SetupBeamFilter");
 
  std::vector<float> minorAngles(minorCount);
  minorAngles[0] = 0.f;
  for (uint32_t i = 1; i < minorCount; i++) {
    minorAngles[i] = minorAngles[i - 1] + std::acos(glm::dot(rayDir[(majorCount / 2)*minorCount + i - 1], rayDir[(majorCount / 2)*minorCount + i]));
  }
 
  std::vector<float> majorAngles(majorCount);
  majorAngles[0] = 0.f;
  for (uint32_t j = 1; j < majorCount; j++) {
    majorAngles[j] = majorAngles[j - 1] + std::acos(glm::dot(rayDir[(j - 1)*minorCount + minorCount / 2], rayDir[j*minorCount + minorCount / 2]));
  }
 
  float clampedStepI = std::max(angularStep, abs((minorAngles.back() - minorAngles.front()) / (maxAngularUpsample*minorCount)));
  float clampedStepJ = std::max(angularStep, abs((majorAngles.back() - majorAngles.front()) / (maxAngularUpsample*majorCount)));
 
  SetupBeamFilterUnivariate(iSamplesOut, minorAngles, clampedStepI, minorCount);
  SetupBeamFilterUnivariate(jSamplesOut, majorAngles, clampedStepJ, majorCount);
}
 
CalculateBeamDirectionsTask::CalculateBeamDirectionsTask(glm::vec3* directionOut,
                                                         const uint32_t coordSys,
                                                         const float* directionX,
                                                         const float* directionY,
                                                         const int beamMajorCount,
                                                         const int beamMinorCount,
                                                         const glm::quat arrayOrientationVessel,
                                                         const int beamBegin,
                                                         const int beamEnd)
  :
  directionOut(directionOut),
  coordSys(coordSys),
  directionX(directionX),
  directionY(directionY),
  beamMajorCount(beamMajorCount),
  beamMinorCount(beamMinorCount),
  arrayOrientationVessel(arrayOrientationVessel),
  beamBegin(beamBegin),
  beamEnd(beamEnd)
{}
 
void CalculateBeamDirectionsTask::operator()()
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "IntBeamDir");
 
  int ja = beamBegin / beamMinorCount;
  int ia = beamBegin % beamMinorCount;
  int jb = beamEnd / beamMinorCount;
  int ib = beamEnd % beamMinorCount;
  for (int j = ja; j <= jb; j++) {
    int la = (j == ja) ? ia : 0;
    int lb = (j == jb) ? ib : beamMinorCount;
 
    for (int i = la; i < lb; i++) {
      const auto ixo = j*beamMinorCount + i;
 
      directionOut[ixo] = arrayOrientationVessel * getBeamDir(coordSys,
                                                              directionX[j*beamMinorCount + i],
                                                              directionY[j*beamMinorCount + i]);
    }
  }
}
  
InterpolateBeamDataMinorTask::InterpolateBeamDataMinorTask(float* valuesOut,
                                                           const int beamMajorCount,
                                                           const int beamMinorCount,
                                                           const int modulo,
                                                           const int sampleCount,
                                                           const float* valuesIn,
                                                           const InterpolationSamplePos* minorSamples,
                                                           const int beamBegin,
                                                           const int beamEnd)
  :
  valuesOut(valuesOut),
  beamMajorCount(beamMajorCount),
  beamMinorCount(beamMinorCount),
  modulo(modulo),
  sampleCount(sampleCount),
  valuesIn(valuesIn),
  minorSamples(minorSamples),
  beamBegin(beamBegin),
  beamEnd(beamEnd)
{}
 
 
void InterpolateBeamDataMinorTask::operator()()
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "IntValMin");
 
  int ja = beamBegin / beamMinorCount;
  int ia = beamBegin % beamMinorCount;
  int jb = beamEnd / beamMinorCount;
  int ib = beamEnd % beamMinorCount;
  for (int j = ja; j <= jb; j++) {
    int la = (j == ja) ? ia : 0;
    int lb = (j == jb) ? ib : beamMinorCount;
 
    for (int i = la; i < lb; i++) {
      const auto  s = minorSamples[i];
      const float* a = valuesIn + (j*modulo + s.i)*sampleCount;
      const float* b = valuesIn + (j*modulo + s.i + 1)*sampleCount;
      float* d = valuesOut + (j*beamMinorCount + i)*sampleCount;
 
      // this one screams for SIMD'ing.
      for (int k = 0; k < sampleCount; k++) {
        *d++ = (1.f - s.t)*(*a++) + s.t*(*b++);
      }
 
    }
  }
}
 
 
InterpolateBeamDataMajorTask::InterpolateBeamDataMajorTask(float* valuesOut,
                                                           const int beamMajorCount,
                                                           const int beamMinorCount,
                                                           const int modulo,
                                                           const int sampleCount,
                                                           const float* valuesIn,
                                                           const InterpolationSamplePos* majorSamples,
                                                           const int beamBegin,
                                                           const int beamEnd)
  :
  valuesOut(valuesOut),
  beamMajorCount(beamMajorCount),
  beamMinorCount(beamMinorCount),
  modulo(modulo),
  sampleCount(sampleCount),
  valuesIn(valuesIn),
  majorSamples(majorSamples),
  beamBegin(beamBegin),
  beamEnd(beamEnd)
{}
 
 
void InterpolateBeamDataMajorTask::operator()()
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "IntValMaj");
 
  int ja = beamBegin / beamMinorCount;
  int ia = beamBegin % beamMinorCount;
  int jb = beamEnd / beamMinorCount;
  int ib = beamEnd % beamMinorCount;
  for (int j = ja; j <= jb; j++) {
    int la = (j == ja) ? ia : 0;
    int lb = (j == jb) ? ib : beamMinorCount;
 
    for (int i = la; i < lb; i++) {
      const auto  s = majorSamples[j];
      const float* a = valuesIn + ((s.i)*modulo + i)*sampleCount;
      const float* b = valuesIn + ((s.i + 1)*modulo + i)*sampleCount;
      float* d = valuesOut + (j*beamMinorCount + i)*sampleCount;
 
      // this one screams for SIMD'ing.
      for (int k = 0; k < sampleCount; k++) {
        *d++ = (1.f - s.t)*(*a++) + s.t*(*b++);
      }
 
    }
  }
}