SwathIsoSurfacesBuildMeshTask.cpp

#include <cassert>
 
#include <glm/glm.hpp>
 
#include "Platform/Instrumentation.h"
#include "Services/Variables.h"
#include "Resources/MeshManager.h"
 
#include "../Extensions/GeometryProcessing/GeometryProcessing.h"
#include "../Extensions/IsoSurfaces/IsoSurfaces.h"
 
#include "../BoundingBox.h"
#include "../BeamUtils.h"
 
#include "../Components/BeamGroupComponent.h"
#include "../Systems/SwathIsoSystem.h"
 
#include "../Tasks/BeamResampleTask.h"
#include "../Tasks/SwathIsoSurfacesBuildMeshTask.h"
#include "../Tasks/DepthResampleTask.h"
 
#include "Foundation/Logging/Logger.h"
#include "Foundation/Platform/Timer.h"
 
namespace {
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("SwathIsoSurfacesBuildMeshTask");
 
  const glm::ivec3 taskSize_Classify = glm::ivec3(32, 16, 16);
  const int   taskSize_PopCnt = 10000;
  const int   taskSize_ExtractVtx = 10000;
  const int   taskSize_ExtractIdx = 10000;
  const int   taskSize_Normals = 10000;
  const int   taskSize_BBox = 10000;
 
  int elapsed_Count = 0;
  double elapsed_init = 0.0;
  double elapsed_move = 0.0;
  double elapsed_Total = 0.0;
  double elapsed_Resample = 0.0;
  double elapsed_Linearize = 0.0;
  double elapsed_ClipVertical = 0.0;
  double elapsed_Analyze = 0.0;
  double elapsed_Extract = 0.0;
  double elapsed_Normals = 0.0;
  double elapsed_SetupMesh = 0.0;
 
  template<typename PContainer, typename TContainer, typename RayDirContainer, typename MetaPingContainer>
  struct VertexEmit
  {
    PContainer& P;
    TContainer& T;
 
    const float * field;
    const float threshold;
    const float separation = 1e1f;
    const float depthOffset;
    const float depthStep;
    const float layer;
    const uint32_t vertexOffset;
    const uint32_t uTextureDomain;
    const uint32_t vTextureDomain;
    const glm::ivec3 Ns;
    const glm::ivec3 maxFieldIndex;
    const RayDirContainer& rayDir;
    const MetaPingContainer& metaPings;
 
    VertexEmit(PContainer& P, TContainer& T,
               const float * field, const float threshold, const float separation, const float depthOffset, const float depthStep, const float layer,
               const uint32_t sampleCount, const uint32_t beamCount, const uint32_t sliceCount,
               const uint32_t vertexOffset, const uint32_t uTextureDomain, const uint32_t vTextureDomain,
               const RayDirContainer& rayDir, const MetaPingContainer& metaPings)
      : P(P), T(T),
      field(field), threshold(threshold), separation(separation), depthOffset(depthOffset), depthStep(depthStep), layer(layer),
      Ns(sampleCount, beamCount, sliceCount),
      maxFieldIndex(glm::max(glm::ivec3(0), glm::ivec3((int)sampleCount - 1, (int)beamCount - 1, (int)sliceCount - 1))),
      vertexOffset(vertexOffset), uTextureDomain(uTextureDomain), vTextureDomain(vTextureDomain),
      rayDir(rayDir), metaPings(metaPings)
    {}
 
    VertexEmit(const VertexEmit&) = default;
 
