ManageLodLevels.cpp

#include "Context.h"
#include "Services/Variables.h"
#include "Services/QualityService.h"
#include "Systems/Core/CameraSystem.h"
#include "Systems/Core/TransformSystem.h"
#include "Systems/Core/ClipShapeSystem.h"
#include "Renderer/Renderer.h"
#include "Utilities/Math.h"
 
#include "PotreeSystem.h"
 
#include "Foundation/Logging/Logger.h"
#include "Foundation/BitTwiddling/PowerOfTwo.h"
 
#include "Rendering/IGraphicsDevice.h"
#include "Rendering/ICapabilities.h"
 
 
#include <algorithm>
 
namespace {
  using namespace Cogs::Core;
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("Potree");
 
  const Cogs::StringView lodFreezeName = "potree.lodFreeze";
 
  const Cogs::StringView pointsMaxName = "potree.points.maxCount";
  constexpr int pointsMaxDefault = 3'000'000;
  const Cogs::StringView chunksMaxName = "potree.chunks.maxCount";
  constexpr int chunksMaxDefault = 1000;
 
  const Cogs::StringView requestsMaxConcurrentName = "potree.requests.maxConcurrent";
  constexpr int requestsMaxConcurrentDefault = 20;
  const Cogs::StringView requestsMaxPerFrameName = "potree.requests.maxPerFrame";
  constexpr int requestsMaxPerFrameDefault = 2;
 
  struct ManageState
  {
    Context* context;
    PotreeSystem* poSystem;
    glm::mat4 P;
    glm::mat4 V;
    float viewHeightPerPixel;
    uint32_t pointsCount;
    uint32_t pointsMaxCount;
    uint32_t activeCellCount;
    uint32_t maxActiveCells;
    uint32_t requestsInFlight;
    uint32_t requestsIssuedThisFrame;
    uint32_t maxConcurrentRequests;
    uint32_t maxPerFrameRequests;
    uint32_t maxOctreeSize;           // Max size of octtree, currently limited by texture 2D max X size.
 
    // Accounting for debugging presented via variables
    uint32_t insideTestEarly1 = 0;
    uint32_t insideTestEarly2 = 0;
    uint32_t insideTestClip = 0;
    uint32_t insideTestSeparated = 0;
    uint32_t insideTestPassed = 0;
 
    bool wantAnotherFrameUpdate = false;
  };
 
  bool potreeNodeHeapCmp(const PotreeNodeHeapItem& a, const PotreeNodeHeapItem& b)
  {
    return a.priority < b.priority;
  }
 
  struct Updater
  {
 
    typedef List<PotreeCell, offsetof(PotreeCell, activeNode)> ActiveList;
 
    const PotreeComponent* poComp = nullptr;
    PotreeData* poData = nullptr;
 
    ActiveList& activeCellsNew;
    ActiveList activeCellsOld;
 
    glm::u8vec4* tex = nullptr;
 
    glm::mat4 VM;
    glm::mat4 PVM;
    glm::mat4 PVMinv;
 
    glm::vec3 eye;  // Camera origin in model space
 
    static constexpr size_t directionsCount = 7 * 3;
    glm::vec3 directions[directionsCount];
    float frustumMin[directionsCount];
    float frustumMax[directionsCount];
 
 
    glm::vec4 clipPlanes[PotreeData::MaxClipPlanes];
    size_t clipPlaneCount = 0;
    bool clipTestInvert = false;
 
    uint32_t numActiveCells = 0;
 
    float VMScale = 1.f;                                    // Overall scale factor for the view-modelview transform
 
    float toleranceSquared = 0.f;
    float rootToleranceSquared = 0.f;
    float priority = 0.f;
    float distanceCutoffScale = 1.f;
    float distanceCutoffMinimum = -1.f;
    float distanceCutoffSquared = 0.f;
 
