PotreeSubtree.cpp

#include "Engine.h"
 
#include "PotreeSystem.h"
 
#include "Foundation/Logging/Logger.h"
#include "Foundation/Platform/IO.h"
 
namespace {
  using namespace Cogs::Core;
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("Potree");
 
  PotreeCell* createCell(PotreeData* poData, PotreeSubtree* subtree)
  {
    auto * cell = poData->cellStore.create();
    cell->owningSubtree = subtree;
    subtree->cells.push(cell);
    return cell;
  }
 
  void parseHrcFile1(PotreeData* poData, PotreeSubtree* subtree, uint32_t instanceId, uint32_t subtreeId, const std::string& url, Cogs::FileContents* data)
  {
    size_t n = data->size / 5;  // number of regular nodes in .hrc file.
 
    const glm::vec3& tbMinCenter = poData->octtreeFrame.tightBBoxMin;
    const glm::vec3& tbMaxCenter = poData->octtreeFrame.tightBBoxMax;
    const glm::vec3& shift = poData->octtreeFrame.toBBoxShift;
    const glm::vec3& scale = poData->octtreeFrame.fullBBoxSize;
 
    List<PotreeCell, offsetof(PotreeCell, tmpNode)> curr;
    List<PotreeCell, offsetof(PotreeCell, tmpNode)> next;
 
    auto* root = createCell(poData, subtree);
    curr.push(root);
    if (subtree->containingCell == nullptr) {
      assert(poData->root == nullptr);
      poData->root = root;
    }
    root->i = 0;
    root->j = 0;
    root->k = 0;
    root->l = 0;
    root->scaleFactor = subtree->containingCell ? subtree->containingCell->scaleFactor : 1;
 
    for (size_t i = 0; i < n; i++) {
 
      // We're done with a layer, start on next.
      if (curr.empty()) {
        curr.swap(next);
      }
      auto* cell = curr.shift();
      //cell->owningSubtree = subtree;
 
      // Calc temporarily bounding box, first by calculating extent of
      // octree cell, and then clamping it to the tight bounding box
      // of overall model.
      // FIXME: Do this with proper rounding.
 
      auto cellSizeAtLevel = std::exp2f(-float(subtree->l + cell->l));
      auto pos = cellSizeAtLevel * glm::vec3(float((subtree->i << cell->l) + cell->i),
                                             float((subtree->j << cell->l) + cell->j),
                                             float((subtree->k << cell->l) + cell->k));
 
      cell->tbmin = glm::max(tbMinCenter, shift + scale * pos);
      cell->tbmax = glm::min(tbMaxCenter, shift + scale * (pos + glm::vec3(cellSizeAtLevel)));
 
      cell->hierarchyNumPoints = ((data->ptr[5 * i + 1] << 0) |
                                  (data->ptr[5 * i + 2] << 8) |
                                  (data->ptr[5 * i + 3] << 16) |
                                  (data->ptr[5 * i + 4] << 24));
 
      if (cell->l < poData->hierarchyStepSize) {
 
        // Cell is a regular cell. Set up links for children.
 
        cell->kind = PotreeCell::Kind::RegularNode;
 
        // Generate child cells.
        auto mask = data->ptr[5 * i + 0];
        for (unsigned b = 0; b < 8; b++) {
          cell->children[b] = nullptr;
          if ((mask >> b) & 1) {
            auto* child = createCell(poData, subtree);
            next.push(child);
 
            child->i = (cell->i << 1) | ((b >> 2) & 1);
            child->j = (cell->j << 1) | ((b >> 1) & 1);
            child->k = (cell->k << 1) | ((b >> 0) & 1);
            child->l = cell->l + 1;
            child->prefix = cell->prefix | (b << (3 * cell->l));
 
            cell->children[b] = child;
          }
        }
      }
      else if (cell->l == poData->hierarchyStepSize) {
        cell->kind = PotreeCell::Kind::SubtreeLink;
        cell->linkedSubtree = nullptr;
      }
      else {
        LOG_ERROR(logger, "[instance=%u,tree=%u] Error in hierarchy file: %s", instanceId, subtreeId, url.c_str());
        poData->state = PotreeState::Error;
        subtree->state = PotreeSubtree::State::Failed;
        if (subtree->containingCell) subtree->containingCell->state = PotreeCell::State::Failed;
        return;
      }
    }
    assert(curr.empty());
    assert(next.empty());
  }
 