    void emit(uint32_t index, glm::ivec3 aSample, glm::ivec3 bSample)
    {
      glm::ivec3 I[2] = { aSample, bSample };
      float value[2];
      float linearValue[2];
      float depth[2];
 
      glm::vec3 positions[2];
      for (int m = 0; m < 2; m++) {
        glm::ivec3 Ir = I[m];
        glm::ivec3 Ic = glm::clamp(Ir, glm::ivec3(0), maxFieldIndex);
 
        value[m] = field[(Ic.z*Ns.y + Ic.y)*Ns.x + Ic.x];
        linearValue[m] = value[m] - threshold;
        depth[m] = depthOffset + Ic.x*depthStep;
 
        glm::vec3 p = metaPings[Ic.z].arrayPositionGlobal + depth[m] * (metaPings[Ic.z].vesselOrientationGlobal * rayDir[Ic.y]);
        glm::vec3 pp = p;
 
        if (Ic.x != Ir.x) {
          int signed_one = Ic.x - Ir.x < 0 ? -1 : 1;
          //assert(std::abs(signed_one) == 1);
          p -= separation * signed_one * (metaPings[Ic.z].vesselOrientationGlobal * rayDir[Ic.y]);
        }
 
        if (Ic.y != Ir.y) {
          int signed_one = Ic.y - Ir.y < 0 ? -1 : 1;
          //assert(0 <= Ic.y + signed_one);
          //assert(Ic.y + signed_one <= maxFieldIndex.y);
          glm::vec3 ppp = metaPings[Ic.z].arrayPositionGlobal + depth[m] * (metaPings[Ic.z].vesselOrientationGlobal * rayDir[Ic.y + signed_one]);
          p += separation * glm::normalize(pp - ppp);
        }
 
        if (Ic.z != Ir.z) {
          int signed_one = Ic.z - Ir.z < 0 ? -1 : 1;
          //assert(0 <= Ic.z + signed_one);
          //assert(Ic.z + signed_one <= maxFieldIndex.z);
          glm::vec3 ppp = metaPings[Ic.z + signed_one].arrayPositionGlobal + depth[m] * (metaPings[Ic.z + signed_one].vesselOrientationGlobal * rayDir[Ic.y]);
          p += separation * glm::normalize(pp - ppp);
        }
 
        positions[m] = p;
      }
      float den = (linearValue[1] - linearValue[0]);
      float t = glm::abs(den) < std::numeric_limits<float>::min() ? 0.5f : -linearValue[0] / den;
      auto p = glm::mix(positions[0], positions[1], t);
 
      int vo = vertexOffset + index;
      P[vo] = p;
      //N[vo] = glm::vec3(0.f);
 
      float e[static_cast<uint32_t>(Cogs::Core::EchoSounder::SwathIsoComponent::TextureDomain::Size)];
      e[static_cast<uint32_t>(Cogs::Core::EchoSounder::SwathIsoComponent::TextureDomain::Zero)] = 0.f;
      e[static_cast<uint32_t>(Cogs::Core::EchoSounder::SwathIsoComponent::TextureDomain::One)] = 1.f;
      e[static_cast<uint32_t>(Cogs::Core::EchoSounder::SwathIsoComponent::TextureDomain::PositionX)] = p.x;
      e[static_cast<uint32_t>(Cogs::Core::EchoSounder::SwathIsoComponent::TextureDomain::PositionY)] = p.y;
      e[static_cast<uint32_t>(Cogs::Core::EchoSounder::SwathIsoComponent::TextureDomain::VerticalDepth)] = -p.z;
      e[static_cast<uint32_t>(Cogs::Core::EchoSounder::SwathIsoComponent::TextureDomain::BeamDistance)] = glm::mix(depth[0], depth[1], t);
      e[static_cast<uint32_t>(Cogs::Core::EchoSounder::SwathIsoComponent::TextureDomain::Value)] = static_cast<float>(threshold);
      e[static_cast<uint32_t>(Cogs::Core::EchoSounder::SwathIsoComponent::TextureDomain::LayerNormalized)] = layer;
      e[static_cast<uint32_t>(Cogs::Core::EchoSounder::SwathIsoComponent::TextureDomain::Age)] = 0.f; // glm::mix(time[0], time[1], static_cast<float>(t));
 
      T[vo].x = e[uTextureDomain];
      T[vo].y = e[vTextureDomain];
    }
 
    void operator()(int32_t ixIndex, glm::ivec4* samples, const uint32_t ixN)
    {
      for (uint32_t k = 0; k < ixN; k++) {
        emit(ixIndex + k,
             glm::ivec3(samples[0][0],
                        samples[1][0],
                        samples[2][0]),
             glm::ivec3(samples[0][1 + k],
                        samples[1][1 + k],
                        samples[2][1 + k]));
      }
    }
 
  };
 
 
 
 
 
  template<bool flip>
  struct IndexEmit
  {
    uint32_t vertexOffset;
    uint32_t * indices;
    uint8_t * keepIndex;
    int32_t minZ, maxZ;
 
    IndexEmit(uint32_t vertexOffset,
              uint32_t * indices,
              uint8_t * keepIndex,
              int32_t minZ,
              int32_t maxZ)
      : vertexOffset(vertexOffset), indices(indices), keepIndex(keepIndex), minZ(minZ), maxZ(maxZ)
    {}
 