    Updater(ManageState& state,
            const PotreeComponent* poComp,
            PotreeData* poData)
      : poComp(poComp),
        poData(poData),
        activeCellsNew(poData->activeCells)
    {
      // Move current active set to activeCellsOld and clean current active set.
      activeCellsOld.swap(activeCellsNew);
      const glm::mat4& M = poData->worldFromOcttreeFrame;
 
      if (poComp->clipPlanes.empty()) {
 
        clipPlaneCount = 0;
        if (const ClipShapeComponent* clipComp = poComp->clipShapeComponent.resolveComponent<ClipShapeComponent>(); clipComp) {
          const ClipShapeData& clipData = state.context->clipShapeSystem->getData(clipComp);
 
          switch (clipData.shape) {
          case ClipShapeType::None:
            break;
          case ClipShapeType::Cube:
          case ClipShapeType::InvertedCube:
            assert(clipData.planeCount == 6);
            clipPlaneCount = clipData.planeCount;
            for (size_t i = 0; i < clipPlaneCount; i++) {
              clipPlanes[i] = clipData.planes[i] * M;
            }
            clipTestInvert = clipData.shape == ClipShapeType::InvertedCube;
            break;
          default:
            assert(false && "Unhandled case");
            break;
          }
        }
      }
      else {
 
        clipPlaneCount = std::min(poComp->disableCustomClipping ? size_t(0) : PotreeData::MaxClipPlanes, poComp->clipPlanes.size());
        for (size_t i = 0; i < clipPlaneCount; i++) {
          clipPlanes[i] = poComp->clipPlanes[i] * M;
        }
      }
 
      for (size_t i = clipPlaneCount; i < PotreeData::MaxClipPlanes; i++) {
        clipPlanes[i] = glm::vec4(0, 0, 0, 1);  // Not used. always positive regardlesss of position
      }
 
 
      VM = state.V * M;
 
      {
        // We use the projection/frustm quite heavily for culling. To improve
        // numerical stability, we restrict the depth range of the frustum to
        // the range where the point cloud exists. 
 
        // First, find the depth range of the point cloud in the full frustum
        const glm::vec3& boundsMin = poData->octtreeFrame.tightBBoxMin;
        const glm::vec3& boundsMax = poData->octtreeFrame.tightBBoxMax;
        glm::vec3 boundsCorners[8] = {
          glm::vec3(boundsMin.x, boundsMin.y, boundsMin.z),
          glm::vec3(boundsMax.x, boundsMin.y, boundsMin.z),
          glm::vec3(boundsMin.x, boundsMax.y, boundsMin.z),
          glm::vec3(boundsMax.x, boundsMax.y, boundsMin.z),
          glm::vec3(boundsMin.x, boundsMin.y, boundsMax.z),
          glm::vec3(boundsMax.x, boundsMin.y, boundsMax.z),
          glm::vec3(boundsMin.x, boundsMax.y, boundsMax.z),
          glm::vec3(boundsMax.x, boundsMax.y, boundsMax.z),
        };
        float clipZMin = std::numeric_limits<float>::max();
        float clipZMax = -std::numeric_limits<float>::max();
        glm::mat4 PVM_ = state.P * VM;
        for (size_t i = 0; i < 8; i++) {
          glm::vec4 h = PVM_ * glm::vec4(boundsCorners[i], 1.f);
          float z = h.z / std::abs(h.w);
          if (!std::isfinite(z)) {
            // Division by zero, skip. Assume that corner landed in plane through eye
            // in a perspective projection, and thus the near plane should work as a
            // depth value.
            z = state.context->variables->get("renderer.reverseDepth", false) ? 1.f : -1.f;
          }
          clipZMin = std::min(clipZMin, z);
          clipZMax = std::max(clipZMax, z);
        }
        clipZMin = std::max(clipZMin, -1.f);
        clipZMax = std::min(clipZMax, 1.f);
 
        // Add a scale & bias matrix after application of the
        // projection mapping the depth range of the point
        // cloud to [-1, 1].
        float scale = 2.f / (clipZMax - clipZMin);
        float bias = -(1.f + scale * clipZMin);
        PVM = glm::mat4(1.f, 0.f, 0.f, 0.f,
                        0.f, 1.f, 0.f, 0.f,
                        0.f, 0.f, scale, 0.f,
                        0.f, 0.f, bias, 1.f) * state.P * VM;
      }
 
      PVMinv = glm::inverse(PVM);
      VMScale = std::cbrt(std::abs(glm::determinant(glm::mat3(VM))));
 
      poData->frustum = poData->worldFromOcttreeFrame * PVMinv;
 
      eye = euclidean(glm::inverse(VM) * glm::vec4(0, 0, 0, 1));
 
      if(0.f < poComp->distanceCutoffScale && 0.f < poComp->distanceCutoffMinimum) {
        
        // Calculate distance-based cutoff controlled by elevation above the hierarchy's z minimum value.
 
        // Add max-norm distance to boundary volume (that is, it is zero inside the volume) to the elevation used for distance-based cutoff
        // so that the effect diminished sharply when the camera is not inside the point cloud.
        glm::vec3 distancesToBoundaryVolume = glm::abs(eye - glm::clamp(eye, poData->root->tbmin, poData->root->tbmax));
        float maxDistanceToBoundaryVolue = std::max(distancesToBoundaryVolume.x, std::max(distancesToBoundaryVolume.y, distancesToBoundaryVolume.z));
        float distanceCutoff = std::max(poComp->distanceCutoffMinimum, poComp->distanceCutoffScale * (maxDistanceToBoundaryVolue + std::abs(eye.z - poData->root->tbmin.z)));
        distanceCutoffSquared = distanceCutoff * distanceCutoff;
      }
      else {
        distanceCutoffSquared = -1.f;
      }
 
 
      // Set up directions for fine-grained frustum tests
      {
        // Frustum corners in model space
        glm::vec3 F[8] = {
          euclidean(PVMinv * glm::vec4(-1, -1, -1, 1)),
          euclidean(PVMinv * glm::vec4( 1, -1, -1, 1)),
          euclidean(PVMinv * glm::vec4(-1,  1, -1, 1)),
          euclidean(PVMinv * glm::vec4( 1,  1, -1, 1)),
          euclidean(PVMinv * glm::vec4(-1, -1,  1, 1)),
          euclidean(PVMinv * glm::vec4( 1, -1,  1, 1)),
          euclidean(PVMinv * glm::vec4(-1,  1,  1, 1)),
          euclidean(PVMinv * glm::vec4( 1,  1,  1, 1))
        };
 
        // Edges of frustum in model space
        glm::vec3 f[6] = {
          F[4] - F[0],
          F[5] - F[1],
          F[6] - F[2],
          F[7] - F[3],
          F[1] - F[0],
          F[2] - F[0]
        };
 
        // Cross between one frustum edge and one chunk aabb
        directions[ 0] = glm::cross(f[0], glm::vec3(1, 0, 0));
        directions[ 1] = glm::cross(f[0], glm::vec3(0, 1, 0));
        directions[ 2] = glm::cross(f[0], glm::vec3(0, 0, 1));
        
        directions[ 3] = glm::cross(f[1], glm::vec3(1, 0, 0));
        directions[ 4] = glm::cross(f[1], glm::vec3(0, 1, 0));
        directions[ 5] = glm::cross(f[1], glm::vec3(0, 0, 1));
        
        directions[ 6] = glm::cross(f[2], glm::vec3(1, 0, 0));
        directions[ 7] = glm::cross(f[2], glm::vec3(0, 1, 0));
        directions[ 8] = glm::cross(f[2], glm::vec3(0, 0, 1));
        
        directions[ 9] = glm::cross(f[3], glm::vec3(1, 0, 0));
        directions[10] = glm::cross(f[3], glm::vec3(0, 1, 0));
        directions[11] = glm::cross(f[3], glm::vec3(0, 0, 1));
        
        directions[12] = glm::cross(f[4], glm::vec3(1, 0, 0));
        directions[13] = glm::cross(f[4], glm::vec3(0, 1, 0));
        directions[14] = glm::cross(f[4], glm::vec3(0, 0, 1));
 
        directions[15] = glm::cross(f[5], glm::vec3(1, 0, 0));
        directions[16] = glm::cross(f[5], glm::vec3(0, 1, 0));
        directions[17] = glm::cross(f[5], glm::vec3(0, 0, 1));
        // Face directions of aabb in model space
        directions[18] = glm::vec3(1, 0, 0);
        directions[19] = glm::vec3(0, 1, 0);
        directions[20] = glm::vec3(0, 0, 1);
        // Frustum faces in model space, this is checked by early tests, no need to check again
        //directions[21] = glm::cross(f[0], f[4]);
        //directions[22] = glm::cross(f[1], f[5]);
        //directions[23] = glm::cross(f[2], f[4]);
        //directions[24] = glm::cross(f[3], f[5]);
        //directions[25] = glm::cross(f[4], f[5]);
 
        for (size_t i = 0; i < directionsCount; i++) {
 
          // Normalize dir or make it zero if it is degenerate
          float s = 1.f / glm::length(directions[i]);
          if (std::isfinite(s)) {
            directions[i] *= s;
          }
          else {
            directions[i] = glm::vec3(0.f);
          }
 
          // Project frustum onto direction and calc cover
          float max_ = -std::numeric_limits<float>::max();
          float min_ = std::numeric_limits<float>::max();
          for (size_t k = 0; k < 8; k++) {
            float e = dot(directions[i], F[k]);
            max_ = std::max(max_, e);
            min_ = std::min(min_, e);
          }
          frustumMin[i] = min_;
          frustumMax[i] = max_;
        }
      }
 
      // Convert screen pixel tolerances to normalized view-height
      float tolerance = state.viewHeightPerPixel * poData->tolerance;
      toleranceSquared = tolerance * tolerance;
 
      float rootTolerance = state.viewHeightPerPixel * poData->rootTolerance;
      rootToleranceSquared = rootTolerance * rootTolerance;
 
      // Traverse tree breadth first.
      poData->lodHeap.clear();
 
      float rootFootprint = calcCellPriority(state, poData->root);
      if (0 <= rootFootprint) {
        poData->lodHeap.push_back(PotreeNodeHeapItem{ poData->root, rootFootprint });
        priority = rootFootprint;
        numActiveCells++;
      }
    }
 
