SparseBuildOctree.cpp

#include "SparseBuildOctree.h"
#include "Batch.h"
 
#include "Resources/Mesh.h"
#include "Resources/MeshManager.h"
#include "Resources/Model.h"
#include "Resources/MaterialManager.h"
 
#include "Utilities/Parallel.h"
#include "Serialization/ModelWriter.h"
 
#include "Foundation/Platform/IO.h"
#include "Foundation/Platform/Timer.h"
#include "Foundation/BitTwiddling/MortonCode.h"
#include "Foundation/Logging/Logger.h"
 
#include "../Extensions/GeometryProcessing/GeometryProcessing.h"
 
#if !defined( EMSCRIPTEN ) && !defined( __APPLE__ )
 
#include <span>
#include <mutex>
#include <atomic>
#include <xmmintrin.h>
#include <glm/gtx/color_space.hpp>
#include <glm/gtx/compatibility.hpp>
 
#ifdef HAS_RATIONAL_REDUCER
#include <rrapi/rrapi.h>
#endif
 
#include <meshoptimizer/meshoptimizer.h>
 
 
namespace
{
  using namespace Cogs::Core;
  using Cogs::Geometry::BoundingBox;
 
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("SparseBuildOctree");
 
 
  struct WorkItem {
    uint64_t sortKey;
    SparseOctree::Cell* cell;
  };
 
  struct SharedModel {
    std::vector<MeshHandle> meshes;
    std::string relativePath;
    std::string absolutePath;
    size_t vertexCount = 0;
  };
 
  struct CellProcessContext
  {
    SparseOctree::Octree& octree;
    SparseOctree::Grid& grid;
    SparseOctree::SceneInfo& scene;
    std::string relativePrefix;
    std::string filenamePrefix;
    MaterialInstanceHandle materialInstance;
    WriteModelSettings settings{};
    size_t level;
    size_t cellCount = 0;
    std::atomic<size_t> processedCells = 0;
    std::atomic<size_t> fileCount = 0;
 
    std::vector<WorkItem> workItems;
 
    struct {
      std::mutex lock;
      std::vector<bool> cellsDone;
      SharedModel model;
      size_t cellsWritten = 0;
    } writeCells;
 
    float vertexMergeEpsilon = 0.0001f; // One tenth of a millimeter
    float clipEpsilon = 0.01f;          // One centimeter
    float featureAngle = 0.7f;
    float protrusionAngle = 0.7f;
    size_t maxMeshVertexCount;          // The number of vertices a mesh can have before we start to split it
    size_t maxModelVertexCount;         // Meshes with more vertices will get its own model file
    size_t maxModelMeshCount;           // Max number of small meshes combined in one cogsbin file.
    size_t maxSplitDepth = 8;           // Safety valve for big-mesh-in-cell-split-recursion.
  };
 
  struct Vertices {
    std::vector<glm::vec3> pos;
    std::vector<glm::vec3> nrm;
    std::vector<glm::vec2> tex;
 
    void clear() { pos.clear(); nrm.clear(); tex.clear(); }
    bool empty() const { return pos.empty(); }
    size_t size() const { return pos.size(); }
    void resize(size_t size) { pos.resize(size); nrm.resize(size); tex.resize(size); }
    void assertSanity() const { assert(pos.size() == nrm.size()); assert(pos.size() == tex.size()); }
  };
 
  struct SplitStackItem {
    Vertices geo;
    size_t depth = 0;
  };
 
  struct Split {
    Vertices sides[2];
  };
  using Splits = Split[3];
 
  // Per thread data
  struct CellWorkerContext
  {
    CellProcessContext& ctx;
    std::vector<glm::vec3> clippedV;
    std::vector<glm::vec3> clippedN;
    std::vector<glm::vec2> clippedT;
    std::vector<uint32_t> map;
    std::vector<uint32_t> unique;
    std::vector<glm::vec3> tmpPos;
    std::vector<glm::vec2> tmpTex;
    std::vector<uint32_t> tmpIx;
    std::vector<uint32_t> tmpIx2;
 
    Splits splits;
 
    struct {
      std::vector<MeshHandle> meshes;
      std::string relativePath;
      std::string absolutePath;
      size_t vertexCount = 0;
    } smallMeshesModelFile;
 
  };
 
  unsigned calculateClippingMask(glm::vec3 const& position, __m128 const& clipBoxMin, __m128 const& clipBoxMax)
  {
    auto pos = _mm_set_ps(0.0f, position.z, position.y, position.x);
 
    auto cmp1 = _mm_cmplt_ps(pos, clipBoxMax);
    auto cmp2 = _mm_cmplt_ps(clipBoxMin, pos);
 
    auto mask1 = _mm_movemask_ps(cmp1) & 0x7;
    auto mask2 = _mm_movemask_ps(cmp2) & 0x7;
 
    return mask1 | (mask2 << 3);
  }
 
  struct Geom
  {
    float* V;
    float* N;
    float* T;
 
    std::vector<float> Vv;
    std::vector<float> Nn;
    std::vector<float> Tt;
 
    void reserve(size_t size)
    {
      Vv.resize(size * 3 * 3);
      Nn.resize(size * 3 * 3);
      Tt.resize(size * 3 * 2);
 
      V = Vv.data();
      N = Nn.data();
      T = Tt.data();
    }
  };
 
  thread_local Geom clipA;
  thread_local Geom clipB;
 
  void filterCells(std::vector<Mesh*>& clipped,
                   std::vector<Mesh*>& contained,
                   const Cogs::Geometry::BoundingBox& geoClip,
                   const SparseOctree::SceneInfo& scene,
                   const std::vector<uint32_t>& touches)
  {
    for (uint32_t index : touches) {
      const SparseOctree::EntityRef& e = scene.entities[index];
      Mesh* mesh = e.mesh.resolve();
 
      if (mesh->primitiveType != Cogs::PrimitiveType::TriangleList) continue;
 
      const Cogs::Geometry::BoundingBox& bbox = e.box;
      if ((geoClip.min.x <= bbox.min.x) &&
          (geoClip.min.y <= bbox.min.y) &&
          (geoClip.min.z <= bbox.min.z) &&
          (bbox.max.x <= geoClip.max.x) &&
          (bbox.max.y <= geoClip.max.y) &&
          (bbox.max.z <= geoClip.max.z))
      {
        contained.push_back(mesh);
      }
      else {
        clipped.push_back(mesh);
      }
    }
  }
 