    IndexEmit(const IndexEmit&) = default;
 
    void operator()(uint32_t ixIndex, glm::ivec3 cell, const uint32_t* ix, const uint32_t ixN)
    {
      for (uint32_t k = 0; k < ixN; k += 3) {
        auto i = ixIndex + k;
 
        keepIndex[i / 3] = (minZ <= cell.z) && (cell.z < maxZ);
        if constexpr (flip == false) {
          indices[i + 0] = ix[k + 0] + vertexOffset;
          indices[i + 1] = ix[k + 1] + vertexOffset;
          indices[i + 2] = ix[k + 2] + vertexOffset;
        }
        else {
          indices[i + 0] = ix[k + 0] + vertexOffset;
          indices[i + 1] = ix[k + 2] + vertexOffset;
          indices[i + 2] = ix[k + 1] + vertexOffset;
        }
      }
    }
  };
 
}
 
Cogs::Core::EchoSounder::SwathIsoSurfacesBuildMeshTask::SwathIsoSurfacesBuildMeshTask(Cogs::Core::Context* context,
                                                                                      Cogs::Core::MeshHandle& mesh,
                                                                                      const uint32_t chunkIndex,
                                                                                      const Cogs::Core::EchoSounder::BeamGroupComponent& groupComp,
                                                                                      const Cogs::Core::EchoSounder::SwathIsoComponent& isoComp,
                                                                                      const Cogs::Core::EchoSounder::SwathIsoSurfacesData& isoData,
                                                                                      const Cogs::Core::EchoSounder::DataSetData& dataData)
: SwathBuildMeshTask(context, mesh, chunkIndex, isoData.pathSpacing, isoData.beamSpacing, groupComp, dataData, isoData.resamplingPositions, isoData.chunks),
  seabedClipOffset(isoData.seabedClipOffset),
  useDecibel(dataData.persistent->config.useDecibel),
  flipOrientation(isoData.flipOrientation),
  depthSpacing(isoData.depthSpacing),
  overflowThreshold(isoData.overflowThreshold),
  minVerticalDepth(isoData.minVerticalDepth),
  maxVerticalDepth(isoData.maxVerticalDepth),
  maxDepthUpsample(context->variables->get("echo.maxDepthUpsample")->getFloat()),
  depthOffset(dataData.persistent->config.depthOffset),
  coordSys(dataData.persistent->config.coordSys),
  depthStep(dataData.persistent->config.depthStep),
  separation(isoComp.capSeparation),
  sampleCount(dataData.persistent->config.sampleCount),
  uTextureDomain(isoComp.uTextureDomain),
  vTextureDomain(isoComp.vTextureDomain),
  uTextureRange(isoComp.uTextureRange),
  vTextureRange(isoComp.vTextureRange),
  thresholds(isoData.thresholds)
{
  Timer timer;
  const auto & dataConf = dataData.persistent->config;
  const auto & buffer = dataData.persistent->buffer;
 
  timer.start();
  depths.resize(beamCount * sliceCount);
  values.resize(sampleCount * beamCount * sliceCount);
  arrayOrientationVessel.resize(sliceCount);
  arrayPositionVessel.resize(sliceCount);
  const auto beamCount_src = dataConf.beamCount;
  for (uint32_t i = 0; i < sliceCount; i++) {
    uint32_t k = (bix_a + i) % dataConf.capacity;
    auto &metaPing = buffer.metaPings[k];
    arrayOrientationVessel[i] = metaPing.arrayOrientationVessel;
    arrayPositionVessel[i] = metaPing.arrayPositionVessel;
    for (uint32_t j = 0; j < beamCount; j++) {
      const auto ix = beams[j];
      depths[beamCount*i + j] = buffer.bottomDepths[beamCount_src*k + ix];
      std::memcpy(values.data() + sampleCount*(beamCount*i + j),
                  buffer.values.data() + sampleCount*(beamCount_src*k + ix),
                  sizeof(float)*sampleCount);
    }
  }
  elapsed_init += timer.elapsedSeconds();
}
 