    [[nodiscard]] bool cellInsideFrustum(ManageState& state, const PotreeCell* cell)
    {
      const glm::vec3 cellMin = cell->tbmin;
      const glm::vec3 cellMax = cell->tbmax;
      glm::vec3 cellCorners[8] = {
        glm::vec3(cellMin.x, cellMin.y, cellMin.z),
        glm::vec3(cellMin.x, cellMin.y, cellMax.z),
        glm::vec3(cellMin.x, cellMax.y, cellMin.z),
        glm::vec3(cellMin.x, cellMax.y, cellMax.z),
        glm::vec3(cellMax.x, cellMin.y, cellMin.z),
        glm::vec3(cellMax.x, cellMin.y, cellMax.z),
        glm::vec3(cellMax.x, cellMax.y, cellMin.z),
        glm::vec3(cellMax.x, cellMax.y, cellMax.z),
      };
 
 
      uint32_t allInsideMask = 0b111111;
      {
        uint32_t anyInsideMask = 0u;
        for (size_t i = 0; i < 8; i++) {
          glm::vec4 p = PVM * glm::vec4(cellCorners[i], 1.f);
          uint32_t cornerMask =
            (p.x <= p.w ? 1 : 0) |
            (p.y <= p.w ? 2 : 0) |
            (p.z <= p.w ? 4 : 0) |
            (-p.w <= p.x ? 8 : 0) |
            (-p.w <= p.y ? 16 : 0) |
            (-p.w <= p.z ? 32 : 0);
          anyInsideMask = anyInsideMask | cornerMask;
          allInsideMask = allInsideMask & cornerMask;
        }
 
        if (anyInsideMask != 0b111111) {
          // At least one plane where all corners are outside => outside.
          state.insideTestEarly1++;
          return false;
        }
      }
 
      if (clipPlaneCount) {
        // Cull against clip planes, inactive planes are [0,0,0,1] which is always positive/inside.
        uint32_t anyInsideClippedMask = 0u;
        uint32_t allInsideClippedMask = 0b111111;
        for (size_t i = 0; i < 8; i++) {
          glm::vec4 q = glm::vec4(cellCorners[i], 1.f);
          uint32_t cornerClippedMask =
            (0.f <= dot(q, clipPlanes[0]) ? (1 << 0) : 0) |
            (0.f <= dot(q, clipPlanes[1]) ? (1 << 1) : 0) |
            (0.f <= dot(q, clipPlanes[2]) ? (1 << 2) : 0) |
            (0.f <= dot(q, clipPlanes[3]) ? (1 << 3) : 0) |
            (0.f <= dot(q, clipPlanes[4]) ? (1 << 4) : 0) |
            (0.f <= dot(q, clipPlanes[5]) ? (1 << 5) : 0);
          anyInsideClippedMask = anyInsideClippedMask | cornerClippedMask;
          allInsideClippedMask = allInsideClippedMask & cornerClippedMask;
        }
 
        if (clipTestInvert == false) {
          // If one of the planes have no point inside, the cell is completely outside clip region.
          if (anyInsideClippedMask != 0b111111) {
            state.insideTestClip++;
            return false;
          }
        }
        else {
          // If all planes have all corners inside the region, the cell is completely inside the cut-out region.
          if (allInsideClippedMask == 0b111111) {
            state.insideTestClip++;
            return false;
          }
        }
      }
 
      if (allInsideMask == 0b111111) {
        // At least one plane where all corners are outside => outside.
        state.insideTestEarly2++;
        return true;
      }
 
 
      // Try a set of directions to separate the sets
      for (size_t i = 0; i < directionsCount; i++) {
        float ma_ = -std::numeric_limits<float>::max();
        float mi_ = std::numeric_limits<float>::max();
        for (size_t k = 0; k < 8; k++) {
          float e = dot(directions[i], cellCorners[k]);
          ma_ = std::max(ma_, e);
          mi_ = std::min(mi_, e);
        }
 
        if (ma_ < frustumMin[i] || frustumMax[i] < mi_) {
          // Convex hull of frustum and convex hull of aabb projected onto this direction does not overlap.
          state.insideTestSeparated++;
          return false;
        }
 
      }
      state.insideTestPassed++;
      return true;
    }
 
 
    [[nodiscard]] float calcCellPriority(ManageState& state, PotreeCell* cell)
    {
      if (cellInsideFrustum(state, cell)) {
 
        const glm::vec3 cellMin = cell->tbmin;
        const glm::vec3 cellMax = cell->tbmax;
 
        const glm::vec3 neareastPoint = glm::clamp(eye, cellMin, cellMax);
        const float eyeDistanceSquared = glm::distance2(eye, neareastPoint);
        if (0.f <= distanceCutoffSquared && distanceCutoffSquared < eyeDistanceSquared) {
          return -1.f;
        }
 
 
 
        // Cell center in view space
        glm::vec3 c = euclidean(VM * glm::vec4(cellMin + cellMax, 2.f));
 
        // Squared distance from eye to cell center
        float d2 = dot(c, c);
 
 
        // Cell diagonal.
        glm::vec3 d = cellMax - cellMin;
 
        // Squared cell radius in view space
        float r2 = VMScale * VMScale * dot(d, d);
 
        float squaredWeight = r2 / d2;
        if (std::isfinite(squaredWeight)) {
 
          if (cell->isRootNode()) {
            if (rootToleranceSquared <= squaredWeight) return squaredWeight;
          }
          else {
            if (toleranceSquared <= squaredWeight) return squaredWeight;
          }
        }
      }
      return -1.f;
    }
 
