OctProviderBuildTileTask.cpp

#include "Services/TaskManager.h"
#include "OctProviderBuildTileTask.h"
#include "../../../Volumetric/Source/Components/OctComponent.h"
#include "../BeamUtils.h"
 
#include <glm/glm.hpp>
 
void Cogs::Core::EchoSounder::OctProviderBuildTileTask::operator()()
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "OctBuild");
  auto & res = persistent->responses[index];
  const auto & N = res->N;
  auto * context = persistent->context;
 
  std::vector<Misc> m;
  setup(m);
 
  Cogs::Memory::TypedBuffer<float> age;
  age.resize(N.x*N.y*N.z, false);
  res->data1.resize(N.x*N.y*N.z, false);
 
  auto group = context->taskManager->createGroup(Cogs::Core::TaskManager::ResourceQueue);
  const auto taskCount = persistent->taskCount[index];
  for (unsigned subTask = 0; subTask < taskCount; subTask++) {
    auto a = (subTask * N.z) / taskCount;
    auto b = std::min(N.z, ((subTask + 1)*N.z) / taskCount);
    context->taskManager->enqueueChild(group,
                                       OctProviderBuildSubTileTask{this,
                                                                   &age,
                                                                   &m,
                                                                   a,
                                                                   b});
  }
  persistent->context->taskManager->wait(group);
 
  res->data0.resize(N.x*N.y*N.z, false);
  calcGradients(res->data0, res->data1, age, N);
}
 
void Cogs::Core::EchoSounder::OctProviderBuildSubTileTask::operator()()
{
  sample();
}
 
 
void Cogs::Core::EchoSounder::OctProviderBuildTileTask::setup(std::vector<Misc>& m)
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "OctBuild_setup");
  auto & res = persistent->responses[index];
 
  // Pull ping data from cache and organize them from most recent to least recent.
  std::vector<SrcPing> pings;
  pings.reserve(res->regionKeys.size());
  for (auto regionKey : res->regionKeys) {
    uint32_t src, ping;
    decodeKey2(src, ping, regionKey);
    auto it = persistent->pingCache.find(encodeKey2(persistent->groupCache[src].dataId, ping));
    assert(it != persistent->pingCache.end());
    assert(it->second->linearized);
    pings.push_back(SrcPing{ it->second.get(), src });
  }
  std::sort(pings.begin(), pings.end(), [](const auto &a, const auto & b) { return a.ping->timestamp > b.ping->timestamp; });
  res->clientData = pings[0].ping->timestamp;
 
  std::vector<Misc> tmp;
  tmp.resize(pings.size());
  for (size_t i = 0; i <tmp.size(); i++) {
    auto & misc = tmp[i];
 
    misc.ping = pings[i].ping;
    const auto & grp = persistent->groupCache[pings[i].src];
 
    const auto minDistance = misc.ping->depthOffset;
    const auto maxDistance = minDistance + misc.ping->sampleCount * misc.ping->depthStep;
 
    glm::vec4 frustum[6];
    getBoundingFrustum(frustum,
                       misc.ping->metaPing.arrayOrientationGlobal,
                       misc.ping->metaPing.arrayPositionGlobal,
                       0, // FIXME - Assume EK/ME/MS for now.
                       grp.minDirectionX, grp.maxDirectionX,
                       grp.minDirectionY, grp.maxDirectionY,
                       minDistance, maxDistance);
 
    for (unsigned k = 0; k < 4; k++) {
      misc.boundingFrustumNormals[k] = glm::vec3(frustum[k]);
    }
    misc.inverseOrientation = glm::inverse(misc.ping->metaPing.arrayOrientationGlobal);
    misc.shift = misc.ping->metaPing.arrayPositionGlobal - res->min;
    misc.minDistanceSquared = minDistance * minDistance;
    misc.maxDistanceSquared = maxDistance * maxDistance;
 
    misc.maxIndices = glm::uvec3(glm::max(1u, grp.minorCount) - 1u,
                                 glm::max(1u, grp.majorCount) - 1u,
                                 glm::max(1u, misc.ping->sampleCount) - 1u);
 
    misc.polarShift = glm::vec3(grp.minDirectionY,
                                grp.minDirectionX,
                                minDistance);
    misc.polarScale = glm::vec3((glm::max(2u, grp.minorCount) - 2u) / (grp.maxDirectionY - grp.minDirectionY),
      (glm::max(2u, grp.majorCount) - 2u) / (grp.maxDirectionX - grp.minDirectionX),
                                (glm::max(2u, misc.ping->sampleCount) - 2u) / (maxDistance - minDistance));
    misc.minorCount = grp.minorCount;
    misc.sampleCount = misc.ping->sampleCount;
    misc.v = misc.ping->linear.data();
  }
 
  // Reduce ping count: TODO do earlier
  // m.clear();
  // unsigned max_pings = 128u;
  // unsigned l_inc = std::max(1u, 1+unsigned((tmp.size()-1)/max_pings));
  // for (unsigned t=0; t < l_inc && m.size() < max_pings; t++){
  //   for (unsigned l=t; l < tmp.size() && m.size() < max_pings; l+=l_inc){
  //     m.push_back(tmp[l]);
  //   }
  // }
 
  m = std::move(tmp);
}
 