Cogs::Core::EchoSounder::SwathIsoSurfacesBuildMeshTask::SwathIsoSurfacesBuildMeshTask(const SwathIsoSurfacesBuildMeshTask& other) // TODO should this be a move?
: SwathBuildMeshTask(other),
  seabedClipOffset(other.seabedClipOffset),
  useDecibel(other.useDecibel),
  flipOrientation(other.flipOrientation),
  depthSpacing(other.depthSpacing),
  maxDepthUpsample(other.maxDepthUpsample),
  depthOffset(other.depthOffset),
  coordSys(other.coordSys),
  depthStep(other.depthStep),
  separation(other.separation),
  sampleCount(other.sampleCount),
  overflowThreshold(other.overflowThreshold),
  minVerticalDepth(other.minVerticalDepth),
  maxVerticalDepth(other.maxVerticalDepth),
  uTextureDomain(other.uTextureDomain),
  vTextureDomain(other.vTextureDomain),
  uTextureRange(other.uTextureRange),
  vTextureRange(other.vTextureRange),
  thresholds(other.thresholds)
{
  Timer timer;
  timer.start();
  depths.copy(other.depths);
  values.copy(other.values);
  arrayOrientationVessel.copy(other.arrayOrientationVessel);
  arrayPositionVessel.copy(other.arrayPositionVessel);
  elapsed_move += timer.elapsedSeconds();
}
 