    [[nodiscard]] PotreeCell* handleSubtreeLink(Context* context, ManageState& state, PotreeCell * cell)
    {
      assert(cell->kind == PotreeCell::Kind::SubtreeLink);
 
      // Node is actually the root node of a child subtree. 
 
      // We haven't populated this subtree, request data if we are allowed.
      if (cell->linkedSubtree == nullptr) {
 
        if (state.requestsInFlight < state.maxConcurrentRequests) {
 
          if (state.requestsIssuedThisFrame < state.maxPerFrameRequests) {
            assert(cell->state == PotreeCell::State::None);
            cell->state = PotreeCell::State::Issued;
            state.poSystem->issueSubtreeFetch(context, poComp, poData, cell);
            state.requestsInFlight++;
            state.requestsIssuedThisFrame++;
          }
          else {
            // Cannot issue more this frame, continue in next frame
            state.wantAnotherFrameUpdate = true;
          }
        }
 
        // We don't have any data on the subtree, ignore this cell for now.
        return nullptr;
      }
 
      // We have indeed a subtree. If subtree request has been fully processed,
      // and we have a root node, recurse into the subtree.
      auto* subtree = cell->linkedSubtree;
      if (subtree->state != PotreeSubtree::State::Ready) {
 
        // We're waiting for data on the subtree, skip this part of the hierarchy for now.
        return nullptr;
      }
 
      if (subtree->cells.empty()) {
        // No cells in subtree, just quit
        return nullptr;
      }
 
      auto* subtreeRoot = subtree->cells.front();
 
      assert(subtreeRoot->kind == PotreeCell::Kind::RegularNode && "Invalid subtree");
 
      // Link node occupies same space as root of subtree, so we sneak the root in
      // into the active list and continue on its children. Note, we do not increment
      // activeCellCount on purpose.
      if (subtreeRoot->isActive()) {
        activeCellsOld.remove(subtreeRoot);
      }
      subtreeRoot->setActive();
      activeCellsNew.push(subtreeRoot);
 
      // Continue with root in subtree
      return subtreeRoot;
    }
 
 
    [[nodiscard]] bool updateIteration(Context* context, ManageState& state)
    {
      assert(!poData->lodHeap.empty());
 
      // Pop top of heap.
      std::pop_heap(poData->lodHeap.begin(), poData->lodHeap.end(), potreeNodeHeapCmp);
      PotreeNodeHeapItem item = poData->lodHeap.back();
      poData->lodHeap.pop_back();
 
      if (state.maxActiveCells <= state.activeCellCount) {
        return false;
      }
 
      if (state.maxOctreeSize <= numActiveCells) {
        // We have filled up the hierarchy texture, stop processing.
        return false;
      }
 
      PotreeCell* cell = item.cell;
 
      if (state.pointsMaxCount < state.pointsCount + cell->pointCount) {
        // We have reached the point budget, stop processing.
        return false;
      }
 
      // Cell have passed all tests, cell is now included, update state
      state.activeCellCount++;
      state.pointsCount += cell->pointCount;
 
      // If cell was already active, remove it from activeOld before putting it into active.
      if ((cell->flags & PotreeCell::Flags::Active) == PotreeCell::Flags::Active) {
        activeCellsOld.remove(cell);
      }
 
      cell->setActive();
      activeCellsNew.push(cell);
 
      // Node is actually the root node of a child subtree.
      // Handle the transition into the subtree.
      if (cell->kind == PotreeCell::Kind::SubtreeLink) {
        cell = handleSubtreeLink(context, state, cell);
        if (cell == nullptr) {
          return !poData->lodHeap.empty(); // Tree not ready, but do continue to process other cells.
        }
      }
      assert(cell->kind == PotreeCell::Kind::RegularNode);
 
      // Check if cell has data, and if not, request if we are allowed.
      // Don't recurse into children unless current node has
      // point data available. Makes hierarchy follow where
      // we actually have data.
      if (cell->state == PotreeCell::State::None) {   // Cell lacks data.
 
        // Handle empty cells. This do happen sometimes, we just set it to ready
        // and the renderer will skip it since it has no data.
        if (cell->byteCount == 0) {
          cell->state = PotreeCell::State::Ready;
          for (PotreeCell* child : cell->children) {
            if (child && child->state != PotreeCell::State::Ready) {
              child->scaleFactor = cell->scaleFactor;
            }
          }
        }
 
        // Request data if we are allowed an request.
        else if (state.requestsInFlight < state.maxConcurrentRequests) {
          
          if (state.requestsIssuedThisFrame < state.maxPerFrameRequests) {
            cell->fetchData(context, state.poSystem, poComp, poData);
            state.requestsInFlight++;
            state.requestsIssuedThisFrame++;
          }
          else {
            // Cannot issue more this frame, continue in next frame
            state.wantAnotherFrameUpdate = true;
          }
        }
 
        return !poData->lodHeap.empty();  // Cell not ready, but do continue to process other cells.
      }
 
      // Push all visible children onto the next iteration
      for (unsigned i = 0; i < 8; i++) {
        if (auto* child = cell->children[i]; child) {
 
          float importance = calcCellPriority(state, child);
 
          // If child is visible, have a significant footprint, and there is room in the octtree texture
 
          // We should set childmask here and handle dead-ends when traversing.
          if (0 <= importance) {
            poData->lodHeap.push_back(PotreeNodeHeapItem{ child, importance });
            std::push_heap(poData->lodHeap.begin(), poData->lodHeap.end(), potreeNodeHeapCmp);
 
            // Update priority of this tree with front of queue
#if 1
            priority = poData->lodHeap.front().priority;
#else
            std::pop_heap(poData->lodHeap.begin(), poData->lodHeap.end(), potreeNodeHeapCmp);
            priority = poData->lodHeap.back().priority;
            std::push_heap(poData->lodHeap.begin(), poData->lodHeap.end(), potreeNodeHeapCmp);
#endif
          }
        }
      }
 
 
      return !poData->lodHeap.empty();
    }
 
