DepthResampleTask.cpp

#include <algorithm>
#include "SinglePingIsoSurfacesTasks.h"
#include "DepthResampleTask.h"
#include "Platform/Instrumentation.h"
 
using std::max;
using std::min;
using std::floor;
 
using namespace Cogs::Core::EchoSounder;
 
 
DepthResampleTask::DepthResampleTask(float* valuesOut,
                                     const float depthStepOut,
                                     const int sampleCountOut,
                                     const float* valuesIn,
                                     const float depthStepIn,
                                     const int sampleCountIn,
                                     const int beamBegin,
                                     const int beamEnd)
  :
  valuesOut(valuesOut),
  depthStepOut(depthStepOut),
  sampleCountOut(sampleCountOut),
  valuesIn(valuesIn),
  depthStepIn(depthStepIn),
  sampleCountIn(sampleCountIn),
  beamBegin(beamBegin),
  beamEnd(beamEnd)
{ }
 
 
void DepthResampleTask::operator()()
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "DepthResample");
 
  //const auto depthStepIn = parent->depthStep;
  //const auto depthStepOut = parent->depthStepResample;
  //const auto sampleCountIn = parent->sampleCount;
  //const auto sampleCountOut = parent->sampleCountResample;
 
  const auto inStepOut = depthStepOut / depthStepIn; //depthStepIn / depthStepOut;
 
  const auto Q = std::max(1, sampleCountIn) - 1;
  for (int b = beamBegin; b < beamEnd; b++) {
    const auto * currIn = valuesIn + b*sampleCountIn;
    auto * currOut = valuesOut + b*sampleCountOut;
    int j0 = 0;
    float j1f = inStepOut;
    float val0 = currIn[0];
    for (int i = 0; i < sampleCountOut; i++, j1f += inStepOut) {
      int j1 = min(Q, static_cast<int>(floor(j1f)));
      float acc = val0;
      for (int k = j0 + 1; k + 1 < j1; k++) {
        acc += currIn[k];
      }
      if (j0 != j1) {
        val0 = currIn[j1];
      }
      acc += val0;
      auto res = acc / (max(1, j1 - j0) + 1.f);
      currOut[i] = res;
      j0 = j1;
    }
  }
 
}