  void clipMeshes(CellWorkerContext& wCtx,
                  Vertices& cellGeo,
                  const Cogs::Geometry::BoundingBox& geoClip,
                  const std::vector<Mesh*>& clipped)
  {
    std::vector<glm::vec3>& clippedV = wCtx.clippedV;
    std::vector<glm::vec3>& clippedN = wCtx.clippedN;
    std::vector<glm::vec2>& clippedT = wCtx.clippedT;
 
    for (Mesh* mesh : clipped) {
 
      std::span<const uint32_t> indexes = mesh->getIndexes();
      auto tris = indexes.size() / 3;
 
      auto [pData, pSize, pStride] = mesh->getPositionStream();
      assert(pStride == sizeof(glm::vec3));
      const glm::vec3* positions = (glm::vec3*)pData;
 
      auto [nData, nSize, nStride] = mesh->getSemanticStream(Cogs::ElementSemantic::Normal, Cogs::DataFormat::X32Y32Z32_FLOAT);
      const glm::vec3* normals = (glm::vec3*)nData;
 
      auto [tData, tSize, tStride] = mesh->getSemanticStream(Cogs::ElementSemantic::TextureCoordinate, Cogs::DataFormat::X32Y32_FLOAT);
      const glm::vec2* texCoords = (glm::vec2*)tData;
 
 
      // Translate clip-box to origin of geometry
      float clipBox[6] = { geoClip.min.x, geoClip.min.y, geoClip.min.z, geoClip.max.x, geoClip.max.y, geoClip.max.z };
 
      auto clipBoxMin = _mm_set_ps(0.0f, geoClip.min.z, geoClip.min.y, geoClip.min.x);
      auto clipBoxMax = _mm_set_ps(0.0f, geoClip.max.z, geoClip.max.y, geoClip.max.x);
 
      // We start with three vertices, and for each half-plane, we trade either one or two vertices for two,
      // i.e., a maximally net increase of 1 vertex per clipping plane.
      clipA.reserve(3 + 6);
      clipB.reserve(3 + 6);
 
      uint32_t N_t = 0;
      for (unsigned t = 0; t < tris; t++) {
        glm::vec3 V_in[3] = {
          positions[indexes[t * 3 + 0]],
          positions[indexes[t * 3 + 1]],
          positions[indexes[t * 3 + 2]],
        };
 
        glm::vec3 N_in[3] = {
          normals[indexes[t * 3 + 0]],
          normals[indexes[t * 3 + 1]],
          normals[indexes[t * 3 + 2]],
        };
 
        glm::vec2 T_in[3] = {
          texCoords ? texCoords[indexes[t * 3 + 0]] : glm::vec2(0, 0),
          texCoords ? texCoords[indexes[t * 3 + 1]] : glm::vec2(0, 0),
          texCoords ? texCoords[indexes[t * 3 + 2]] : glm::vec2(0, 0),
        };
 
        unsigned masks[3] = { 0,0,0 };
        for (unsigned k = 0; k < 3; k++) {
          masks[k] = calculateClippingMask(V_in[k], clipBoxMin, clipBoxMax);
        }
 
        // When clipping against one halfplane, a triangle spawns maximally two triangles.
        // And with six halfplanes, an upper bound is 12 times the input triangles.
        if (clippedV.size() < 3 * N_t + 12) {
          clippedV.resize(3 * N_t + 10 * 1024);
          clippedN.resize(3 * N_t + 10 * 1024);
          clippedT.resize(3 * N_t + 10 * 1024);
        }
 
        auto V_dst = &clippedV[N_t * 3];
        auto N_dst = &clippedN[N_t * 3];
        auto T_dst = &clippedT[N_t * 3];
 
        if ((masks[0] & masks[1] & masks[2]) == 63) {
          V_dst[0] = V_in[0];
          V_dst[1] = V_in[1];
          V_dst[2] = V_in[2];
 
          N_dst[0] = N_in[0];
          N_dst[1] = N_in[1];
          N_dst[2] = N_in[2];
 
          T_dst[0] = T_in[0];
          T_dst[1] = T_in[1];
          T_dst[2] = T_in[2];
 
          N_t++;
        }
        else if ((masks[0] | masks[1] | masks[2]) == 63) {
          // partially inside, needs clipping
          N_t += SparseOctree::clipTriangle((float*)V_dst, (float*)N_dst, (float*)T_dst, clipBox, (float*)V_in, (float*)N_in, (float*)T_in);
        }
        else {
          // completely outside
        }
      }
 
      size_t begin = cellGeo.size();
 
      size_t numPos = N_t * 3;
      cellGeo.resize(begin + numPos);
 
      std::copy(clippedV.data(), clippedV.data() + numPos, cellGeo.pos.data() + begin);
      std::copy(clippedN.data(), clippedN.data() + numPos, cellGeo.nrm.data() + begin);
      std::copy(clippedT.data(), clippedT.data() + numPos, cellGeo.tex.data() + begin);
    }
  }
 
  void appendMeshes(Vertices& cellGeo,
                    const std::vector<Mesh*>& contained)
  {
    for (Mesh* mesh : contained) {
 
      auto [pData, pSize, pStride] = mesh->getPositionStream();
      assert(pStride == sizeof(glm::vec3));
      auto positions = (glm::vec3*)pData;
 
      auto indexes = mesh->getIndexes();
 
      if (!pData) continue;
 
      auto [nData, nSize, nStride] = mesh->getSemanticStream(Cogs::ElementSemantic::Normal, Cogs::DataFormat::X32Y32Z32_FLOAT);
      auto normals = (glm::vec3*)nData;
 
      auto [tData, tSize, tStride] = mesh->getSemanticStream(Cogs::ElementSemantic::TextureCoordinate, Cogs::DataFormat::X32Y32_FLOAT);
      auto texCoords = (glm::vec2*)tData;
 
      auto numPos = mesh->getCount();
      size_t begin = cellGeo.size();
 
      cellGeo.resize(begin + numPos);
 
      glm::vec3* pos = cellGeo.pos.data() + begin;
      glm::vec3* nor = cellGeo.nrm.data() + begin;
      glm::vec2* tex = cellGeo.tex.data() + begin;
 
      if (texCoords) {
        for (size_t i = 0; i < numPos; ++i) {
          pos[i] = positions[indexes[i]];
          nor[i] = normals[indexes[i]];
          tex[i] = texCoords[indexes[i]];
        }
      }
      else {
        for (size_t i = 0; i < numPos; ++i) {
          pos[i] = positions[indexes[i]];
          nor[i] = normals[indexes[i]];
        }
      }
    }
  }
 
 
  struct RationalReducerContext {
    std::vector<glm::vec3>& positions;
    std::vector<glm::vec2>& texCoords;
  };
 
  rr_bool_t rrExtractPrimitives(void* rrHandle, void* userdata)
  {
    RationalReducerContext& rrCtx = *reinterpret_cast<RationalReducerContext*>(userdata);
 
    size_t Nv = rrCtx.positions.size();
    for (size_t i = 0; i < Nv; i += 3) {
      size_t id = static_cast<size_t>(rrCtx.texCoords[i].x);
      rr_add_polygon(rrHandle, 3, &rrCtx.positions[i].x, (void*)id, nullptr, RR_REDUCABLE);
    }
    return TRUE;
  }
 
  float rrSameMaterial(void* triangle1data, void* triangle2data, void* /*userdata*/)
  {
    size_t tri1 = reinterpret_cast<size_t>(triangle1data);
    size_t tri2 = reinterpret_cast<size_t>(triangle2data);
    return tri1 == tri2 ? 0.0f : 1.0f;
  }
 