void Cogs::Core::EchoSounder::OctProviderBuildSubTileTask::sample()
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "OctBuild_sample");
  auto & res = parent->persistent->responses[parent->index];
  auto & value = res->data1;
 
  const auto & N = res->N;
  const unsigned Np = static_cast<unsigned>(m->size());
 
  auto scale = (res->max - res->min)*glm::vec3(1.f / N.x,
                                               1.f / N.y,
                                               1.f / N.z);
 
  //for (unsigned k = 0; k < N.z; k++) {
  for (size_t k = a; k < b; k++) {
    for (size_t j = 0; j < N.y; j++) {
      for (size_t i = 0; i < N.x; i++) {
        auto p = scale * glm::vec3(i + 0.5f,      // Sample pos with origin in res->min.
                                   j + 0.5f,
                                   k + 0.5f);
 
        float v = 0.f;
        float e = 1.f;
        for (size_t l = 0; l < Np; l++) {
          const auto & misc = (*m)[l];
          const auto q = p - misc.shift;                // Sample pos with origin in beam bundle origin.
          const auto r2 = glm::dot(q, q);
 
          bool in_0 = 0.f <= glm::dot(misc.boundingFrustumNormals[0], q);
          bool in_1 = 0.f <= glm::dot(misc.boundingFrustumNormals[1], q);
          bool in_2 = 0.f <= glm::dot(misc.boundingFrustumNormals[2], q);
          bool in_3 = 0.f <= glm::dot(misc.boundingFrustumNormals[3], q);
          bool in_4 = misc.minDistanceSquared <= r2;
          bool in_5 = r2 <= misc.maxDistanceSquared;
          if (!in_0 || !in_1 || !in_2 || !in_3 || !in_4 || !in_5) {
            continue;
          }
 
          const auto a = misc.inverseOrientation * q;
          const float r = std::sqrt(r2);
 
          const float dirX = std::asin(a.x / r);
          const float dirY = std::asin(a.y / r);
          const auto polarPos = glm::vec3(dirY, dirX, r);                                         // Polar coordinate.
          const auto xi = glm::max(glm::vec3(0.f), misc.polarScale*(polarPos - misc.polarShift)); // Initial float index.
 
                                                                                                  // Figure out interpolation parameters.
          const auto tau = glm::floor(xi);
          const auto t = xi - tau;
          const auto ii = glm::min(misc.maxIndices, glm::uvec3(tau));
          const auto jj = glm::min(glm::uvec2(misc.maxIndices), glm::uvec2(ii) + glm::uvec2(1));
 
          const auto val00 = misc.v[(ii.y*misc.minorCount + ii.x)*misc.sampleCount + ii.z];
          const auto val01 = misc.v[(jj.y*misc.minorCount + ii.x)*misc.sampleCount + ii.z];
          const auto val0 = (1.f - t.y)*val00 + t.y*val01;
 
          const auto val10 = misc.v[(ii.y*misc.minorCount + jj.x)*misc.sampleCount + ii.z];
          const auto val11 = misc.v[(jj.y*misc.minorCount + jj.x)*misc.sampleCount + ii.z];
          const auto val1 = (1.f - t.y)*val10 + t.y*val11;
 
          v = (1.f - t.x)*val0 + t.x*val1;
          e = float(10e-7*((*m)[0].ping->timestamp - misc.ping->timestamp));
          break;
        }
 
        value[(k*N.y + j)*N.x + i] = v;
        (*age)[(k*N.y + j)*N.x + i] = e;
      }
    }
  }
}
 
void Cogs::Core::EchoSounder::OctProviderBuildTileTask::calcGradients(Cogs::Memory::TypedBuffer<glm::vec4>& out, const Cogs::Memory::TypedBuffer<float>& in, const Cogs::Memory::TypedBuffer<float>& age, const glm::uvec3& N)
{
  CpuInstrumentationScope(SCOPE_ECHOSOUNDER, "OctBuild_gradients");
 
  for (unsigned k = 0; k < N.z; k++) {
    for (unsigned j = 0; j < N.y; j++) {
      for (unsigned i = 0; i < N.x; i++) {
 
        unsigned km = std::max(k, 1u) - 1;
        unsigned kp = std::min(k + 1, N.z - 1);
        unsigned jm = std::max(j, 1u) - 1;
        unsigned jp = std::min(j + 1, N.y - 1);
        unsigned im = std::max(i, 1u) - 1;
        unsigned ip = std::min(i + 1, N.x - 1);
 
        out[(k*N.y + j)*N.x + i] = glm::vec4((1.0 / (ip - im))*(in[(k*N.y + j)*N.x + ip] - in[(k*N.y + j)*N.x + im]),
                                             (1.0 / (jp - jm))*(in[(k*N.y + jp)*N.x + i] - in[(k*N.y + jm)*N.x + i]),
                                             (1.0 / (kp - km))*(in[(kp*N.y + j)*N.x + i] - in[(km*N.y + j)*N.x + i]),
                                             age[(k*N.y + j)*N.x + i]);
 
      }
    }
  }
}