    void populateTexture(ManageState& state)
    {
      poData->octtreeTextureData.resize(state.maxOctreeSize, false);
      std::memset(static_cast<void*>(poData->octtreeTextureData.data()), ~0u, poData->octtreeTextureData.byteSize());
 
      // Process queue, numCells denote tail of queue and i is head.
      // We process cell from the front of the queue and if the cell
      // has active children, we push them on the back of the queue.
      //
      // This ends up as a breadth-first traversal of the cell hierarchy.
      //
      // We size the queue to hold all active cells, so we don't have
      // to think about resize or wraparound.
      poData->tmpCellPointers.resize(state.maxOctreeSize, false);
      std::memset(poData->tmpCellPointers.data(), 0u, poData->tmpCellPointers.byteSize());
 
      glm::u8vec4* tex = poData->octtreeTextureData.data();
      PotreeCell** cells = poData->tmpCellPointers.data();
 
      size_t numCells = 0;
      if (poData->root->isActive()) {
        cells[numCells++] = poData->root;
      }
      else {
        tex[0].r = 0xffu; // Empty tree, store invalid data to trigger asserts
        tex[0].g = 0xffu;
        tex[0].b = 0xffu;
        tex[0].a = 0xffu;
      }
 
      for (size_t currIx = 0; currIx < numCells; currIx++) {
        PotreeCell* cell = cells[currIx];
        size_t firstChild = numCells;
        cell->texOffset = static_cast<uint16_t>(currIx);
 
        unsigned childMask = 0;
        for (unsigned k = 0; k < 8; k++) {
          if (auto* child = cell->children[k]; (child != nullptr) && (child->isActive())) {
            if (child->kind == PotreeCell::Kind::SubtreeLink) {
              if (child->linkedSubtree == nullptr ||
                  child->linkedSubtree->state != PotreeSubtree::State::Ready ||
                  child->linkedSubtree->cells.empty())
              {
                continue; // Subtree not ready
              }
              child = child->linkedSubtree->cells.front();
              assert(child->kind == PotreeCell::Kind::RegularNode);
            }
            if (HandleIsValid(child->pointData)) {
              // If child exists, is active, and has data available, it is
              // included in this tree representation.
              //
              // Children of a cells will lie back-to-back in in their proper order.
              assert(numCells < state.maxOctreeSize);
              childMask |= 1u << k;
              cells[numCells++] = child;
            }
          }
        }
 
        if (childMask) {
          tex[currIx].g = static_cast<uint8_t>(firstChild & 0xffu); // offset to first child, MSB
          tex[currIx].b = static_cast<uint8_t>(firstChild >> 8);    // offset to first child, LSB
        }
        else {
          tex[currIx].g = 0xffu;    // No children, store invalid offset to trigger asserts
          tex[currIx].b = 0xffu;
        }
        tex[currIx].r = static_cast<uint8_t>(childMask);
        tex[currIx].a = cell->scaleFactor;
      }
    }
 