  rr_bool_t rrStuffPrimitives(void* rrHandle, void* userdata)
  {
    RationalReducerContext& rrCtx = *reinterpret_cast<RationalReducerContext*>(userdata);
 
    size_t Nt = std::max(0, rr_get_num_triangles(rrHandle));
    rrCtx.positions.resize(3 * Nt);
    rrCtx.texCoords.resize(3 * Nt);
    if (Nt) {
      for (size_t t = 0; t < Nt; t++) {
        rr_triangle_t* rrtri = rr_get_triangle(rrHandle, static_cast<int>(t));
        size_t id = reinterpret_cast<size_t>(rr_triangle_get_data(rrtri));
        for (size_t i = 0; i < 3; i++) {
          rr_vertex_t* vertex = rr_triangle_get_vertex(rrtri, static_cast<unsigned int>(i));
          std::memcpy(&rrCtx.positions[3 * t + i].x, rr_vertex_get_coord(vertex), 3 * sizeof(float));
          rrCtx.texCoords[3 * t + i] = glm::vec2(static_cast<float>(id), 0.f);
        }
      }
    }
    return TRUE;
  }
 
  void simplifyCellRationalReducer(CellProcessContext& ctx,
                                   std::vector<glm::vec3>& positions,
                                   std::vector<glm::vec2>& texCoords,
                                   float percentage)
  {
    if (positions.empty()) return;
 
    RationalReducerContext rrCtx{
      .positions = positions,
      .texCoords = texCoords
    };
 
    //size_t Nt1 = positions.size() / 3;
 
    void* rrHandle = rr_init(FALSE);
    rr_set_extract_primitives(rrHandle, rrExtractPrimitives, &rrCtx);
    rr_set_stuff_primitives(rrHandle, rrStuffPrimitives, &rrCtx);
    rr_set_same_material(rrHandle, rrSameMaterial, &rrCtx);
    rr_set_num_native_triangles(rrHandle, static_cast<int>(positions.size()/3));
    rr_set_epsilon(rrHandle, 0.1f);
    if (ctx.octree.info.edgeLengthWeight != 1.0f) {
      rr_set_edgelength_weight(rrHandle, ctx.octree.info.edgeLengthWeight);
    }
    rr_set_percentage(rrHandle, percentage);
 
    rr_status_t ret = rr_reduce(rrHandle);
    rr_end(rrHandle);
    //size_t Nt2 = positions.size() / 3;
 
    if (ret != RR_OK) {
      positions.clear();
      texCoords.clear();
      switch (ret) {
      case RR_ABORT: LOG_ERROR(logger, "RR_ABORT"); break;
      case RR_CALLBACK_ERROR: LOG_ERROR(logger, "RR_CALLBACK_ERROR"); break;
      case RR_ERROR: LOG_ERROR(logger, "RR_ERROR"); break;
      case RR_NODATA: LOG_ERROR(logger, "RR_NODATA"); break;
      case RR_NONUMTRIS: LOG_ERROR(logger, "RR_NONUMTRIS"); break;
      case RR_OK:      break;
      default:
        assert(false && "Illegal enum");
      }
    }
    //LOG_DEBUG(logger, "reduction=%.2f%%, Nt=%zu => %zu (%.2f%%)", percentage, Nt1, Nt2, (100.0 * Nt2) / Nt1);
  }
 
  void simplifyCellMeshOptimizer(CellWorkerContext& wCtx,
                                 std::vector<glm::vec3>& positions,
                                 std::vector<glm::vec2>& texCoords,
                                 float percentage,
                                 bool sloppy)
  {
    std::vector<uint32_t>& indices = wCtx.tmpIx2; indices.clear();
 
    {
      // Make vertices unique. Do not consider texcoords for this (we will let one unique vertex have a texcoord)
      std::vector<uint32_t>& map = wCtx.map;          map.clear();
      std::vector<uint32_t>& unique = wCtx.unique;    unique.clear();
      GeometryProcessing::uniqueVertexSubset(wCtx.ctx.octree.context, unique, map,
                                             glm::value_ptr(positions[0]), sizeof(glm::vec3),
                                             nullptr, 0,
                                             nullptr, 0,
                                             static_cast<uint32_t>(positions.size()));
 
      size_t vertexCount = unique.size();
      std::vector<glm::vec3>& tmpPos = wCtx.tmpPos; tmpPos.resize(vertexCount);
      std::vector<glm::vec2>& tmpTex = wCtx.tmpTex; tmpTex.resize(vertexCount);
      for (size_t i = 0; i < vertexCount; i++) {
        const size_t old = unique[i];
        tmpPos[i] = positions[old];
        tmpTex[i] = texCoords[old];
      }
      positions.swap(tmpPos);
      texCoords.swap(tmpTex);
      indices.swap(map);            // since initial mesh is un-indexed, indices are just the map table
    }
 
    { // Simplification reduces the amount if indices
 
      size_t targetIndexCount = static_cast<size_t>(std::floor(0.01f * percentage * indices.size()));
      float relativeTolerance = 1.f;
 
      size_t outputIndexCount = 0;
      std::vector<uint32_t>& tmpIx = wCtx.tmpIx;  tmpIx.resize(indices.size());
      if (sloppy) {
        outputIndexCount = meshopt_simplifySloppy(tmpIx.data(),
                                                  indices.data(), indices.size(),
                                                  glm::value_ptr(positions[0]), positions.size(), sizeof(positions[0]),
                                                  targetIndexCount, relativeTolerance, nullptr);
      }
      else {
        outputIndexCount = meshopt_simplify(tmpIx.data(),
                                            indices.data(), indices.size(),
                                            glm::value_ptr(positions[0]), positions.size(), sizeof(positions[0]),
                                            targetIndexCount, relativeTolerance, /*options*/0, nullptr);
      }
      //LOG_DEBUG(logger, "%zu -> %zu, percentage=%.2f", indices.size(), outputIndexCount, percentage);
      if (outputIndexCount < indices.size()) {
        tmpIx.resize(outputIndexCount);
        indices.swap(tmpIx);
      }
    }
 
    { // Create unindexed triangles with identical texcoords on the corners
      size_t vertexCount = indices.size();
      std::vector<glm::vec3>& tmpPos = wCtx.tmpPos; tmpPos.resize(vertexCount);
      std::vector<glm::vec2>& tmpTex = wCtx.tmpTex; tmpTex.resize(vertexCount);
 
      for (size_t i = 0; i + 2 < vertexCount; i += 3) {
        float t0 = texCoords[indices[i + 0]].x;
        float t1 = texCoords[indices[i + 1]].x;
        float t2 = texCoords[indices[i + 2]].x;
        float tex;
        if (t0 == t1 || t0 == t2) {
          tex = t0;
        }
        else if (t1 == t2) {
          tex = t1;
        }
        else {
          tex = t0;
        }
        for (size_t k = 0; k < 3; k++) {
          tmpPos[i + k] = positions[indices[i + k]];
          tmpTex[i + k] = glm::vec2(tex, 0.f);
        }
      }
      positions.swap(tmpPos);
      texCoords.swap(tmpTex);
    }
  }
 
