SwathSeabedBuildMeshTask.cpp

#include "Resources/MeshManager.h"
 
#include "../Components/BeamGroupComponent.h"
#include "../Components/DataSetComponent.h"
#include "../Components/WindowComponent.h"
#include "../Systems/SwathBottomSystem.h"
 
#include "../BeamUtils.h"
 
#include "../Tasks/SwathSeabedBuildMeshTask.h"
#include "../Tasks/BeamResampleTask.h"
 
#include <glm/gtx/compatibility.hpp>
 
Cogs::Core::EchoSounder::SwathSeabedBuildMeshTask::SwathSeabedBuildMeshTask(Cogs::Core::Context* context,
                                                                            Cogs::Core::MeshHandle& mesh,
                                                                            const uint32_t chunkIndex,
                                                                            const Cogs::Core::EchoSounder::BeamGroupComponent& groupComp,
                                                                            const Cogs::Core::EchoSounder::SwathBottomComponent& swathComp,
                                                                            const Cogs::Core::EchoSounder::SwathBottomData& swathData,
                                                                            const Cogs::Core::EchoSounder::DataSetData& dataData)
: SwathBuildMeshTask(context, mesh, chunkIndex, swathData.pathSpacing, swathData.beamSpacing, groupComp, dataData, swathData.resamplingPositions, swathData.chunks),
  uTexDom(swathComp.uTextureDomain),
  uTexRange(swathComp.uTextureRange),
  vTexDom(swathComp.vTextureDomain),
  vTexRange(swathComp.vTextureRange)
{
  const auto & dataConf = dataData.persistent->config;
  const auto & buffer = dataData.persistent->buffer;
 
  depths.resize(beamCount * sliceCount);
  reflectivities.resize(beamCount * sliceCount);
 
  const auto beamCount_src = dataConf.beamCount;
  for (uint32_t i = 0; i < sliceCount; i++) {
    uint32_t k = (bix_a + i) % dataConf.capacity;
    for (uint32_t j = 0; j < beamCount; j++) {
      const auto ix = beams[j];
      depths[beamCount*i + j] = buffer.bottomDepths[beamCount_src*k + ix];
      reflectivities[beamCount*i + j] = buffer.bottomReflectivities[beamCount_src*k + ix];
    }
  }
}
 
Cogs::Core::EchoSounder::SwathSeabedBuildMeshTask::SwathSeabedBuildMeshTask(const SwathSeabedBuildMeshTask& other)
: SwathBuildMeshTask(other),
  uTexDom(other.uTexDom),
  vTexDom(other.vTexDom),
  uTexRange(other.uTexRange),
  vTexRange(other.vTexRange)
{
  depths.copy(other.depths);
  reflectivities.copy(other.reflectivities);
}
 
void Cogs::Core::EchoSounder::SwathSeabedBuildMeshTask::operator()()
{
  // We use NaN for undefined values. The NaN will creep into expressions using
  // undefined values in any way, rendering the results undefined. Using zero,
  // we would get linear interpolations towards zero with just exactly zero
  // wiped out, which will look like spikes.
  for (uint32_t i = 0; i < sliceCount*beamCount; i++) {
    if (depths[i] == 0.f) depths[i] = std::numeric_limits<float>::quiet_NaN();
  }
 
  if (pathSpacing != 0.f) {
    pathResample();
  }
 
  std::vector<glm::vec3> beamDir(beamCount);
  for (uint32_t i = 0; i < beamCount; i++) {
    beamDir[i] = metaPings[0].arrayOrientationVessel * EchoSounder::getBeamDir(coordSys, directionX[i], directionY[i]);
  }
 
  if (beamSpacing != 0.f) {
    beamResample(beamDir);
  }
 
  auto proxy = context->meshManager->createLocked();
  auto P = proxy->mapPositions(0, sliceCount*beamCount);
  auto N = proxy->map<glm::vec3>(VertexDataType::Normals, VertexFormats::Norm3f, 0, sliceCount*beamCount);
  auto T = proxy->map<glm::vec2>(VertexDataType::TexCoords0, VertexFormats::Tex2f, 0, sliceCount*beamCount);
  auto bbox = Cogs::Geometry::makeEmptyBoundingBox<Cogs::Geometry::BoundingBox>();
  std::vector<uint32_t> indices;
  std::vector<uint32_t> supportIndices; //used to get smooth normals between chunks
 
  calcVertexPositions(bbox, P, T, beamDir);
  calcIndices(indices, supportIndices);
  calcNormalVectors(P, N, indices, supportIndices);
  if (!indices.empty()) {
    proxy->setIndexData(indices.data(), indices.size());
    proxy->primitiveType = Cogs::PrimitiveType::TriangleList;
    proxy->setMeshFlag(Cogs::Core::MeshFlags::ClockwiseWinding);
    proxy->setBounds(bbox);
 
    mesh = proxy.getHandle();
    if (context->callbacks.resourceLoadCallback) {
 
      // HACK: Force redraw of view.
      context->callbacks.resourceLoadCallback(context, 0, -1, 0);
    }
  }
}
 