    void cleanup(Context* context)
    {
      // Retire cells that was active but is no longer active.
      List<PotreeSubtree, offsetof(PotreeSubtree, member)> inactiveSubtrees;
 
      while (!activeCellsOld.empty()) {
        auto* cell = activeCellsOld.shift();
 
        cell->texOffset = static_cast<uint16_t>(~0u);  // illegal offset to trigger assert if anything goes wrong.
        cell->flags &= ~PotreeCell::Flags::Active;
        cell->releaseData(context, poData);
 
        if (cell->kind == PotreeCell::Kind::SubtreeLink && cell->linkedSubtree) {
          poData->subtrees.remove(cell->linkedSubtree);
          inactiveSubtrees.push(cell->linkedSubtree);
          cell->linkedSubtree = nullptr;
        }
      }
 
      while (!inactiveSubtrees.empty()) {
        auto* subtree = inactiveSubtrees.shift();
        assert(subtree->containingCell && "Don't delete the root tree");
        //LOG_DEBUG(logger, "Deleting subtree %s", subtree->path.c_str());
        subtree->release(context, poData);
        poData->subtreeStore.destroy(subtree);
      }
    }
 
 
  };
 
  bool updaterPointerCmp(const Updater* a, const Updater* b)
  {
    assert(!a->poData->lodHeap.empty());
    assert(!b->poData->lodHeap.empty());
    return a->priority < b->priority;
  }
 
 
}
 
 
void Cogs::Core::PotreeSystem::updateLodLevels(Context* context)
{
  // Set up state that is used for figuring out the lod levels
  const CameraComponent* mainCamComp = context->cameraSystem->getMainCamera();
  const CameraData& mainCamData = context->cameraSystem->getData(mainCamComp);
 
  ManageState state{};
  state.context = context;
  state.poSystem = this;
  state.P = mainCamData.rawProjectionMatrix;
  state.V = mainCamData.viewMatrix;
 
  if (mainCamComp->projectionMode == Cogs::Core::ProjectionMode::Perspective) {
    state.viewHeightPerPixel = std::sin(mainCamComp->fieldOfView) / std::max(1.f, glm::length(mainCamData.viewportSize));
  }
  else {
    state.viewHeightPerPixel = 1.f / std::max(1.f, glm::length(mainCamData.viewportSize));
  }
 
  state.maxOctreeSize = context->renderer->getDevice()->getCapabilities()->getDeviceCapabilities().MaxTexture2DSize;
 
  // Set up variables controlling lod levels
  if (!context->variables->exist(lodFreezeName)) { context->variables->set(lodFreezeName, false); }
  if (!context->variables->exist(pointsMaxName)) { context->variables->set(pointsMaxName, pointsMaxDefault); }
  if (!context->variables->exist(chunksMaxName)) { context->variables->set(chunksMaxName, std::min(int(state.maxOctreeSize), chunksMaxDefault)); }
  if (!context->variables->exist(requestsMaxConcurrentName)) { context->variables->set(requestsMaxConcurrentName, requestsMaxConcurrentDefault); }
  if (!context->variables->exist(requestsMaxPerFrameName)) { context->variables->set(requestsMaxPerFrameName, requestsMaxPerFrameDefault); }
 
 
  // Skip updating if we are lod-freezing
  if (const Variable* var = context->variables->get(lodFreezeName); var->getBool()) {
    return;
  }
 
  state.pointsCount = 0;
  state.pointsMaxCount = static_cast<unsigned>(std::max(1, static_cast<int>(context->qualityService->potreeSystemChunkCountScale * context->variables->get(pointsMaxName, pointsMaxDefault))));
 
  // Desktop async fetches return immediately, hence we
  // use an extra variable here to make sure that we don't
  // issue more than max number of requests per frame.
  state.activeCellCount = 0;
  state.maxActiveCells = static_cast<unsigned>(std::max(1, static_cast<int>(context->qualityService->potreeSystemChunkCountScale * context->variables->get(chunksMaxName, chunksMaxDefault))));
  state.requestsInFlight = requestsInFlight;
  state.requestsIssuedThisFrame = 0;
  state.maxConcurrentRequests = static_cast<unsigned>(std::max(0, context->variables->get(requestsMaxConcurrentName, requestsMaxConcurrentDefault)));
  state.maxPerFrameRequests = static_cast<uint32_t>(std::max(0, context->variables->get(requestsMaxPerFrameName, requestsMaxPerFrameDefault)));
 
 
  // Create a set of updaters, one per visible, running potree instance with data.
  std::vector<Updater> updaters;
  updaters.reserve(pool.size());
  for (auto& poComp : pool) {
    PotreeData* poData = getData(&poComp).poData.get();
    if (!poComp.isVisible() || poData->state != PotreeState::Running || poData->root == nullptr) {
      continue;
    }
    if (poComp.disableLodUpdates) {
      continue;
    }
    assert(!poData->subtrees.empty());
    updaters.emplace_back(state, &poComp, poData);
  }
 
  // Limit max number of cells to the theoretical limit of cells
  state.maxActiveCells = std::min(state.maxActiveCells, state.maxOctreeSize * static_cast<uint32_t>(updaters.size()));
 
  // Keep list of active updaters in a heap ordered by the instance's most important cell
  std::vector<Updater*> updaterHeap;
  updaterHeap.reserve(updaters.size());
  for (auto& updater : updaters) {
    if (!updater.poData->lodHeap.empty()) {
      updaterHeap.push_back(&updater);
    }
  }
  std::make_heap(updaterHeap.begin(), updaterHeap.end(), updaterPointerCmp);
 
  // Repeatedly process the most important instance, removing instances that
  // has nothing more to do or has exhausted resource limits, until all instances
  // are done.
  while (!updaterHeap.empty()) {
    std::pop_heap(updaterHeap.begin(), updaterHeap.end(), updaterPointerCmp);
    if (updaterHeap.back()->updateIteration(context, state)) { // returns true if more work to do
      std::push_heap(updaterHeap.begin(), updaterHeap.end(), updaterPointerCmp);
    }
    else {
      updaterHeap.pop_back();
    }
  }
  for (auto& updater : updaters) {
    updater.cleanup(context);
    updater.populateTexture(state);
  }
 
 
  if (state.wantAnotherFrameUpdate) {
    context->engine->triggerUpdate();
  }
  context->variables->set("potree.instrument.wantAnotherFrameUpdate", int(state.wantAnotherFrameUpdate));
 
  context->variables->set("potree.instrument.currentPointCount", int(state.pointsCount));
  context->variables->set("potree.instrument.activeCellCount", int(state.activeCellCount));
 
  context->variables->set("potree.instrument.requests.issuedThisFrame", int(state.requestsIssuedThisFrame));
  context->variables->set("potree.instrument.requests.inFlight", int(state.requestsInFlight));
 
  uint32_t insideTestTotal = state.insideTestEarly1 + state.insideTestEarly2 + state.insideTestSeparated + state.insideTestPassed;
  context->variables->set("potree.instrument.insideTest.total", int(insideTestTotal));
  context->variables->set("potree.instrument.insideTest.early1", int(state.insideTestEarly1));
  context->variables->set("potree.instrument.insideTest.clip", int(state.insideTestClip));
  context->variables->set("potree.instrument.insideTest.early2", int(state.insideTestEarly2));
  context->variables->set("potree.instrument.insideTest.separated", int(state.insideTestSeparated));
  context->variables->set("potree.instrument.insideTest.passed", int(state.insideTestPassed));
}