void Cogs::Core::EchoSounder::SwathIsoSurfacesBuildMeshTask::operator()()
{
  Cogs::Timer timerTotal, timer;
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "SwathIsoBuildMesh");
  timerTotal.start();
 
  timer.start();
  std::vector<float> linearizedThresholds;
  linearizeAndClipValues(linearizedThresholds);
  elapsed_Linearize = timer.elapsedSeconds();
 
  if (std::isfinite(minVerticalDepth) || std::isfinite(maxVerticalDepth)) {
    clipVerticalMask();
    elapsed_ClipVertical = timer.elapsedSeconds();
  }
 
 
  std::vector<glm::vec3> beamDir;
  assert(directionX.size() == beamCount);
  assert(directionY.size() == beamCount);
  beamDir.resize(beamCount);
  CalculateBeamDirectionsTask(beamDir.data(),
                              coordSys,
                              directionX.data(), directionY.data(),
                              1, beamCount, metaPings[0].arrayOrientationVessel,
                              0, beamCount)();
 
 
  timer.start();
  depthResample();
  beamResample(beamDir);
  pathResample();
  elapsed_Resample += timer.elapsedSeconds();
 
 
  // Done resampling, build mesh.
 
  const auto Ns = glm::ivec3(sampleCount, beamCount, sliceCount);
  const auto gridA = glm::ivec3(-1, -1, tail ? -1 : 0);
  const auto gridB = glm::max(gridA, Ns + glm::ivec3(1, 1, head ? 1 : 0));
  const auto layers = static_cast<uint32_t>(thresholds.size());
  const auto M = gridB - gridA;
 
  std::vector<int32_t> vertexOffsets;
  std::vector<int32_t> indexOffsets;
  std::vector<int32_t> cellOffsets;
  Cogs::Memory::TypedBuffer<int32_t> cellMap;
  Cogs::Memory::TypedBuffer<uint8_t> activeCellCases;
  Cogs::Memory::TypedBuffer<int32_t> activeCellVertexIndexOffsets;
  Cogs::Memory::TypedBuffer<int32_t> activeCellTriangleIndexOffsets;
  Cogs::Memory::TypedBuffer<int32_t> activeCellIndices;
  Cogs::Memory::TypedBuffer<Cogs::Core::IsoSurfaces::Scratch::ijk_t> activeCellIJK;
  {
    CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "Analyze");
    timer.start();
    analyze(context,
            vertexOffsets, indexOffsets, cellOffsets,
            cellMap,
            activeCellCases, activeCellVertexIndexOffsets, activeCellTriangleIndexOffsets, activeCellIndices, activeCellIJK,
            linearizedThresholds, true, values.data(),
            Ns, gridA, gridB,
            nullptr);
    elapsed_Analyze += timer.elapsedSeconds();
  }
 
  const auto vertexCount = vertexOffsets.back();
  const auto indexCount = indexOffsets.back();
 
  if (0 < vertexCount) {
    auto proxyMesh = context->meshManager->createLocked();
 
    Cogs::Memory::TypedBuffer<uint32_t> indices;
    Cogs::Geometry::BoundingBox bbox;
 
    auto P = proxyMesh->mapPositions(0, vertexCount+1); // TODO: Fix out of bound SSE access (+1)
    auto N = proxyMesh->map<glm::vec3>(VertexDataType::Normals, VertexFormats::Norm3f, 0, vertexCount);
    auto T = proxyMesh->map<glm::vec2>(VertexDataType::TexCoords0, VertexFormats::Tex2f, 0, vertexCount);
 
    indices.resize(indexCount, false);
    Cogs::Memory::TypedBuffer<uint8_t> keepIndex(indices.size());
 
 
    // PASS: Extract vertices and indices
    {
      CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "Extract");
      timer.start();
 
      auto extractGroup = context->taskManager->createGroup();
      for (uint32_t i = 0; i < layers; i++) {
        IsoSurfaces::VertexEmitFunc vertexEmit = VertexEmit<decltype(P), decltype(T), decltype(beamDir), decltype(metaPings)>
          (P, T,
           values.data(), linearizedThresholds[i], 2.f*separation * (layers - i) , depthOffset, depthStep, (i + 0.5f) / layers,
           sampleCount, beamCount, sliceCount,
           vertexOffsets[i], (uint32_t)uTextureDomain, (uint32_t)vTextureDomain,
           beamDir, metaPings);
 
        IsoSurfaces::extractVertices(context, extractGroup,
                                     vertexEmit,
                                     activeCellCases.data() + cellOffsets[i],
                                     activeCellVertexIndexOffsets.data() + cellOffsets[i] + i,
                                     activeCellIndices.data() + cellOffsets[i],
                                     activeCellIJK.data() + cellOffsets[i],
                                     cellOffsets[i + 1] - cellOffsets[i],
                                     gridA, gridB,
                                     nullptr);
 
        Cogs::Core::IsoSurfaces::IndexEmitFunc indexEmit;
        if (flipOrientation == false) {
          indexEmit = IndexEmit<false>(vertexOffsets[i],
                                       indices.data() + indexOffsets[i],
                                       keepIndex.data() + indexOffsets[i] / 3,
                                       (int)sliceA - (tail ? 1 : 0),
                                       (int)sliceB + (head ? 1 : 0));
        }
        else {
          indexEmit = IndexEmit<true>(vertexOffsets[i],
                                      indices.data() + indexOffsets[i],
                                      keepIndex.data() + indexOffsets[i] / 3,
                                      (int)sliceA - (tail ? 1 : 0),
                                      (int)sliceB + (head ? 1 : 0));
        }
 
 
        IsoSurfaces::extractIndices(context,
                                    extractGroup,
                                    indexEmit,
                                    cellMap.data() + i * M.x*M.y*M.z,
                                    activeCellCases.data() + cellOffsets[i],
                                    activeCellVertexIndexOffsets.data() + cellOffsets[i] + i,
                                    activeCellTriangleIndexOffsets.data() + cellOffsets[i] + i,
                                    activeCellIndices.data() + cellOffsets[i],
                                    activeCellIJK.data() + cellOffsets[i],
                                    cellOffsets[i + 1] - cellOffsets[i],
                                    gridA, gridB,
                                    nullptr);
 
      }
      context->taskManager->wait(extractGroup);
      context->taskManager->destroy(extractGroup);
 
      elapsed_Extract += timer.elapsedSeconds();
    }
 
    {
      CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "NormalsBBox");
      timer.start();
 
      auto bboxGroup = context->taskManager->createGroup();
      context->taskManager->enqueueChild(bboxGroup, [&]
      {
        bbox = Cogs::Geometry::makeEmptyBoundingBox<Cogs::Geometry::BoundingBox>();
        includeInBBox(context,
                      bbox,
                      reinterpret_cast<const float*>(P.data), 3,
                      vertexOffsets.back(),
                      taskSize_BBox);
      });
      GeometryProcessing::normalsFromIndexedTriangles(context,
                                                     reinterpret_cast<float*>(N.data), 3,
                                                     reinterpret_cast<const float*>(P.data), 3,
                                                     vertexOffsets.back(),
                                                     indices.data(),
                                                     indexOffsets.back(),
                                                     taskSize_Normals);
      context->taskManager->wait(bboxGroup);
      context->taskManager->destroy(bboxGroup);
 
      elapsed_Normals += timer.elapsedSeconds();
    }
 
    // PASS: Setup mesh
    {
      CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "SetupMesh");
      timer.start();
 
      std::vector<int32_t> prunedIndexOffsets(layers + 1);
 
      // remove helper triangles
      auto d = indices.data();
 
      for (uint32_t l = 0; l < layers; l++) {
        for (int i = indexOffsets[l]; i < indexOffsets[l + 1]; i += 3) {
          if (keepIndex[i / 3]) {
            *d++ = indices[i + 0];
            *d++ = indices[i + 1];
            *d++ = indices[i + 2];
          }
        }
        prunedIndexOffsets[l + 1] = static_cast<int32_t>(d - indices.data());
      }
      indices.resize(prunedIndexOffsets.back());
 
      proxyMesh->setBounds(bbox);
      proxyMesh->setIndexData(indices.data(), indices.size());
      proxyMesh->mapSubMeshes(layers);
      for (uint32_t l = 0; l < layers; l++) {
        auto & subMesh = proxyMesh->getSubMeshes()[l];
        subMesh.startIndex = prunedIndexOffsets[l];
        subMesh.numIndexes = prunedIndexOffsets[l + 1] - prunedIndexOffsets[l];
        subMesh.primitiveType = Cogs::PrimitiveType::TriangleList;
      }
      proxyMesh->setMeshFlag(MeshFlags::ClockwiseWinding);
 
      this->mesh = proxyMesh.getHandle();
 
      elapsed_SetupMesh += timer.elapsedSeconds();
    }
  }
 
  //context->callbacks.resourceLoadCallback(context, 0, -1, 0);
 
  elapsed_Total += timerTotal.elapsedSeconds();
  elapsed_Count++;
 