  void simplifyCell(CellWorkerContext& wCtx,
                    Vertices& cellGeo,
                    std::vector<uint32_t>& cellGeoIndices)
  {
    CellProcessContext& ctx = wCtx.ctx;
 
    if (cellGeo.empty()) return;
    assert(cellGeoIndices.empty());  // Mesh are assumed to be non-indexed.
 
    {
      // Run reduction.
      //
      // Contents of positions, normals and texcoords will be replaced with simplified mesh. The
      // mesh is not indexed.
 
      size_t level = wCtx.ctx.level;
 
      SparseOctree::OctreeInfo& info = wCtx.ctx.octree.info;
 
      float reduction = wCtx.ctx.octree.inputs[level].reductionError;
      const uint32_t dropReductionLevelThreshold = info.dropReductionLevelThreshold;
      if (level >= dropReductionLevelThreshold && info.dropReductionThreshold != 0) {
        const uint32_t dropReductionThreshold = info.dropReductionThreshold;
        const uint32_t minReductionThreshold = info.minReductionThreshold;
        const uint32_t dropRange = dropReductionThreshold - minReductionThreshold;
        const size_t vertexCount = cellGeo.size();
        if (vertexCount < dropReductionThreshold) {
          float ratio = glm::clamp((float)(dropReductionThreshold - vertexCount) / (float)dropRange, 0.0f, 1.0f);
          reduction = glm::lerp(reduction, 0.0f, ratio);
        }
      }
      if (reduction <= 0.f) return;
 
      switch (wCtx.ctx.octree.info.levelReducer) {
      case SparseOctree::LevelReducer::RationalReducerVertexCount:
        simplifyCellRationalReducer(ctx, cellGeo.pos, cellGeo.tex, reduction);
        break;
      case SparseOctree::LevelReducer::MeshOptimizerVertexCount:
        simplifyCellMeshOptimizer(wCtx, cellGeo.pos, cellGeo.tex, reduction, false);
        break;
      case SparseOctree::LevelReducer::MeshOptimizerSloppyVertexCount:
        simplifyCellMeshOptimizer(wCtx, cellGeo.pos, cellGeo.tex, reduction, true);
        break;
      default:
        assert(false && "Unhandled switch case");
        break;
      }
 
      if (cellGeo.pos.empty()) {
        cellGeo.clear();
        cellGeoIndices.clear();
        return;
      }
    }
 
    {
      assert(cellGeoIndices.empty());
 
      std::vector<uint32_t>& map = wCtx.map;
      std::vector<uint32_t>& unique = wCtx.unique;
      std::vector<glm::vec3>& tmpPos = wCtx.tmpPos;
      std::vector<glm::vec2>& tmpTex = wCtx.tmpTex;
 
      // Convert mesh to indexed mesh.
      map.clear();
      unique.clear();
      GeometryProcessing::uniqueVertexSubset(wCtx.ctx.octree.context, unique, map,
                                             glm::value_ptr(cellGeo.pos[0]), sizeof(glm::vec3),
                                             nullptr, 0,
                                             glm::value_ptr(cellGeo.tex[0]), sizeof(glm::vec2),
                                             static_cast<uint32_t>(cellGeo.size()));
 
      // Create indexed meshes using the unique vertices
      size_t vertexCount = unique.size();
      tmpPos.resize(vertexCount);
      tmpTex.resize(vertexCount);
      for (size_t i = 0; i < vertexCount; i++) {
        const size_t old = unique[i];
        tmpPos[i] = cellGeo.pos[old];
        tmpTex[i] = cellGeo.tex[old];
      }
 
      cellGeo.pos.swap(tmpPos);
      cellGeo.tex.swap(tmpTex);
      cellGeoIndices.swap(map);            // since initial mesh is un-indexed, indices are just the map table
    }
    {
      // Calculate new normal vectors.
      //
      // Some vertices are duplicated along feature angles, hence we need a subsequent round of remapping.
 
      std::vector<uint32_t>& unique = wCtx.unique;    unique.clear();
      std::vector<uint32_t>& tmpIx = wCtx.tmpIx;      tmpIx.clear();
 
      cellGeo.nrm.clear();
      GeometryProcessing::normalsFromIndexedTriangles(wCtx.ctx.octree.context, cellGeo.nrm, unique, tmpIx,
                                                      glm::value_ptr(cellGeo.pos[0]), sizeof(cellGeo.pos[0]), static_cast<uint32_t>(cellGeo.pos.size()),
                                                      cellGeoIndices.data(), static_cast<uint32_t>(cellGeoIndices.size()),
                                                      wCtx.ctx.featureAngle,
                                                      wCtx.ctx.protrusionAngle,
                                                      true);
 
      // map positions and texcoords to match new ordering (vertices might have been duplicated and feature edges)
      size_t vertexCount = unique.size();
      assert(cellGeo.nrm.size() == vertexCount);
 
      std::vector<glm::vec3>& tmpPos = wCtx.tmpPos; tmpPos.resize(vertexCount);
      std::vector<glm::vec2>& tmpTex = wCtx.tmpTex; tmpTex.resize(vertexCount);
      for (size_t i = 0; i < vertexCount; i++) {
        const size_t old = unique[i];
        tmpPos[i] = cellGeo.pos[old];
        tmpTex[i] = cellGeo.tex[old];
      }
 
      cellGeo.pos.swap(tmpPos);
      cellGeo.tex.swap(tmpTex);
      cellGeoIndices.swap(tmpIx);
    }
  }
 
 
  void pushTriangle(Vertices& out, const glm::vec3* p, const glm::vec3* n, const glm::vec2* t)
  {
    for (size_t i = 0; i < 3; i++) {
      out.pos.emplace_back(p[i]);
      out.nrm.emplace_back(n[i]);
      out.tex.emplace_back(t[i]);
    }
  }
 
  void pushSplitTriangle(Vertices& out014, Vertices& out1234, const glm::vec2& tex,
                         const glm::vec3& p0, const glm::vec3& p1, const glm::vec3& p2, const glm::vec3& p3, const glm::vec3& p4,
                         const glm::vec3& n0, const glm::vec3& n1, const glm::vec3& n2, const glm::vec3& n3, const glm::vec3& n4)
  {
    out014.pos.push_back(p0); out014.nrm.push_back(n0); out014.tex.push_back(tex);
    out014.pos.push_back(p1); out014.nrm.push_back(n1); out014.tex.push_back(tex);
    out014.pos.push_back(p4); out014.nrm.push_back(n4); out014.tex.push_back(tex);
 
    out1234.pos.push_back(p1); out1234.nrm.push_back(n1); out1234.tex.push_back(tex);
    out1234.pos.push_back(p2); out1234.nrm.push_back(n2); out1234.tex.push_back(tex);
    out1234.pos.push_back(p3); out1234.nrm.push_back(n3); out1234.tex.push_back(tex);
 
    out1234.pos.push_back(p4); out1234.nrm.push_back(n4); out1234.tex.push_back(tex);
    out1234.pos.push_back(p1); out1234.nrm.push_back(n1); out1234.tex.push_back(tex);
    out1234.pos.push_back(p3); out1234.nrm.push_back(n3); out1234.tex.push_back(tex);
  }
 
  float finiteOrZero(float x) {
    return std::isfinite(x) ? x : 0.f;
  }
 