  uint32_t getUint32LE(const uint8_t* ptr)
  {
    uint32_t rv = ptr[0];
    for (size_t i = 1; i < 4; i++) { rv = rv | (static_cast<uint32_t>(ptr[i]) << (8 * i)); }
    return rv;
  }
 
  uint64_t getUint64LE(const uint8_t* ptr)
  {
    uint64_t rv = ptr[0];
    for (size_t i = 1; i < 8; i++) { rv = rv | (static_cast<uint64_t>(ptr[i]) << (8 * i)); }
    return rv;
  }
 
  void parseHrcFile2(PotreeData* poData, PotreeSubtree* subtree, uint32_t instanceId, uint32_t subtreeId, const std::string& url, Cogs::FileContents* data)
  {
    size_t bytesPerNode = 22;
    size_t numNodes = data->size / bytesPerNode;
    if (numNodes * bytesPerNode != data->size) {
      LOG_ERROR(logger, "[instance=%u,tree=%u] Hierarchy file part size (%zu) not a multiple of node size (22): %s",
                instanceId, subtreeId, data->size, url.c_str());
      poData->state = PotreeState::Error;
      subtree->state = PotreeSubtree::State::Failed;
      if (subtree->containingCell) subtree->containingCell->state = PotreeCell::State::Failed;
      return;
    }
 
    const glm::vec3& tbMinCenter = poData->octtreeFrame.tightBBoxMin;
    const glm::vec3& tbMaxCenter = poData->octtreeFrame.tightBBoxMax;
    const glm::vec3& shift = poData->octtreeFrame.toBBoxShift;
    const glm::vec3& scale = poData->octtreeFrame.fullBBoxSize;
 
    std::vector<PotreeCell*> cells(numNodes);
    for (auto& ptr : cells) { ptr = createCell(poData, subtree); }
    if (subtree->containingCell == nullptr) {
      assert(poData->root == nullptr);
      poData->root = cells[0];
    }
    cells[0]->i = 0;
    cells[0]->j = 0;
    cells[0]->k = 0;
    cells[0]->l = 0;
    cells[0]->scaleFactor = subtree->containingCell ? subtree->containingCell->scaleFactor : 1;
 
    unsigned childOffset = 1;
    for (size_t i = 0; i < numNodes; i++) {
      const uint8_t* ptr = data->ptr + bytesPerNode * i;
      uint8_t nodeType = ptr[0];
      uint8_t childMask = ptr[1];
      uint32_t numPoints = getUint32LE(ptr + 2);
      uint64_t byteOffset = getUint64LE(ptr + 6);
      uint64_t byteCount = getUint64LE(ptr + 14);
 
      PotreeCell* current = cells[i];
      auto cellSizeAtLevel = std::exp2f(-float(subtree->l + current->l));
      auto pos = cellSizeAtLevel * glm::vec3(float((subtree->i << current->l) + current->i),
                                             float((subtree->j << current->l) + current->j),
                                             float((subtree->k << current->l) + current->k));
 
      current->hierarchyNumPoints = byteCount ? numPoints : 0;  // https://github.com/potree/potree/issues/1125
      current->byteOffset = byteOffset;
      current->byteCount = byteCount;
      current->tbmin = glm::max(tbMinCenter, shift + scale * pos);
      current->tbmax = glm::min(tbMaxCenter, shift + scale * (pos + glm::vec3(cellSizeAtLevel)));
 
      if (nodeType == 2) {
        // Link node
        current->kind = PotreeCell::Kind::SubtreeLink;
        current->linkedSubtree = nullptr;
        continue;
      }
 
      for (unsigned childIndex = 0; childIndex < 8; childIndex++) {
 
        if (((1u << childIndex) & childMask) == 0) {
          continue; // child does not exist.
        }
 
        auto* child = cells[childOffset++];
        child->i = (current->i << 1) | ((childIndex >> 2) & 1);
        child->j = (current->j << 1) | ((childIndex >> 1) & 1);
        child->k = (current->k << 1) | ((childIndex >> 0) & 1);
        child->l = current->l + 1;
        child->prefix = current->prefix | (childIndex << (3 * current->l));  // probably don't need this with V2
 
        current->children[childIndex] = child;
      }
    }
 
  }
 