#if defined(COGS_AUTO_PROFILE)
  if (2.0 < elapsed_Total) {
    double total = elapsed_Total / elapsed_Count;
    double resample = elapsed_Resample / elapsed_Count;
    double linearize = elapsed_Linearize / elapsed_Count;
    double clipVertical = elapsed_ClipVertical / elapsed_Count;
    double analyze = elapsed_Analyze / elapsed_Count;
    double extract = elapsed_Extract / elapsed_Count;
    double normals = elapsed_Normals / elapsed_Count;
    double setupmesh = elapsed_SetupMesh / elapsed_Count;
    LOG_DEBUG(logger, "N=%d TOT=%.1f RS=%.1f L=%.1f CL=%.1f A=%.1f E=%.1f N=%.3f M=%.1f",
              elapsed_Count,
              1000.0*total,
              1000.0*resample,
              1000.0*linearize,
              1000.0*clipVertical,
              1000.0*analyze,
              1000.0*extract,
              1000.0*normals,
              1000.0*setupmesh);
    elapsed_Count = 0;
    elapsed_Total = 0.0;
    elapsed_Resample = 0.0;
    elapsed_Linearize = 0.0;
    elapsed_ClipVertical = 0.0;
    elapsed_Analyze = 0.0;
    elapsed_Extract = 0.0;
    elapsed_Normals = 0.0;
    elapsed_SetupMesh = 0.0;
  }