  void splitGeometry(CellProcessContext& ctx, Vertices& outA, Vertices& outB, const Vertices& in, const glm::vec4& plane)
  {
    const float epsilon = ctx.clipEpsilon;
    const size_t count = in.size();
    for (size_t i = 0; i + 3 <= count; i += 3) {
      const glm::vec3* P = &in.pos[i];
      const glm::vec3* N = &in.nrm[i];
      const glm::vec2 T = in.tex[i];
      float d0 = dot(P[0], glm::vec3(plane)) + plane.w;
      float d1 = dot(P[1], glm::vec3(plane)) + plane.w;
      float d2 = dot(P[2], glm::vec3(plane)) + plane.w;
 
      // Expected common case: Triangle lies entirely inside our outside the clip plane.
      // We add a little slack on the test so that triangles that barely pierces the clip plane
      // isn't clipped. The point of that is to avoid creating a lot of tiny triangles at the
      // cost of a slightly wiggly clipping, yielding a slightly bigger bounding box.
 
      if (-epsilon <= std::min(d0, std::min(d1, d2))) {           // situation == 0b000
        pushTriangle(outB, &in.pos[i], &in.nrm[i], &in.tex[i]);
      }
      else if (std::max(d0, std::max(d1, d2)) <= epsilon) {       // situation == 0b111
        pushTriangle(outA, &in.pos[i], &in.nrm[i], &in.tex[i]);
      }
      else {
 
        // Involved case: Triangle intersects the clip plane. One side gets a triangle
        // and the other gets a quad (two triangles).
        const int situation = (d0 < 0.f ? 1 : 0) + (d1 < 0.f ? 2 : 0) + (d2 < 0.f ? 4 : 0);
 
        float w_01 = finiteOrZero(std::abs(d0) / (std::abs(d0) + std::abs(d1)));
        glm::vec3 p_01 = (1.f - w_01) * P[0] + w_01 * P[1];
        glm::vec3 n_01 = (1.f - w_01) * N[0] + w_01 * N[1];
 
        float w_12 = finiteOrZero(std::abs(d1) / (std::abs(d1) + std::abs(d2)));
        glm::vec3 p_12 = (1.f - w_12) * P[1] + w_12 * P[2];
        glm::vec3 n_12 = (1.f - w_12) * N[1] + w_12 * N[2];
 
        float w_20 = finiteOrZero(std::abs(d2) / (std::abs(d2) + std::abs(d0)));
        glm::vec3 p_20 = (1.f - w_20) * P[2] + w_20 * P[0];
        glm::vec3 n_20 = (1.f - w_20) * N[2] + w_20 * N[0];
 
        switch (situation) {
        case 0b001:
          pushSplitTriangle(outA, outB, T,
                            P[0], p_01, P[1], P[2], p_20,
                            N[0], n_01, N[1], N[2], n_20);
          break;
        case 0b010:
          pushSplitTriangle(outA, outB, T,
                            P[1], p_12, P[2], P[0], p_01,
                            N[1], n_12, N[2], N[0], n_01);
          break;
        case 0b011:
          pushSplitTriangle(outB, outA, T,
                            P[2], p_20, P[0], P[1], p_12,
                            N[2], n_20, N[0], N[1], n_12);
          break;
        case 0b100:
          pushSplitTriangle(outA, outB, T,
                            P[2], p_20, P[0], P[1], p_12,
                            N[2], n_20, N[0], N[1], n_12);
          break;
        case 0b101:
          pushSplitTriangle(outB, outA, T,
                            P[1], p_12, P[2], P[0], p_01,
                            N[1], n_12, N[2], N[0], n_01);
          break;
        case 0b110:
          pushSplitTriangle(outB, outA, T,
                            P[0], p_01, P[1], P[2], p_20,
                            N[0], n_01, N[1], N[2], n_20);
          break;
        default:
          // assert(false); // Should never happen.
          // TODO: Fix this assert
          // ChrisDy: I think commenting out the assert temporarily might be fine (and just discarding the triangle).
          // I would like to figure out why the nan (if it is a nan) appears.
          break;
        }
      }
    }
  }
 
  void writeModelFile(CellProcessContext& ctx, const std::string& path, std::span<MeshHandle> meshes)
  {
    size_t N = meshes.size();
 
    Model model{};
 
    model.meshes.assign(meshes.begin(), meshes.end());
 
    if (ctx.octree.info.colorizeLevels) {
      model.materials.emplace_back(ctx.materialInstance);
    }
 
    model.parts.resize(N);
    for (size_t i = 0; i < N; i++) {
      ModelPart& part = model.parts[i];
      part.parentIndex = NoParentPart;
      part.boundsIndex = NoIndex;
      part.meshIndex = static_cast<uint32_t>(i);
      if (ctx.octree.info.colorizeLevels) {
        part.materialIndex = 0;
      }
    }
 
    uint32_t numIndexes;
    uint32_t numVertexes;
    bool rv = writeModel(ctx.octree.context, numIndexes, numVertexes, path, &model, &ctx.settings);
    assert(rv);
  }
 
  void recordMesh(CellWorkerContext& wCtx, SparseOctree::Cell& cell, const Vertices& cellGeo, const std::vector<uint32_t>& cellGeoIndices = std::vector<uint32_t>())
  {
    CellProcessContext& ctx = wCtx.ctx;
 
    // Calculate bounds
    glm::vec3 bbMin = glm::vec3(std::numeric_limits<float>::max());
    glm::vec3 bbMax = -glm::vec3(std::numeric_limits<float>::max());
    for (const glm::vec3& p : cellGeo.pos) {
      bbMin = glm::min(bbMin, p);
      bbMax = glm::max(bbMax, p);
    }
 
    // Create mesh
    size_t meshVertexCount = 0;
    MeshHandle mesh = ctx.octree.context->meshManager->create();
    mesh->setPositions(cellGeo.pos);
    mesh->setNormals(cellGeo.nrm);
    mesh->setTexCoords(cellGeo.tex);
    if (cellGeoIndices.empty()) {
      meshVertexCount = cellGeo.size();
    }
    else {
      meshVertexCount = cellGeoIndices.size();
      mesh->setIndexes(cellGeoIndices);
    }
 
    // If mesh is considered small, try to stuff it into a shared cogsbin file
    if (ctx.maxModelVertexCount && (meshVertexCount < ctx.maxModelVertexCount)) {
      cell.meshRecs.emplace_back(SparseOctree::MeshRec{ .mesh = std::move(mesh), .bbox = BoundingBox{ bbMin, bbMax } });
    }
 
    // Otherwise mesh is considered large, and it will get its own cogsbin file.
    else {
 
      // File paths
      std::string stem = "S_" + std::to_string(ctx.fileCount.fetch_add(1)) + ".cogsbin";
      std::string absolutePath = Cogs::IO::combine(ctx.filenamePrefix, stem);
      std::string relativePath = Cogs::IO::combine(ctx.relativePrefix, stem);
 
      writeModelFile(wCtx.ctx, absolutePath, std::span<MeshHandle>(&mesh, 1));
 
      cell.meshRefs.emplace_back(SparseOctree::MeshRef{ .fileName = relativePath, .bbox = BoundingBox{ bbMin, bbMax } });
    }
  }
 