void Cogs::Core::EchoSounder::SwathSeabedBuildMeshTask::pathResample()
{
  auto sliceCount_new = static_cast<uint32_t>(resamplingPositions.size());
 
  decltype(depths) newDepths(beamCount * sliceCount_new);
  decltype(reflectivities) newReflectivities(beamCount * sliceCount_new);
  decltype(metaPings) newMetaPings(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++) {
      newDepths[beamCount*i + j] = s*depths[beamCount*k + j] + t*depths[beamCount*(k + 1) + j];
      newReflectivities[beamCount*i + j] = s*reflectivities[beamCount*k + j] + t*reflectivities[beamCount*(k + 1) + j];
    }
  }
 
  depths.swap(newDepths);
  reflectivities.swap(newReflectivities);
  metaPings.swap(newMetaPings);
  sliceCount = sliceCount_new;
}
 
void Cogs::Core::EchoSounder::SwathSeabedBuildMeshTask::beamResample(std::vector<glm::vec3>& beamDir)
{
  if (beamSpacing == 0.0) 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;
  }
 
  Cogs::Memory::TypedBuffer<float> depths_new(sliceCount*beamCountNew);
  Cogs::Memory::TypedBuffer<float> reflectivities_new(sliceCount*beamCountNew);
  for (uint32_t j = 0; j < sliceCount; j++) {
    for (uint32_t i = 0; i < beamCountNew; i++) {
      // Convert to vertical depth before interpolation
      const auto & s = samplesOut[i];
      const auto depthA = depths[j*beamCount + s.i] * beamDir[s.i].z;
      const auto depthB = depths[j*beamCount + s.i + 1] * beamDir[s.i + 1].z;
      const auto depth = ((1.f - s.t)*depthA + s.t*depthB) / beamDirNew[i].z;
      depths_new[j*beamCountNew + i] = depth;
      const auto reflA = reflectivities[j*beamCount + s.i];
      const auto reflB = reflectivities[j*beamCount + s.i + 1];
      reflectivities_new[j*beamCountNew + i] = (1.f - s.t)*reflA + s.t*reflB;
    }
  }
 
  beamDir.swap(beamDirNew);
  depths.swap(depths_new);
  reflectivities.swap(reflectivities_new);
  beamCount = beamCountNew;
}
 
template<typename PType, typename TType, typename DType>
void Cogs::Core::EchoSounder::SwathSeabedBuildMeshTask::calcVertexPositions(Cogs::Geometry::BoundingBox& bbox, PType& P, TType& T, DType& beamDir)
{
  const glm::vec2 uReMap(uTexRange.x, 1.f / glm::max(std::numeric_limits<float>::min(), uTexRange.y - uTexRange.x));
  const glm::vec2 vReMap(vTexRange.x, 1.f / glm::max(std::numeric_limits<float>::min(), vTexRange.y - vTexRange.x));
  for (uint32_t s = 0; s < sliceCount; s++) {
    for (uint32_t b = 0; b < beamCount; b++) {
      const auto o = s*beamCount + b;
      const auto d = depths[o];
      const auto p = metaPings[s].arrayPositionGlobal + d * (metaPings[s].vesselOrientationGlobal * beamDir[b]);
 
      if (std::isfinite(d)) {
        bbox += p;
      }
 
      P[o] = p;
 
      float e[static_cast<int32_t>(EchoSounder::SwathBottomComponent::TextureDomain::Size)];
      e[static_cast<int32_t>(EchoSounder::SwathBottomComponent::TextureDomain::Zero)] = 0.f;
      e[static_cast<int32_t>(EchoSounder::SwathBottomComponent::TextureDomain::One)] = 1.f;
      e[static_cast<int32_t>(EchoSounder::SwathBottomComponent::TextureDomain::PositionX)] = p.x;
      e[static_cast<int32_t>(EchoSounder::SwathBottomComponent::TextureDomain::PositionY)] = p.y;
      e[static_cast<int32_t>(EchoSounder::SwathBottomComponent::TextureDomain::VerticalDepth)] = -p.z;
      e[static_cast<int32_t>(EchoSounder::SwathBottomComponent::TextureDomain::BeamDistance)] = d;
      e[static_cast<int32_t>(EchoSounder::SwathBottomComponent::TextureDomain::Reflectivity)] = reflectivities[o];
      float tu = e[static_cast<int32_t>(uTexDom)];
      float tv = e[static_cast<int32_t>(vTexDom)];
      T[o].x = uReMap.y*(tu - uReMap.x);
      T[o].y = vReMap.y*(tv - vReMap.x);
    }
  }
}
 