#endif
 
}
 
 
void Cogs::Core::EchoSounder::SwathIsoSurfacesBuildMeshTask::linearizeAndClipValues(std::vector<float>& linearizedThresholds)
{
  static const float factorA = (float)(log2(10.0) / 10.0);
  static const float scale_dB = 100.0f; // Constant added to dB value to utilize both positive and negative exponents.
 
  float overflowThresholdLin = std::exp2(factorA*(overflowThreshold + scale_dB));
  if (useDecibel) {
 
    for (uint32_t s = 0; s < sliceCount; s++) {
      for (uint32_t b = 0; b < beamCount; b++) {
        auto bottomDepth = depths[s*beamCount + b];
        auto clipDist = bottomDepth - seabedClipOffset;
        auto N = (bottomDepth != 0.f && std::isfinite(seabedClipOffset)) ? std::min(static_cast<uint32_t>(std::max(0.f, std::floor((clipDist - depthOffset) / depthStep))), sampleCount) : sampleCount;
        for (uint32_t i = 0; i < N; i++) {
          auto v = std::exp2(factorA*(values[(s*beamCount + b)*sampleCount + i] + scale_dB));
          if (!std::isfinite(v)) {
            v = v < 0.f ? 0.f : std::numeric_limits<float>::max();
          }
          values[(s*beamCount + b)*sampleCount + i] = v;
        }
        for (uint32_t i = N; i < sampleCount; i++) {
          values[(s*beamCount + b)*sampleCount + i] = 0.f;
        }
        if (std::isfinite(overflowThresholdLin)) {
          for (uint32_t i = 0; i < N; i++) {
            float &v = values[(s*beamCount + b)*sampleCount + i];
            if (v > overflowThresholdLin) {
              v = 0;
            }
          }
        }
      }
    }
 
    linearizedThresholds.resize(thresholds.size());
    for (size_t i = 0; i < thresholds.size(); i++) {
      auto v = std::exp2(factorA*(thresholds[i] + scale_dB));
      if (!std::isfinite(v)) {
        v = v < 0.f ? 0.f : std::numeric_limits<float>::max();
      }
      linearizedThresholds[i] = v;
    }
  }
  else {
 
    for (uint32_t s = 0; s < sliceCount; s++) {
      for (uint32_t b = 0; b < beamCount; b++) {
        auto bottomDepth = depths[s*beamCount + b];
        auto clipDist = bottomDepth - seabedClipOffset;
        auto N = (bottomDepth != 0.f && std::isfinite(seabedClipOffset)) ? std::min(static_cast<uint32_t>(std::max(0.f, std::floor((clipDist - depthOffset) / depthStep))), sampleCount) : sampleCount;
        for (uint32_t i = N; i < sampleCount; i++) {
          values[(s*beamCount)*sampleCount + i] = 0.f;
        }
 
        if (std::isfinite(overflowThresholdLin)) {
          for (uint32_t i = 0; i < N; i++) {
            if (values[i] > overflowThresholdLin) {
              values[i] = 0;
            }
          }
        }
      }
    }
 
    linearizedThresholds = thresholds;
 
  }
}
 
void Cogs::Core::EchoSounder::SwathIsoSurfacesBuildMeshTask::clipVerticalMask()
{
  const float doff = depthOffset;
  const float dstp = depthStep;
  const float maxRange = doff + dstp * sampleCount;
 
 
  float minVerticalDepth1 = std::isfinite(minVerticalDepth) ? std::max(0.f, minVerticalDepth) : 0.f;
  float maxVerticalDepth1 = std::isfinite(maxVerticalDepth) ? std::max(minVerticalDepth, maxVerticalDepth) : 10000.f;
 
  std::vector<glm::vec3> relBeamDir(beamCount);
 
  for (size_t j = 0; j < beamCount; j++) {
    relBeamDir[j] = getBeamDir(coordSys, directionX[j], directionY[j]);
  }
 
  for (size_t k = 0; k < sliceCount; k++) {
    std::vector<glm::vec3> vesselBeamDir(beamCount);
    for (size_t j = 0; j < beamCount; ++j) {
      vesselBeamDir[j] = arrayOrientationVessel[k] * relBeamDir[j];
    }
    clipVertical(values, sampleCount * beamCount * k,
      arrayPositionVessel[k], vesselBeamDir, 0,
      minVerticalDepth1, maxVerticalDepth1,
      depthOffset, depthStep, maxRange, 
      beamCount, sampleCount);
  }
}
 
 
void Cogs::Core::EchoSounder::SwathIsoSurfacesBuildMeshTask::depthResample()
{
  if (depthSpacing == 0.f) return;
 
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "DepthResample");
 
  uint32_t sampleCountResample = std::min(10000u, static_cast<uint32_t>(floor((depthStep * sampleCount) / depthSpacing)));
  floatTmp.resize(sliceCount * beamCount * sampleCountResample);
 
  auto depthResampleGroup = context->taskManager->createGroup();
  for (uint32_t s = 0; s < sliceCount; s++) {
    context->taskManager->enqueueChild(depthResampleGroup, DepthResampleTask(floatTmp.data() + s * beamCount * sampleCountResample,
                                                                             depthSpacing,
                                                                             sampleCountResample,
                                                                             values.data() + s * beamCount * sampleCount,
                                                                             depthStep,
                                                                             sampleCount,
                                                                             0, beamCount));
  }
  context->taskManager->wait(depthResampleGroup);
  context->taskManager->destroy(depthResampleGroup);
 
  sampleCount = sampleCountResample;
  depthStep = depthSpacing;
  values.swap(floatTmp);
}
 