  void createCellMesh(CellWorkerContext& wCtx,
                      SparseOctree::Cell& cell,
                      const Vertices& cellGeo,
                      const std::vector<uint32_t>& cellGeoIndices)
  {
    assert(cellGeo.pos.size() == cellGeo.nrm.size());
    assert(cellGeo.pos.size() == cellGeo.tex.size());
    if (cellGeo.pos.empty()) return;
 
    CellProcessContext& ctx = wCtx.ctx;
 
    size_t vertexCount = cellGeoIndices.empty() ? cellGeo.size() : cellGeoIndices.size();
    cell.vertexCount = static_cast<uint32_t>(vertexCount);
    cell.sizeEstimate = static_cast<uint32_t>(2 * sizeof(glm::vec3) + sizeof(glm::vec2) * cellGeo.pos.size() +
                                              sizeof(uint32_t) * cellGeoIndices.size());
 
 
    // If the mesh is small enough, we won't split it.
    if (ctx.maxMeshVertexCount == 0 || vertexCount < ctx.maxMeshVertexCount) {
      recordMesh(wCtx, cell, cellGeo, cellGeoIndices);
      return;
    }
 
    // Initialize the stack of meshes to split with the cell's mesh.
    // For simplicity, we convert it to non-indexed. It will be indexed by repackaging.
    std::vector<SplitStackItem> splitStack;
    if (cellGeoIndices.empty()) {
      splitStack.push_back({ .geo = cellGeo });
    }
    else {
      splitStack.emplace_back();
      const size_t N = cellGeoIndices.size();
      Vertices& out = splitStack.back().geo;
      out.resize(N);
      for (size_t i = 0; i < N; i++) {
        out.pos[i] = cellGeo.pos[cellGeoIndices[i]];
        out.nrm[i] = cellGeo.nrm[cellGeoIndices[i]];
        out.tex[i] = cellGeo.tex[cellGeoIndices[i]];
      }
    }
 
    // Continue until the set of meshes to be split is empty.
    while (!splitStack.empty()) {
      SplitStackItem item = std::move(splitStack.back());
      splitStack.pop_back();
 
      // Calculate the actual bounds of mesh, used to suggest clipping planes.
      // 
      // Note: We do not shrink the cell bounding box to actual mesh bounds on purpose as the cell
      // bounding box is used for lod-calcuations of the cell *including* empty space. 
      glm::vec3 bbMin = glm::vec3(std::numeric_limits<float>::max());
      glm::vec3 bbMax = glm::vec3(-std::numeric_limits<float>::max());
      for (const glm::vec3& p : item.geo.pos) {
        bbMin = glm::min(bbMin, p);
        bbMax = glm::max(bbMax, p);
      }
 
      // Recycle split meshes to reduce memory allocs
      Splits& splits = wCtx.splits;
      for (Split& split : splits) {
        split.sides[0].clear();
        split.sides[1].clear();
      }
 
      // Try three splits (YZ XZ and XY planes through bb center) and choose the best valid split
      int bestIx = -1;
      size_t countIn = item.geo.size();
      size_t bestCount = std::numeric_limits<size_t>::max();
      for (int k = 0; k < 3; k++) {
        glm::vec4 plane(0.f, 0.f, 0.f, -0.5f * (bbMin[k] + bbMax[k]));
        plane[k] = 1.f;
        splitGeometry(ctx, splits[k].sides[0], splits[k].sides[1], item.geo, plane);
 
        size_t countA = splits[k].sides[0].size();
        size_t countB = splits[k].sides[1].size();
        size_t count = countA + countB;
 
        // Update best split that are considered a valid split.
        if (0 < countA &&
            0 < countB &&
            countA < countIn &&
            countB < countIn &&
            count < bestCount)
        {
          bestCount = count;
          bestIx = k;
        }
      }
 
      if (bestIx < 0) {
        // Failed to split, just give up
        recordMesh(wCtx, cell, item.geo);
        continue;
      }
 
      Split& bestSplit = splits[bestIx];
      for (size_t k = 0; k < 2; k++) {
        Vertices& side = bestSplit.sides[k];
 
        // Resulting side is small enough, store it
        if (side.size() < ctx.maxMeshVertexCount || ctx.maxSplitDepth <= item.depth) {
          recordMesh(wCtx, cell, side);
        }
 
        // Still big, continue to try splitting it.
        else {
          splitStack.push_back({ .geo = std::move(side), .depth = item.depth + 1 });
        }
      }
    }
  }
 