  void parseHrcFile(Context* context, uint32_t instanceId, uint32_t subtreeId, const std::string& url, Cogs::FileContents* data)
  {
    PotreeSystem* potreeSystem = nullptr;
    PotreeComponent* poComp = nullptr;
    PotreeData* poData = nullptr;
    PotreeSubtree* subtree = nullptr;
    bool ret = PotreeSystem::lookupAndCheckForStaleness(context, potreeSystem, poData, poComp, subtree, instanceId, subtreeId);
    if(subtree) subtree->fetch_id = DataFetcherManager::NoFetchId;
 
    if (!ret) {
      if(data){
        LOG_TRACE(logger, "[instance=%u,tree=%u] Received stale *.hrc file, ignoring: %.*s", instanceId, subtreeId, StringViewFormat(data->origin()));
      }
      return;
    }
    else {
#if defined(COGS_PARANOIA)
      assert(!subtree->containingCell || subtree->containingCell->state == PotreeCell::State::Issued);
      assert(subtree->state == PotreeSubtree::State::Issued);
#else
      if (subtree->containingCell && subtree->containingCell->state != PotreeCell::State::Issued) {
        LOG_FATAL(logger, "[instance=%u,tree=%u] Got hierarchy file, but containing cell is not in state Issued(=1) (state=%u), discarding",
                  instanceId, subtreeId, static_cast<unsigned>(subtree->containingCell->state));
        return;
      }
      if (subtree->state != PotreeSubtree::State::Issued) {
        LOG_FATAL(logger, "[instance=%u,tree=%u] Got hierarchy file, but subtree is not in state Issued(=1) (state=%u), discarding.",
                  instanceId, subtreeId, static_cast<unsigned>(subtree->state));
        return;
      }
#endif
 
      if (!data) {
        LOG_ERROR(logger, "[instance=%u,tree=%u] Failed to get hierarchy file: %s", instanceId, subtreeId, url.c_str());
        poData->state = PotreeState::Error;
        subtree->state = PotreeSubtree::State::Failed;
        if (subtree->containingCell) subtree->containingCell->state = PotreeCell::State::Failed;
      }
      else {
        subtree->state = PotreeSubtree::State::Ready;
        if (subtree->containingCell) subtree->containingCell->state = PotreeCell::State::Ready;
 
        if (poData->versionMajor == 1) {
          parseHrcFile1(poData, subtree, instanceId, subtreeId, url, data);
        }
        else {
          parseHrcFile2(poData, subtree, instanceId, subtreeId, url, data);
        }
      }
 
    }
  }
 
}
 