void Cogs::Core::EchoSounder::SwathIsoSurfacesBuildMeshTask::beamResample(std::vector<glm::vec3>& beamDir)
{
  if (beamSpacing == 0.f) return;
 
  std::vector<float> minorAngles(beamCount);
  minorAngles[0] = 0.f;
  for (uint32_t i = 1; i < beamCount; i++) {
    minorAngles[i] = minorAngles[i - 1] + std::acos(glm::dot(beamDir[i - 1], beamDir[i]));
  }
  float clampedStep = std::max(beamSpacing, abs((minorAngles.back() - minorAngles.front()) / (maxBeamUpsample*beamCount)));
 
  std::vector<InterpolationSamplePos> samplesOut;
  SetupBeamFilterUnivariate(samplesOut, minorAngles, clampedStep, beamCount);
 
  uint32_t beamCountNew = static_cast<uint32_t>(samplesOut.size());
 
  std::vector<glm::vec3> beamDirNew(beamCountNew);
  for (uint32_t i = 0; i < beamCountNew; i++) {
    const auto & d0 = beamDir[samplesOut[i].i + 0];
    const auto & d1 = beamDir[samplesOut[i].i + 1];
 
    // Spherical linear interpolation
    const auto G = std::acos(glm::dot(d0, d1));
    const auto t = samplesOut[i].t;
    const auto d = (1.f / std::sin(G))*(std::sin(G*(1.f - t))*d0 + std::sin(G*t)*d1);
 
    beamDirNew[i] = d;
  }
 
  floatTmp.resize(beamCountNew * sliceCount * sampleCount);
  auto interpolateGroup1 = context->taskManager->createGroup();
 
  for (uint32_t s = 0; s < sliceCount; s++) {
    context->taskManager->enqueueChild(interpolateGroup1,
                                       InterpolateBeamDataMinorTask(floatTmp.data() + s* beamCountNew * sampleCount,
                                                                    1, beamCountNew, beamCount, sampleCount,
                                                                    values.data() + s* beamCount * sampleCount,
                                                                    samplesOut.data(), 0, beamCountNew));
  }
  context->taskManager->wait(interpolateGroup1);
  context->taskManager->destroy(interpolateGroup1);
 
  beamDir.swap(beamDirNew);
  values.swap(floatTmp);
  beamCount = beamCountNew;
}
 
 
void Cogs::Core::EchoSounder::SwathIsoSurfacesBuildMeshTask::pathResample()
{
  if (pathSpacing == 0.f) return;
 
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "PathResample");
 
  auto sliceCount_new = static_cast<uint32_t>(resamplingPositions.size());
  decltype(metaPings) newMetaPings(sliceCount_new);
  floatTmp.resize(sampleCount*beamCount*sliceCount_new);
 
  const auto o = resamplingPositions[0].bufferIndex;
  for (uint32_t i = 0; i < sliceCount_new; i++) {
    const auto k = (resamplingPositions[i].bufferIndex - o + bufferCapacity) % bufferCapacity;
    const auto t = resamplingPositions[i].fractional;
    const auto s = (1.f - t);
 
    newMetaPings[i].vesselPositionGlobal = s*metaPings[k].vesselPositionGlobal + t*metaPings[k + 1].vesselPositionGlobal;
    newMetaPings[i].arrayPositionVessel = metaPings[k].arrayPositionVessel; // Fixed during a run.
    newMetaPings[i].arrayPositionGlobal = s*metaPings[k].arrayPositionGlobal + t*metaPings[k + 1].arrayPositionGlobal;
 
    newMetaPings[i].vesselOrientationGlobal = glm::slerp(metaPings[k].vesselOrientationGlobal, metaPings[k + 1].vesselOrientationGlobal, t);
    newMetaPings[i].arrayOrientationVessel = metaPings[k].arrayOrientationVessel;
    newMetaPings[i].arrayOrientationGlobal = newMetaPings[i].vesselOrientationGlobal * newMetaPings[i].arrayOrientationVessel;
 
    for (uint32_t j = 0; j < beamCount; j++) {
      for (uint32_t l = 0; l < sampleCount; l++) {
        floatTmp[(beamCount*i + j)*sampleCount + l] = s*values[(beamCount*k + j)*sampleCount + l] + t*values[(beamCount*(k + 1) + j)*sampleCount + l];
      }
    }
  }
 
  values.swap(floatTmp);
  metaPings.swap(newMetaPings);
  sliceCount = sliceCount_new;
}