  void processCellWorkerFunc(CellProcessContext* ctx_, size_t /*id*/)
  {
    CellWorkerContext wCtx{
      .ctx = *ctx_
    };
    CellProcessContext& ctx = wCtx.ctx;
 
    std::vector<Mesh*> clipped;
    std::vector<Mesh*> contained;
    Vertices cellGeo;
    SharedModel modelReadyForWriting;
 
    std::vector<uint32_t> cell_indices;
 
    const size_t cellCount = ctx.cellCount;
 
    while (true) {
      size_t item = ctx.processedCells.fetch_add(1);
      if (ctx.cellCount <= item) break;
 
      // Process a cell
      {
        SparseOctree::Cell& cell = *ctx.workItems[item].cell;
 
        clipped.clear();
        contained.clear();
        filterCells(clipped, contained, cell.bbox, ctx.scene, cell.touches);
 
        cellGeo.clear();
 
        cell_indices.clear();
        clipMeshes(wCtx, cellGeo, cell.bbox, clipped);
        appendMeshes(cellGeo, contained);
        simplifyCell(wCtx, cellGeo, cell_indices);
        createCellMesh(wCtx, cell, cellGeo, cell_indices);
 
        // workItems[item] is now considered done. Will be tagged as done
        // when we have the lock below.
      }
 
      {
        auto& writeCells = ctx.writeCells;
        std::unique_lock guard(writeCells.lock);
 
        // Tag this cell as done.
        assert(writeCells.cellsDone[item] == false);
        writeCells.cellsDone[item] = true;
 
        // Try to write as many cells as possible
        while (writeCells.cellsWritten < cellCount && writeCells.cellsDone[writeCells.cellsWritten]) {
 
          SharedModel& sharedModel = ctx.writeCells.model;
 
          if (!ctx.workItems[writeCells.cellsWritten].cell->meshRecs.empty()) {
            SparseOctree::Cell& cell = *ctx.workItems[writeCells.cellsWritten].cell;
 
            // Current cell has still meshes, pop one
            SparseOctree::MeshRec meshRec = std::move(cell.meshRecs.back());
            cell.meshRecs.pop_back();
 
            // Check if we should flush the current shared model first
            size_t vertexCount = meshRec.mesh->getCount();
            if (!sharedModel.meshes.empty())
            {
              bool tooManyVerticesInFile = ctx.maxModelVertexCount < sharedModel.vertexCount + vertexCount;
              bool tooManyMeshesInFile = ctx.maxModelMeshCount && (ctx.maxModelMeshCount < (sharedModel.meshes.size() + 1));
              if (tooManyVerticesInFile || tooManyMeshesInFile) {
                std::swap(sharedModel, modelReadyForWriting);
                assert(sharedModel.meshes.empty() && sharedModel.absolutePath.empty() && sharedModel.relativePath.empty() && sharedModel.vertexCount == 0);
              }
            }
 
            // Check if we should set up a new shared model file
            if (sharedModel.meshes.empty()) {
              std::string stem = "M_" + std::to_string(ctx.fileCount.fetch_add(1)) + ".cogsbin";
              sharedModel.absolutePath = Cogs::IO::combine(ctx.filenamePrefix, stem);
              sharedModel.relativePath = Cogs::IO::combine(ctx.relativePrefix, stem);
            }
 
            // Push mesh into shared model
            uint32_t part = static_cast<uint32_t>(sharedModel.meshes.size());
            sharedModel.meshes.emplace_back(std::move(meshRec.mesh));
            sharedModel.vertexCount += vertexCount;
 
            // Record mesh reference
            cell.meshRefs.emplace_back(SparseOctree::MeshRef{ .fileName = sharedModel.relativePath, .part = part, .bbox = meshRec.bbox });
          }
          else {
            // No more meshes in this cell, jump to next cell.
            writeCells.cellsWritten++;
 
            // This concludes the last cell.
            // - we have the lock, so no other threads are pushing meshes
            // - this is the last iteration of this loop for any thread
            if (writeCells.cellsWritten == cellCount) {
 
              // Flush out remainig models
              if (!sharedModel.meshes.empty()) {
                std::swap(modelReadyForWriting, sharedModel);
              }
            }
          }
 
          // Write mesh to disk
          if (!modelReadyForWriting.meshes.empty()) {
            guard.unlock();
 
            // Do write file. Release lock and let other threads store meshes in the meantime
            writeModelFile(ctx, modelReadyForWriting.absolutePath, std::span<MeshHandle>(modelReadyForWriting.meshes));
            modelReadyForWriting.meshes.clear();
            modelReadyForWriting.relativePath.clear();
            modelReadyForWriting.absolutePath.clear();
            modelReadyForWriting.vertexCount = 0;
 
            guard.lock();
          }
        }
      }
    }
  }
 
}
 
 
void Cogs::Core::SparseOctree::distributeGeometryWithIds(Octree & octree, PropertyStore& properties, const size_t level)
{
  CellProcessContext ctx{
    .octree = octree,
    .grid = octree.grids[level],
    .scene = octree.scenes[level],
    .relativePrefix = Cogs::IO::combine(octree.info.name, std::to_string(level)),
    .filenamePrefix = IO::combine(octree.info.filesDirectory, std::to_string(level)),
    .level = level,
    .cellCount = octree.grids[level].cellStore.size(),
    .processedCells = 0,
    .maxMeshVertexCount = static_cast<size_t>(octree.info.maxMeshVertexCount < 1 ? 0 : std::max(1000, octree.info.maxMeshVertexCount)),
    .maxModelVertexCount = static_cast<size_t>(std::max(0, octree.info.maxModelVertexCount)),
    .maxModelMeshCount = static_cast<size_t>(std::max(0, octree.info.maxModelMeshCount))
  };
 
  if (ctx.octree.info.useCompression) {
    ctx.settings.flags |= WriteModelFlags::COMPRESS_ZSTD;
    ctx.settings.compressionLevel = octree.info.compressionLevel;
  }
  ctx.featureAngle = properties.getProperty("featureAngle", ctx.featureAngle);
 
  IO::createDirectories(IO::combine(ctx.filenamePrefix, "dummy"));
 
  if (octree.info.colorizeLevels) {
    MaterialHandle material = octree.context->materialManager->getDefaultMaterial();
    ctx.materialInstance = octree.context->materialInstanceManager->createMaterialInstance(material);
#if 1
    float hue = glm::fract(1.61803398875f * level);
    ctx.materialInstance->setVec4Property(material->getVec4Key("diffuseColor"), glm::vec4(glm::rgbColor(glm::vec3(360.f * hue, 1.f, 1.f)), 1.f));
#else
    ctx.materialInstance->setVec4Property(material->getVec4Key("diffuseColor"),
                                          glm::vec4(((level >> 2) & 1) ? 1.f : 0.5f,
                                                    ((level >> 1) & 1) ? 1.f : 0.5f,
                                                    ((level >> 0) & 1) ? 1.f : 0.5f,
                                                    1.f));
#endif
  }
 
  // Order processing of cells in Z-order so adjacent cells are processed in the same time,
  // and we can pack them early
  {
    Cogs::Timer timer = Cogs::Timer::startNew();
    ctx.writeCells.cellsDone.resize(ctx.cellCount, false);
    ctx.workItems.reserve(ctx.cellCount);
    for (size_t i = 0; i < ctx.cellCount; i++) {
      Cell& cell = ctx.grid.cellStore[i];
      ctx.workItems.emplace_back(WorkItem{ .sortKey = mortonCode(static_cast<uint16_t>(cell.x),
                                            static_cast<uint16_t>(cell.y),
                                            static_cast<uint16_t>(cell.z)),
                                 .cell = &cell });
    }
    std::sort(ctx.workItems.begin(), ctx.workItems.end(), [](const WorkItem& a, const WorkItem& b) -> bool {
      if (a.sortKey < b.sortKey) return true;
      else if (b.sortKey < a.sortKey) return false;
      else if (a.cell->x < b.cell->x) return true;
      else if (b.cell->x < a.cell->x) return false;
      else if (a.cell->y < b.cell->y) return true;
      else if (b.cell->y < a.cell->y) return false;
      else if (a.cell->z < b.cell->z) return true;
      else if (b.cell->z < a.cell->z) return false;
      else return true;
    });
 
    LOG_DEBUG(logger, "Level %zu: Sorting occupied cells into Z-order: %.2fs ", level, timer.elapsedSeconds());
  }
 
  {
    Cogs::Timer timer = Cogs::Timer::startNew();
    size_t hwc = std::min(ctx.cellCount, static_cast<size_t>(std::thread::hardware_concurrency()));
    std::vector<std::thread> workers;
    workers.reserve(hwc);
    for (size_t i = 0; i < hwc; i++) {
      workers.emplace_back(std::thread(processCellWorkerFunc, &ctx, i));
    }
 
    while (ctx.processedCells.load() < ctx.cellCount) {
      std::this_thread::sleep_for(std::chrono::milliseconds(500));
      Cogs::Core::runFrames(ctx.octree.context, 1);
    }
 
    for (std::thread& thread : workers) {
      thread.join();
    }
    workers.clear();
    LOG_DEBUG(logger, "Level %zu: Clipping, processing and writing models: %.2fs, cells=%zu, threads=%zu ", level, timer.elapsedSeconds(), ctx.cellCount, hwc);
  }
 
}
 