void Cogs::Core::PotreeSubtree::fetchLayoutAndInitialize(Context* context, PotreeSystem* poSystem, const PotreeComponent* poComp, PotreeData* poData, PotreeCell* containingCell)
{
  assert(state == State::None);
  state = State::Issued;
 
  this->containingCell = containingCell;
  if (containingCell == nullptr) {
    // We are fetching the root, index is always [0,0,0,0]
    i = 0;
    j = 0;
    k = 0;
    l = 0;
  }
  else {
    assert(containingCell->kind == PotreeCell::Kind::SubtreeLink);
    assert(containingCell->state == PotreeCell::State::Issued);
    i = (containingCell->owningSubtree->i << containingCell->l) + containingCell->i;
    j = (containingCell->owningSubtree->j << containingCell->l) + containingCell->j;
    k = (containingCell->owningSubtree->k << containingCell->l) + containingCell->k;
    l = containingCell->owningSubtree->l + containingCell->l;
    containingCell->linkedSubtree = this;
  }
 
  std::string url;
  uint64_t fileOffset = 0u;
  uint64_t fileByteSize = 0u;
  if (poData->versionMajor == 1) {
    // Build Potree 1.x URL
    assert(poData->octreeDir.empty() || poData->octreeDir.back() == '/' || poData->octreeDir.back() == '\\');
    if (containingCell == nullptr) {
      path = "r/";
    }
    else {
      std::string localPrefix;
      containingCell->appendDescendSteps(localPrefix);
      prefix = containingCell->owningSubtree->prefix + localPrefix;
      path = containingCell->owningSubtree->path + localPrefix + "/";
    }
    url = poData->octreeDir + path + "r" + prefix + ".hrc";
  }
 
  else {
    // Build Potree 2.x URL
    assert(poData->rootPath.empty() || poData->rootPath.back() == '/' || poData->rootPath.back() == '\\');
    if (containingCell == nullptr) {
      fileOffset = 0;
      fileByteSize = poData->firstChunkSize;
    }
    else {
      fileOffset = containingCell->byteOffset;
      fileByteSize = containingCell->byteCount;
    }
    url = poData->rootPath + "hierarchy.bin";
  }
 
  PotreeSystem::startFetch(context, poComp, poData);
 
  poSystem->requestsInFlight++;
 
  //LOG_DEBUG(logger, "Fetching subtree %s", url.c_str());
  fetch_id = DataFetcherManager::fetchAsync(context, url, [context, url, instanceId = poData->instanceId, treeId = subtreeId](std::unique_ptr<Cogs::FileContents> data)
  {
    auto task = [context, url, instanceId, treeId, dataPtr = data.release()]() {
      std::unique_ptr<Cogs::FileContents> data(dataPtr);
      parseHrcFile(context, instanceId, treeId, url, data.get());
 
      PotreeSystem* poSystem = nullptr;
      PotreeComponent* poComp = nullptr;
      PotreeData* poData = nullptr;
      bool ret = PotreeSystem::lookupAndCheckForStaleness(context, poSystem, poData, poComp, instanceId);
      poSystem->requestsInFlight--;
      PotreeSystem::endFetch(context, poComp, poData);
 
      if (!ret) {
        if(data){
          LOG_DEBUG(logger, "[instance=%u] Received *.hrc file, instance does not exist anymore, ignoring: %.*s", instanceId, StringViewFormat(data->origin()));
        }
        return;
      }
 
      context->engine->setDirty();
    };
    context->engine->runTaskInMainThread(std::move(task));
  }, fileOffset, fileByteSize, true, Cogs::FileContentsHints::None);
}
 
void Cogs::Core::PotreeSubtree::release(Context* context, PotreeData* poData)
{
  while (!cells.empty()) {
    auto* cell = cells.pop();
    cell->releaseData(context, poData); // FIXME: shouldn't happen.
    assert(cell->state == PotreeCell::State::None);
    assert(!HandleIsValid(cell->boxParametersBuffer));
    assert(!HandleIsValid(cell->pointData));
    poData->cellStore.destroy(cell);
  }
  if (state == PotreeSubtree::State::Issued) {
    DataFetcherManager::cancelAsyncFetch(context, fetch_id);
    fetch_id = DataFetcherManager::NoFetchId;
  }
}