template<typename PType, typename NType, typename IType>
void Cogs::Core::EchoSounder::SwathSeabedBuildMeshTask::calcNormalVectors(PType& P, NType& N, IType& I1, IType& I2) {
  for (uint32_t i = 0; i < sliceCount*beamCount; i++) {
    N[i] = glm::vec3(0.0);
  }
 
  calcNormalVectors(P, N, I1);
  calcNormalVectors(P, N, I2);
 
  for (uint32_t i = 0; i < sliceCount*beamCount; i++) {
    N[i] = glm::normalize(N[i]);
    if (!glm::all(glm::isfinite(N[i]))) {
      N[i] = glm::vec3(0, 0, 1);
    }
  }
}
 
template<typename PType, typename NType, typename IType>
void Cogs::Core::EchoSounder::SwathSeabedBuildMeshTask::calcNormalVectors(PType& P, NType& N, IType& I)
{    
  for (uint32_t i = 0; i < I.size(); ) {
    auto index0 = I[i++];
    auto index1 = I[i++];
    auto index2 = I[i++];
    auto p0 = P[index0];
    auto p1 = P[index1];
    auto p2 = P[index2];
    auto n = glm::cross(p2 - p0, p1 - p0);
    N[index0] += n;
    N[index1] += n;
    N[index2] += n;
  }
}
 
void Cogs::Core::EchoSounder::SwathSeabedBuildMeshTask::calcIndices(std::vector<uint32_t>& indices,
                                                                    std::vector<uint32_t>& supportIndices)
{
  indices.clear();
  indices.reserve(6 * sliceCount*beamCount);
  supportIndices.clear();
  supportIndices.reserve(6 * beamCount *(sliceA + (sliceCount-1 - sliceB))); //contains indices defining triangles in skirt of chunk [0, sliceA] and [sliceB, sliceCount-1]
    
  for (uint32_t s = 0; s + 1 < sliceCount; ++s) {
    for (uint32_t b = 0; b + 1 < beamCount; b++) {
      auto c00 = (s + 0)*beamCount + (b + 0);
      auto c01 = (s + 0)*beamCount + (b + 1);
      auto c10 = (s + 1)*beamCount + (b + 0);
      auto c11 = (s + 1)*beamCount + (b + 1);
 
      uint32_t CellIndices[2][3] = { { (uint32_t)c00, (uint32_t)c01, (uint32_t)c10 },{ (uint32_t)c11, (uint32_t)c10, (uint32_t)c01 } };
      if (!isfinite(depths[c01]) || !isfinite(depths[c10])) {
        CellIndices[0][0] = c01;
        CellIndices[0][1] = c11;
        CellIndices[0][2] = c00;
        CellIndices[1][0] = c10;
        CellIndices[1][1] = c00;
        CellIndices[1][2] = c11;
      }
 
      for (int t = 0; t < 2; t++) {
        if (!isfinite(depths[CellIndices[t][0]]) ||
            !isfinite(depths[CellIndices[t][1]]) ||
            !isfinite(depths[CellIndices[t][2]]))
        {
          continue;
        }
        auto& indicesList = (s < sliceA || s >= sliceB) ? supportIndices : indices;
 
        indicesList.push_back(CellIndices[t][0]);
        indicesList.push_back(CellIndices[t][1]);
        indicesList.push_back(CellIndices[t][2]);
      }
    }
  }
}