uint32_t Cogs::Core::SparseOctree::clipTriangle(float* V_dst, float *N_dst, float *T_dst, const float (&clipBox)[6], const float *V_in, const float * N_in, const float *T_in)
{
  auto * tmp_a = &clipA;
  auto * tmp_b = &clipB;
 
  // Create a line loop from the triangle
  for (unsigned k = 0; k < 3 * 3; k++) {
    tmp_a->V[k] = V_in[k];
    tmp_a->N[k] = N_in[k];
  }
  for (unsigned k = 0; k < 3 * 2; k++) {
    tmp_a->T[k] = T_in[k];
  }
  auto N_a = 3u;
 
  // Both line loop and half-space are convex, so the intersection of the two is convex as well
  // If a point is inside the half-space, we keep it, otherwise we discard it.
  // Between two points that are inside and outside, we interpolate to find the intersection and insert it.
  for (unsigned o = 0; o < 2; o++) {
    for (unsigned a = 0; a < 3; a++) {
 
      auto N_b = 0u;
      unsigned ip = N_a - 1;
 
      // signed distance for previous point
      float dp = (o == 0 ? -1.f : 1.f) * (tmp_a->V[3 * ip + a] - clipBox[3 * o + a]);
 
      for (unsigned in = 0; in < N_a; in++) {
 
        // signed distance for next point
        float dn = (o == 0 ? -1.f : 1.f) * (tmp_a->V[3 * in + a] - clipBox[3 * o + a]);
 
        bool dp_neg = dp < 0.f;
        bool dn_neg = dn < 0.f;
 
        // If sign changes from previous point to next point, insert intersection
        if (dp_neg != dn_neg) {
 
          // Weights
          auto wa = std::abs(dn / (dp - dn));
          auto wb = std::abs(dp / (dp - dn));
          if (!std::isfinite(wa) || !std::isfinite(wb)) {
            wa = wb = 0.5f;
          }
 
          // Blend
          for (unsigned k = 0; k < 3; k++) {
            tmp_b->V[3 * N_b + k] = wa * tmp_a->V[3 * ip + k] + wb * tmp_a->V[3 * in + k];
            tmp_b->N[3 * N_b + k] = wa * tmp_a->N[3 * ip + k] + wb * tmp_a->N[3 * in + k];
 
          }
          for (unsigned k = 0; k < 2; k++) {
            tmp_b->T[2 * N_b + k] = tmp_a->T[2 * ip + k];
          }
          N_b++;
          assert(N_b <= 9);
        }
 
        // If next point is inside half-space, keep it.
        if (dn_neg) {
          for (unsigned k = 0; k < 3; k++) {
            tmp_b->V[3 * N_b + k] = tmp_a->V[3 * in + k];
            tmp_b->N[3 * N_b + k] = tmp_a->N[3 * in + k];
          }
          for (unsigned k = 0; k < 2; k++) {
            tmp_b->T[2 * N_b + k] = tmp_a->T[2 * in + k];
          }
          N_b++;
          assert(N_b <= 9);
        }
 
        dp = dn;
        ip = in;
      }
 
      // If we have less than three points, it is degenerate or empty, just bail out.
      if (N_b < 3) {
        return 0;
      }
 
      // and swap
      N_a = N_b;
      std::swap(tmp_a, tmp_b);
    }
  }
 
  // Since the loop is convex, triangulating it is trivial, we just form
  // triangles from all edges not touching point 0 and use point 0 as apex.
  for (unsigned i = 1; i + 1 < N_a; i++) {
 
    for (unsigned k = 0; k < 3; k++) {
      *V_dst++ = tmp_a->V[3 * (0) + k];
      *N_dst++ = tmp_a->N[3 * (0) + k];
    }
    for (unsigned k = 0; k < 2; k++) {
      *T_dst++ = tmp_a->T[2 * (0) + k];
    }
    for (unsigned k = 0; k < 3; k++) {
      *V_dst++ = tmp_a->V[3 * (i + 0) + k];
      *N_dst++ = tmp_a->N[3 * (i + 0) + k];
    }
    for (unsigned k = 0; k < 2; k++) {
      *T_dst++ = tmp_a->T[2 * (i + 0) + k];
    }
    for (unsigned k = 0; k < 3; k++) {
      *V_dst++ = tmp_a->V[3 * (i + 1) + k];
      *N_dst++ = tmp_a->N[3 * (i + 1) + k];
    }
    for (unsigned k = 0; k < 2; k++) {
      *T_dst++ = tmp_a->T[2 * (i + 1) + k];
    }
  }
 
  return N_a - 2;
}
 
uint32_t Cogs::Core::SparseOctree::clipTriangle(float * V_dst, float * N_dst, const float (&clipBox)[6], const float * V_in, const float * N_in)
{
  auto * tmp_a = &clipA;
  auto * tmp_b = &clipB;
 
  // Create a line loop from the triangle
  for (unsigned k = 0; k < 3 * 3; k++) {
    tmp_a->V[k] = V_in[k];
    tmp_a->N[k] = N_in[k];
  }
  auto N_a = 3u;
 
  // Both line loop and half-space are convex, so the intersection of the two is convex as well
  // If a point is inside the half-space, we keep it, otherwise we discard it.
  // Between two points that are inside and outside, we interpolate to find the intersection and insert it.
  for (unsigned o = 0; o < 2; o++) {
    for (unsigned a = 0; a < 3; a++) {
 
      auto N_b = 0u;
      unsigned ip = N_a - 1;
 
      // signed distance for previous point
      float dp = (o == 0 ? -1.f : 1.f) * (tmp_a->V[3 * ip + a] - clipBox[3 * o + a]);
 
      for (unsigned in = 0; in < N_a; in++) {
 
        // signed distance for next point
        float dn = (o == 0 ? -1.f : 1.f) * (tmp_a->V[3 * in + a] - clipBox[3 * o + a]);
 
        bool dp_neg = dp < 0.f;
        bool dn_neg = dn < 0.f;
 
        // If sign changes from previous point to next point, insert intersection
        if (dp_neg != dn_neg) {
 
          // Weights
          auto wa = std::abs(dn / (dp - dn));
          auto wb = std::abs(dp / (dp - dn));
          if (!std::isfinite(wa) || !std::isfinite(wb)) {
            wa = wb = 0.5f;
          }
 
          // Blend
          for (unsigned k = 0; k < 3; k++) {
            tmp_b->V[3 * N_b + k] = wa * tmp_a->V[3 * ip + k] + wb * tmp_a->V[3 * in + k];
            tmp_b->N[3 * N_b + k] = wa * tmp_a->N[3 * ip + k] + wb * tmp_a->N[3 * in + k];
 
          }
          N_b++;
          assert(N_b <= 9);
        }
 
        // If next point is inside half-space, keep it.
        if (dn_neg) {
          for (unsigned k = 0; k < 3; k++) {
            tmp_b->V[3 * N_b + k] = tmp_a->V[3 * in + k];
            tmp_b->N[3 * N_b + k] = tmp_a->N[3 * in + k];
          }
          N_b++;
          assert(N_b <= 9);
        }
 
        dp = dn;
        ip = in;
      }
 
      // If we have less than three points, it is degenerate or empty, just bail out.
      if (N_b < 3) {
        return 0;
      }
 
      // and swap
      N_a = N_b;
      std::swap(tmp_a, tmp_b);
    }
  }
 
  // Since the loop is convex, triangulating it is trivial, we just form
  // triangles from all edges not touching point 0 and use point 0 as apex.
  for (unsigned i = 1; i + 1 < N_a; i++) {
 
    for (unsigned k = 0; k < 3; k++) {
      *V_dst++ = tmp_a->V[3 * (0) + k];
      *N_dst++ = tmp_a->N[3 * (0) + k];
    }
    for (unsigned k = 0; k < 3; k++) {
      *V_dst++ = tmp_a->V[3 * (i + 0) + k];
      *N_dst++ = tmp_a->N[3 * (i + 0) + k];
    }
    for (unsigned k = 0; k < 3; k++) {
      *V_dst++ = tmp_a->V[3 * (i + 1) + k];
      *N_dst++ = tmp_a->N[3 * (i + 1) + k];
    }
  }
 
  return N_a - 2;
}
 
#else
 
namespace Cogs::Core::SparseOctree
{
  void distributeGeometryWithIds(Octree & /*octree*/, PropertyStore& /*properties*/, const size_t /*level*/) {}
 
  uint32_t clipTriangle(float* V_dst, float *N_dst, float *T_dst, const float(&clipBox)[6], const float *V_in, const float * N_in, const float *T_in) { return 0; }
  uint32_t clipTriangle(float* V_dst, float *N_dst, const float(&clipBox)[6], const float *V_in, const float * N_in) { return 0; }
}
 
#endif