PackMeshCommand.cpp

#include "Foundation/Logging/Logger.h"
 
#include "Components/Core/MeshComponent.h"
#include "Components/Core/SceneComponent.h"
#include "Components/Core/TransformComponent.h"
 
#include "Systems/Core/TransformSystem.h"
 
#include "Resources/Mesh.h"
#include "Resources/MeshManager.h"
#include "Resources/VertexFormats.h"
 
#include "EntityStore.h"
#include "Context.h"
 
#include "PackMeshCommand.h"
 
#include <meshoptimizer/meshoptimizer.h>
 
 
namespace {
  using namespace Cogs::Core;
  const Cogs::Logging::Log logger = Cogs::Logging::getLogger("PackMeshCommand");
 
  void handle(Context* context, PackMeshCommand* cmd, EntityPtr entity)
  {
    // Recurse into children
    if (SceneComponent* sceneComp = entity->getComponent<SceneComponent>(); sceneComp && !sceneComp->children.empty()) {
      for (EntityPtr& child : sceneComp->children) {
        handle(context, cmd, child);
      }
    }
 
    // Handle mesh
    if (MeshComponent* meshComp = entity->getComponent<MeshComponent>(); meshComp && meshComp->meshHandle) {
      cmd->undoItems.emplace_back(PackMeshCommand::UndoItem{ entity, meshComp->meshHandle });
 
      glm::mat4 transform(1.f);
 
      std::string status;
      if (!PackMeshCommand::pack(context, status, transform, meshComp->meshHandle, cmd->packOptions)) {
        LOG_ERROR(logger, "Faild to pack mesh: %s", status.c_str());
      }
      else {
        if (!status.empty()) {
          LOG_DEBUG(logger, "%s", status.c_str());
        }
 
        if (TransformComponent* trComp = entity->getComponent<TransformComponent>(); trComp) {
          context->transformSystem->setLocalTransform(trComp, context->transformSystem->getLocalTransform(trComp) * transform);
        }
 
      }
      meshComp->setChanged();
    }
  }
 
}
 
 
Cogs::Core::PackMeshCommand::PackMeshCommand(EditorState* state) :
  EditorCommand(state, state->context)
{
}
 
void Cogs::Core::PackMeshCommand::apply()
{
  for (EntityId entityId : state->selected) {
    if(EntityPtr entity = context->store->getEntity(entityId); entity) {
      handle(context, this, entity);
    }
  }
 
}
 
void Cogs::Core::PackMeshCommand::undo()
{
  for (UndoItem& undoItem : undoItems) {
    if (EntityPtr entity = undoItem.entity.lock(); entity) {
      if (MeshComponent* meshComp = entity->getComponent<MeshComponent>(); meshComp) {
        meshComp->meshHandle = undoItem.meshHandle;
        meshComp->setChanged();
      }
      else {
        LOG_ERROR(logger, "Failed to undo entity, entity lost its mesh component");
      }
    }
    else {
      LOG_ERROR(logger, "Failed to undo entity, entity does not exist");
    }
  }
}
 
 
namespace {
 
  glm::vec2 octEncode(const glm::vec3& n)
  {
    const float t = std::abs(n.x) + std::abs(n.y) + std::abs(n.z);
    glm::vec2 p = (1.f / t)*glm::vec2(n);
    if (n.z <= 0.f) {
      p = glm::vec2((0.f <= p.x ? 1.f : -1.f) * (1.f - std::abs(p.y)),
                    (0.f <= p.y ? 1.f : -1.f) * (1.f - std::abs(p.x)));
 
    }
    return p;
  }
 
  glm::vec3 octDecode(const glm::vec2& f)
  {
    // From https://twitter.com/Stubbesaurus/status/937994790553227264
    glm::vec3 n = glm::vec3(f.x, f.y, 1.f - std::abs(f.x) - std::abs(f.y));
    float t = std::max(-n.z, 0.f);
    n.x += 0.f <= n.x ? -t : t;
    n.y += 0.f <= n.y ? -t : t;
    return glm::normalize(n);
  }
 
  glm::i16vec2 octEncodei16(const glm::vec3& n)
  {
#if 1
    glm::vec2 enc = glm::clamp(32767.f * octEncode(n), glm::vec2(-32767.f), glm::vec2(32767.f));
    glm::i16vec2 q[4] = {
      glm::i16vec2(std::floor(enc.x), floor(enc.y)),
      glm::i16vec2(std::floor(enc.x), ceil(enc.y)),
      glm::i16vec2(std::ceil(enc.x), floor(enc.y)),
      glm::i16vec2(std::ceil(enc.x), ceil(enc.y)),
    };
    size_t best = 0;
    float error = 1.f - std::abs(dot(n, octDecode((1.f / 32767.f) * glm::vec2(q[0]))));
    for (size_t i = 1; i < 4; i++) {
      float e = 1.f - std::abs(dot(n, octDecode((1.f / 32767.f) * glm::vec2(q[i]))));
      if (e < error) {
        best = i;
        error = e;
      }
    }
    return q[best];
#else
    return glm::i16vec2(glm::clamp(glm::round(32767.f * octEncode(n)),
                                   glm::vec2(-32767.f),
                                   glm::vec2(32767.f)));
#endif
  }
 
  glm::i8vec2 octEncodei8(const glm::vec3& n)
  {
#if 1
    glm::vec2 enc = glm::clamp(127.f * octEncode(n), glm::vec2(-127.f), glm::vec2(127.f));
    glm::i8vec2 q[4] = {
      glm::i8vec2(std::floor(enc.x), floor(enc.y)),
      glm::i8vec2(std::floor(enc.x), ceil(enc.y)),
      glm::i8vec2(std::ceil(enc.x), floor(enc.y)),
      glm::i8vec2(std::ceil(enc.x), ceil(enc.y)),
    };
 
    size_t best = 0;
    float error = 1.f - std::abs(dot(n, octDecode((1.f / 127.f) * glm::vec2(q[0]))));
    for (size_t i = 1; i < 4; i++) {
      float e = 1.f - std::abs(dot(n, octDecode((1.f / 127.f) * glm::vec2(q[i]))));
      if (e < error) {
        best = i;
        error = e;
      }
    }
    return q[best];
#else
    return glm::i8vec2(glm::clamp(glm::round(127.f * octEncode(n)),
                                  glm::vec2(-127.f),
                                  glm::vec2(127.f)));
#endif
  }
 
  enum struct PackFormat
  {
    Pos10un_Nrm8sn_Id16ui,
    Pos16un_Nrm16sn_Id16ui,
    Pos16un_Nrm8sn_Id32ui,
    Pos16un_Nrm8sn_Id32f,
    Count
  };
 
  namespace Pos10un_Nrm8sn_Id16ui {
    struct Vtx
    {
      union {
        struct {
          unsigned pos_x : 10;
          unsigned pos_y : 10;
          unsigned pos_z : 10;
          unsigned pos_w : 2;
        };
        uint32_t pos;
      };
      uint16_t tex;
      glm::u8vec2 nrm;
    };
    static_assert(sizeof(Vtx) == 8);
 
    const Cogs::VertexElement Fmt[] = {
      {
        .offset = uint16_t(offsetof(Vtx, pos)),
        .format = Cogs::Format::R10G10B10A2_UNORM,
        .semantic = Cogs::ElementSemantic::Position,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      },
      {
        .offset = uint16_t(offsetof(Vtx, nrm)),
        .format = Cogs::Format::R8G8_SNORM,
        .semantic = Cogs::ElementSemantic::Normal,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      },
      {
        .offset = uint16_t(offsetof(Vtx, tex)),
        .format = Cogs::Format::R16_UINT,
        .semantic = Cogs::ElementSemantic::TextureCoordinate,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      }
    };
  }
 
  namespace Pos16un_Nrm16sn_Id16ui {
    struct Vtx
    {
      glm::u16vec3 pos;
      uint16_t tex;
      glm::u16vec2 nrm;
    };
    static_assert(sizeof(Vtx) == 12);
 
    const Cogs::VertexElement Fmt[] = {
      {
        .offset = uint16_t(offsetof(Vtx, pos)),
        .format = Cogs::Format::R16G16B16_UNORM,
        .semantic = Cogs::ElementSemantic::Position,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      },
      {
        .offset = uint16_t(offsetof(Vtx, nrm)),
        .format = Cogs::Format::R16G16_SNORM,
        .semantic = Cogs::ElementSemantic::Normal,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      },
      {
        .offset = uint16_t(offsetof(Vtx, tex)),
        .format = Cogs::Format::R16_UINT,
        .semantic = Cogs::ElementSemantic::TextureCoordinate,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      }
    };
  }
 
  namespace Pos16un_Nrm8sn_Id32ui {
    struct Vtx
    {
      glm::u16vec3 pos;
      glm::u8vec2 nrm;
      uint32_t tex;
    };
    static_assert(sizeof(Vtx) == 12);
 
    const Cogs::VertexElement Fmt[] = {
      {
        .offset = uint16_t(offsetof(Vtx, pos)),
        .format = Cogs::Format::R16G16B16_UNORM,
        .semantic = Cogs::ElementSemantic::Position,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      },
      {
        .offset = uint16_t(offsetof(Vtx, nrm)),
        .format = Cogs::Format::R8G8_SNORM,
        .semantic = Cogs::ElementSemantic::Normal,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      },
      {
        .offset = uint16_t(offsetof(Vtx, tex)),
        .format = Cogs::Format::R32_UINT,
        .semantic = Cogs::ElementSemantic::TextureCoordinate,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      }
    };
  }
 
  namespace Pos16un_Nrm8sn_Id32f {
    struct Vtx
    {
      glm::u16vec3 pos;
      glm::u8vec2 nrm;
      float tex;
    };
    static_assert(sizeof(Vtx) == 12);
 
    const Cogs::VertexElement Fmt[] = {
      {
        .offset = uint16_t(offsetof(Vtx, pos)),
        .format = Cogs::Format::R16G16B16_UNORM,
        .semantic = Cogs::ElementSemantic::Position,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      },
      {
        .offset = uint16_t(offsetof(Vtx, nrm)),
        .format = Cogs::Format::R8G8_SNORM,
        .semantic = Cogs::ElementSemantic::Normal,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      },
      {
        .offset = uint16_t(offsetof(Vtx, tex)),
        .format = Cogs::Format::R32_FLOAT,
        .semantic = Cogs::ElementSemantic::TextureCoordinate,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      }
    };
  }
 
  namespace Pos16un_Nrm8sn {
    struct Vtx
    {
      glm::u16vec3 pos;
      glm::u8vec2 nrm;
    };
    static_assert(sizeof(Vtx) == 8);
 
    const Cogs::VertexElement Fmt[2] = {
      {
        .offset = uint16_t(offsetof(Vtx, pos)),
        .format = Cogs::Format::R16G16B16_UNORM,
        .semantic = Cogs::ElementSemantic::Position,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      },
      {
        .offset = uint16_t(offsetof(Vtx, nrm)),
        .format = Cogs::Format::R8G8_SNORM,
        .semantic = Cogs::ElementSemantic::Normal,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      }
    };
  }
 
  namespace Id16ui {
    const Cogs::VertexElement Fmt[] = {
      {
        .offset = 0,
        .format = Cogs::Format::R16_UINT,
        .semantic = Cogs::ElementSemantic::TextureCoordinate,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      }
    };
  }
 
  namespace Id32ui {
    const Cogs::VertexElement Fmt[] = {
      {
        .offset = 0,
        .format = Cogs::Format::R32_UINT,
        .semantic = Cogs::ElementSemantic::TextureCoordinate,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      }
    };
  }
 
  namespace Id32f {
    const Cogs::VertexElement Fmt[] = {
      {
        .offset = 0,
        .format = Cogs::Format::R32_FLOAT,
        .semantic = Cogs::ElementSemantic::TextureCoordinate,
        .semanticIndex = 0,
        .inputType = Cogs::InputType::VertexData,
        .instanceStep = 0
      }
    };
  }
 
  namespace Id32ui_Inst {
    const Cogs::VertexElement Fmt[] = {
      {
        .offset = 0,
        .format = Cogs::Format::R32_UINT,
        .semantic = Cogs::ElementSemantic::TextureCoordinate,
        .semanticIndex = 1,
        .inputType = Cogs::InputType::InstanceData,
        .instanceStep = 1
      },
    };
  }
 
  void findUniqueVertices(Cogs::Memory::MemoryBuffer& uniqueVertexData, std::vector<uint32_t>& uniqueVertexMap, const Cogs::Memory::MemoryBuffer& inVertexData, const size_t stride)
  {
    if (inVertexData.empty()) return;
 
    const char* inVertices = (const char*)inVertexData.data();
    const size_t inVertexCount = inVertexData.size() / stride;
 
    std::vector<uint32_t> permutation(inVertexCount);
    for (uint32_t i = 0; i < inVertexCount; i++) {
      permutation[i] = i;
    }
    std::sort(permutation.begin(), permutation.end(), 
              [inVertices, stride](const uint32_t& a, const uint32_t& b) -> bool
              {
                return std::memcmp(&inVertices[stride * a], &inVertices[stride * b], stride) < 0;
              });
 
    uniqueVertexData.resize(inVertexData.size());
    char* uniqueVertices = (char*)uniqueVertexData.data();
    size_t currentUniqueVertex = 0;
 
    uniqueVertexMap.resize(inVertexCount);
    uniqueVertexMap[permutation[0]] = uint32_t(currentUniqueVertex);
    std::memcpy(&uniqueVertices[stride * currentUniqueVertex], &inVertices[stride * permutation[0]], stride);
 
    for (size_t i = 1; i < inVertexCount; i++) {
      if (std::memcmp(&uniqueVertices[stride * currentUniqueVertex], &inVertices[stride * permutation[i]], stride) != 0) {
        std::memcpy(&uniqueVertices[stride * (++currentUniqueVertex)], &inVertices[stride * permutation[i]], stride);
      }
      uniqueVertexMap[permutation[i]] = uint32_t(currentUniqueVertex);
    }
 
    const size_t uniqueVertexCount = currentUniqueVertex + 1;
 
    assert(uniqueVertexCount <= inVertexCount);
    uniqueVertexData.resize(stride * uniqueVertexCount);
  }
 
  bool buildPackedVertices(Cogs::VertexFormatHandle& fmtHandle,
                           Cogs::Memory::MemoryBuffer& uniqueVertexData,
                           std::vector<uint32_t>& uniqueVertexMap,
                           const PackFormat packFmt,
                           const MappedStreamReadOnly<glm::vec3>& srcPos,
                           const MappedStreamReadOnly<glm::vec3>& srcNrm,
                           const MappedStreamReadOnly<glm::vec2>& srcTex,
                           const size_t vertexCount,
                           const glm::vec3& posMin,
                           const float invScale,
                           const uint32_t idOffset)
  {
    Cogs::Memory::MemoryBuffer vertexData;
 
    size_t fmtSize = 0;
 
    float posFormatScale = 0.f;
    switch (packFmt) {
 
    case PackFormat::Pos10un_Nrm8sn_Id16ui: {
      fmtHandle = Cogs::VertexFormats::createVertexFormat(Pos10un_Nrm8sn_Id16ui::Fmt, std::size(Pos10un_Nrm8sn_Id16ui::Fmt));
      fmtSize = Cogs::getSize(fmtHandle);
      assert(sizeof(Pos10un_Nrm8sn_Id16ui::Vtx) == fmtSize);
      posFormatScale = 1023.f;
 
      vertexData.resize(fmtSize * vertexCount, false);
      Pos10un_Nrm8sn_Id16ui::Vtx* vtx = (Pos10un_Nrm8sn_Id16ui::Vtx*)vertexData.data();
      for (size_t i = 0; i < vertexCount; i++) {
        glm::vec3 p = glm::clamp(glm::round(posFormatScale * invScale * (srcPos[i] - posMin)), glm::vec3(0.f), glm::vec3(posFormatScale));
        vtx[i].pos_x = uint32_t(p.x);
        vtx[i].pos_y = uint32_t(p.y);
        vtx[i].pos_z = uint32_t(p.z);
        vtx[i].pos_w = 3u;
        vtx[i].nrm = octEncodei8(glm::normalize(srcNrm[i]));
        vtx[i].tex = uint16_t(uint32_t(std::max(0.f, srcTex[i].x)) - idOffset);
      }
 
      findUniqueVertices(uniqueVertexData, uniqueVertexMap, vertexData, fmtSize);
      break;
    }
 
    case PackFormat::Pos16un_Nrm16sn_Id16ui: {
      fmtHandle = Cogs::VertexFormats::createVertexFormat(Pos16un_Nrm16sn_Id16ui::Fmt, std::size(Pos16un_Nrm16sn_Id16ui::Fmt));
      fmtSize = Cogs::getSize(fmtHandle);
      assert(sizeof(Pos16un_Nrm16sn_Id16ui::Vtx) == fmtSize);
      posFormatScale = std::numeric_limits<uint16_t>::max();
 
      vertexData.resize(fmtSize * vertexCount, false);
      Pos16un_Nrm16sn_Id16ui::Vtx* vtx = (Pos16un_Nrm16sn_Id16ui::Vtx*)vertexData.data();
      for (size_t i = 0; i < vertexCount; i++) {
        glm::vec3 p = glm::clamp(glm::round(posFormatScale * invScale * (srcPos[i] - posMin)), glm::vec3(0.f), glm::vec3(posFormatScale));
        vtx[i].pos = glm::u16vec3(p);
        vtx[i].nrm = octEncodei16(glm::normalize(srcNrm[i]));
        vtx[i].tex = uint16_t(uint32_t(std::max(0.f, srcTex[i].x)) - idOffset);
      }
 
      findUniqueVertices(uniqueVertexData, uniqueVertexMap, vertexData, fmtSize);
      break;
    }
 
    case PackFormat::Pos16un_Nrm8sn_Id32ui: {
      fmtHandle = Cogs::VertexFormats::createVertexFormat(Pos16un_Nrm8sn_Id32ui::Fmt, std::size(Pos16un_Nrm8sn_Id32ui::Fmt));
      fmtSize = Cogs::getSize(fmtHandle);
      assert(sizeof(Pos16un_Nrm8sn_Id32ui::Vtx) == fmtSize);
      posFormatScale = std::numeric_limits<uint16_t>::max();
 
      vertexData.resize(fmtSize * vertexCount, false);
      Pos16un_Nrm8sn_Id32ui::Vtx* vtx = (Pos16un_Nrm8sn_Id32ui::Vtx*)vertexData.data();
      for (size_t i = 0; i < vertexCount; i++) {
        glm::vec3 p = glm::clamp(glm::round(posFormatScale * invScale * (srcPos[i] - posMin)), glm::vec3(0.f), glm::vec3(posFormatScale));
        vtx[i].pos = glm::u16vec3(p);
        vtx[i].nrm = octEncodei8(glm::normalize(srcNrm[i]));
        vtx[i].tex = uint32_t(std::max(0.f, srcTex[i].x)) - idOffset;
      }
 
      findUniqueVertices(uniqueVertexData, uniqueVertexMap, vertexData, fmtSize);
      break;
    }
 
    case PackFormat::Pos16un_Nrm8sn_Id32f: {
      fmtHandle = Cogs::VertexFormats::createVertexFormat(Pos16un_Nrm8sn_Id32f::Fmt, std::size(Pos16un_Nrm8sn_Id32f::Fmt));
      fmtSize = Cogs::getSize(fmtHandle);
      assert(sizeof(Pos16un_Nrm8sn_Id32f::Vtx) == fmtSize);
      posFormatScale = std::numeric_limits<uint16_t>::max();
 
      vertexData.resize(fmtSize * vertexCount, false);
      Pos16un_Nrm8sn_Id32f::Vtx* vtx = (Pos16un_Nrm8sn_Id32f::Vtx*)vertexData.data();
      for (size_t i = 0; i < vertexCount; i++) {
        glm::vec3 p = glm::clamp(glm::round(posFormatScale * invScale * (srcPos[i] - posMin)), glm::vec3(0.f), glm::vec3(posFormatScale));
        vtx[i].pos = glm::u16vec3(p);
        vtx[i].nrm = octEncodei8(glm::normalize(srcNrm[i]));
        vtx[i].tex = float(uint32_t(std::max(0.f, srcTex[i].x)) - idOffset);
      }
 
      findUniqueVertices(uniqueVertexData, uniqueVertexMap, vertexData, fmtSize);
      break;
    }
 
    default:
      assert(false && "Invalid enum");
      return false;
    }
 
    return true;
  }
 
  bool buildIndices(std::vector<uint32_t>& indices, const std::vector<uint32_t>& uniqueVertexMap, const Mesh* srcMesh)
  {
    // Build indexed representation
    if (srcMesh->isIndexed()) {
      const DataStream& indexStream = srcMesh->getStream(VertexDataType::Indexes);
 
      indices.reserve(indexStream.numElements);
      switch (indexStream.stride)
      {
      case 2: {
        const uint16_t* srcIndices = (const uint16_t*)indexStream.data();
        for (size_t i = 0; i + 2 < indexStream.numElements; i += 3) {
          uint32_t a = uniqueVertexMap[srcIndices[i + 0]];
          uint32_t b = uniqueVertexMap[srcIndices[i + 1]];
          uint32_t c = uniqueVertexMap[srcIndices[i + 2]];
          if (a != b && b != c && a != c) {
            indices.emplace_back(a);
            indices.emplace_back(b);
            indices.emplace_back(c);
          }
        }
        break;
      }
      case 4: {
        const uint32_t* srcIndices = (const uint32_t*)indexStream.data();
        for (size_t i = 0; i + 2 < indexStream.numElements; i += 3) {
          uint32_t a = uniqueVertexMap[srcIndices[i + 0]];
          uint32_t b = uniqueVertexMap[srcIndices[i + 1]];
          uint32_t c = uniqueVertexMap[srcIndices[i + 2]];
          if (a != b && b != c && a != c) {
            indices.emplace_back(a);
            indices.emplace_back(b);
            indices.emplace_back(c);
          }
        }
        break;
      }
      default:
        assert(false && "Illegal index size");
        return false;
      }
    }
    else {
      size_t inVertexCount = uniqueVertexMap.size();
      indices.reserve(inVertexCount);
      for (size_t i = 0; i + 2 < inVertexCount; i += 3) {
        uint32_t a = uniqueVertexMap[i + 0];
        uint32_t b = uniqueVertexMap[i + 1];
        uint32_t c = uniqueVertexMap[i + 2];
        if (a != b && b != c && a != c) {
          indices.emplace_back(a);
          indices.emplace_back(b);
          indices.emplace_back(c);
        }
      }
    }
 
    return true;
  }
 
 
  MeshHandle buildIndexedMesh(Context* context,
                              const Mesh* srcMesh,
                              const Cogs::Geometry::BoundingBox& bbox,
                              const Cogs::VertexFormatHandle& posFmtHandle,
                              const Cogs::Memory::MemoryBuffer& posVertexData,
                              const Cogs::VertexFormatHandle& idsFmtHandle,
                              const Cogs::Memory::MemoryBuffer& idsVertexData,
                              const size_t vertexCount,
                              const std::vector<uint32_t>& indexData,
                              const uint32_t dstIndexTypeSize,
                              const uint32_t idOffset)
  {
    MeshManager::ResourceProxy dstMesh = context->meshManager->createLocked();
    dstMesh->primitiveType = srcMesh->primitiveType;
 
    // Position, normal and maybe id goes into Interleaved0 stream.
    const size_t posFmtSize = Cogs::getSize(posFmtHandle);
    assert(posVertexData.size() == posFmtSize * vertexCount);
    if (uint8_t* dst = dstMesh->mapStream(VertexDataType::Interleaved0, posFmtHandle, 0, vertexCount, posFmtSize, true); dst != nullptr) {
      std::memcpy(dst, posVertexData.data(), posVertexData.size());
      dstMesh->unmap(VertexDataType::Interleaved0);
    }
    else {
      return MeshHandle::NoHandle;
    }
 
    // We have a separate ids tream, store it in TexCoord0 stream
    if (idsFmtHandle) {
      const size_t idsFmtSize = Cogs::getSize(idsFmtHandle);
      assert(idsVertexData.size() == idsFmtSize * vertexCount);
      if (uint8_t* dst = dstMesh->mapStream(VertexDataType::TexCoords0, idsFmtHandle, 0, vertexCount, idsFmtSize, true); dst != nullptr) {
        std::memcpy(dst, idsVertexData.data(), idsVertexData.size());
        dstMesh->unmap(VertexDataType::Interleaved0);
      }
      else {
        return MeshHandle::NoHandle;
      }
    }
 
    // If we have an offset to the ids, put it texcoord1
    if (idOffset) {
      Cogs::VertexFormatHandle instanceFmtHandle = Cogs::VertexFormats::createVertexFormat(Id32ui_Inst::Fmt, std::size(Id32ui_Inst::Fmt));
      if (uint8_t* dst = dstMesh->mapStream(VertexDataType::TexCoords1, instanceFmtHandle, 0, 1, Cogs::getSize(instanceFmtHandle), true); dst != nullptr) {
        std::memcpy(dst, &idOffset, sizeof(idOffset));
        dstMesh->unmap(VertexDataType::TexCoords1);
      }
      else {
        return MeshHandle::NoHandle;
      }
    }
 
    // Store indices
    const size_t indexCount = indexData.size();
 
    if (uint8_t* ptr = dstMesh->mapStream(VertexDataType::Indexes, 0, indexCount, dstIndexTypeSize, true); ptr != nullptr) {
      switch (dstIndexTypeSize) {
      case 2: {
        uint16_t* dstIndices = (uint16_t*)ptr;
        for (size_t i = 0; i < indexCount; i++) {
          dstIndices[i] = uint16_t(indexData[i]);
        }
        break;
      }
      case 4:
        std::memcpy(ptr, indexData.data(), sizeof(uint32_t) * indexCount);
        break;
      default:
        assert(false);
        return MeshHandle::NoHandle;
      }
      dstMesh->unmap(VertexDataType::Indexes);
      dstMesh->setMeshFlag(MeshFlags::Indexed);
      dstMesh->setMeshFlag(MeshFlags::IndexesChanged);
      dstMesh->setCount(indexCount);
    }
    else {
      return MeshHandle::NoHandle;
    }
 
    dstMesh->setBounds(bbox);
    return dstMesh.getHandle();
  }
 
  MeshHandle buildUnindexedMesh(Context* context,
                                const Mesh* srcMesh,
                                const Cogs::Geometry::BoundingBox& bbox,
                                Cogs::VertexFormatHandle& posFmtHandle,
                                const Cogs::Memory::MemoryBuffer& posVertexData,
                                const Cogs::VertexFormatHandle& idsFmtHandle,
                                const Cogs::Memory::MemoryBuffer& idsVertexData,
                                const size_t vertexCount,
                                const std::vector<uint32_t>& indexData,
                                const uint32_t idOffset)
  {
    const size_t indexCount = indexData.size();
 
    MeshManager::ResourceProxy dstMesh = context->meshManager->createLocked();
    dstMesh->primitiveType = srcMesh->primitiveType;
 
    // Position, normal and maybe id goes into Interleaved0 stream.
    const size_t posFmtSize = Cogs::getSize(posFmtHandle);
    assert(posVertexData.size() == posFmtSize * vertexCount);
    if (uint8_t* dst = dstMesh->mapStream(VertexDataType::Interleaved0, posFmtHandle, 0, indexCount, posFmtSize, true); dst != nullptr)
    {
      const uint8_t* src = static_cast<const uint8_t*>(posVertexData.data());
      for (size_t i = 0; i < indexCount; i++) {
        const size_t ix = indexData[i];
        assert(ix < vertexCount);
        std::memcpy(dst + posFmtSize * i, src + posFmtSize * ix, posFmtSize);
      }
      dstMesh->unmap(VertexDataType::Interleaved0);
    }
    else {
      return MeshHandle::NoHandle;
    }
 
    // We have a separate ids tream, store it in TexCoord0 stream
    if (idsFmtHandle) {
      const size_t idsFmtSize = Cogs::getSize(idsFmtHandle);
      assert(idsVertexData.size() == idsFmtSize * vertexCount);
      if (uint8_t* dst = dstMesh->mapStream(VertexDataType::TexCoords0, idsFmtHandle, 0, indexCount, idsFmtSize, true); dst != nullptr)
      {
        const uint8_t* src = static_cast<const uint8_t*>(idsVertexData.data());
        for (size_t i = 0; i < indexCount; i++) {
          const size_t ix = indexData[i];
          assert(ix < vertexCount);
          std::memcpy(dst + idsFmtSize * i, src + idsFmtSize * ix, idsFmtSize);
        }
        dstMesh->unmap(VertexDataType::TexCoords0);
      }
      else {
        return MeshHandle::NoHandle;
      }
    }
 
    // If we have an offset to the ids, put it texcoord1
    if (idOffset) {
      Cogs::VertexFormatHandle instanceFmtHandle = Cogs::VertexFormats::createVertexFormat(Id32ui_Inst::Fmt, std::size(Id32ui_Inst::Fmt));
      if (uint8_t* dst = dstMesh->mapStream(VertexDataType::TexCoords1, instanceFmtHandle, 0, 1, Cogs::getSize(instanceFmtHandle), true); dst != nullptr) {
        std::memcpy(dst, &idOffset, sizeof(idOffset));
        dstMesh->unmap(VertexDataType::TexCoords1);
      }
      else {
        return MeshHandle::NoHandle;
      }
    }
 
    dstMesh->setBounds(bbox);
    return dstMesh.getHandle();
  }
}
 
bool Cogs::Core::PackMeshCommand::pack(Context* context, std::string& status, glm::mat4& transform, MeshHandle& h, const Options& options)
{
  MeshHandle src = h;
 
  Mesh* srcMesh = src.resolve();
  if (srcMesh == nullptr) {
    status = "Empty mesh";
    return false;
  }
 
  size_t srcVertexCount = 0;
 
  // -- Verify and map positions
  if (srcMesh->hasStream(VertexDataType::Positions)) {
    const DataStream& dataStream = srcMesh->getStream(VertexDataType::Positions);
    srcVertexCount = dataStream.numElements;
  }
  else {
    status = "Mesh does not have positions stream.";
    return false;
  }
  MappedStreamReadOnly<glm::vec3> srcPos = srcMesh->mapPositionsReadOnly(0, srcVertexCount);
  if (!srcPos.isValid()) {
    status = "Failed to map mesh positions as float3.";
    return false;
  }
 
  // -- Verify and map normals
  if (srcMesh->hasStream(VertexDataType::Normals)) {
    const DataStream& srcNrmStream = srcMesh->getStream(VertexDataType::Normals);
    if (srcNrmStream.numElements < srcVertexCount) {
      status = "Mesh has less normals than positions.";
      return false;
    }
  }
  else {
    status = "Mesh does not have normals stream, ignoring";
    return false;
  }
  MappedStreamReadOnly<glm::vec3> srcNrm = srcMesh->mapNormalsReadOnly(0, srcVertexCount);
  if (!srcNrm.isValid()) {
    status = "Failed to map mesh normals as float3.";
    return false;
  }
 
  // -- Verify and map texcoords
  if (srcMesh->hasStream(VertexDataType::TexCoords0)) {
    const DataStream& dataStream = srcMesh->getStream(VertexDataType::TexCoords0);
    if (dataStream.numElements < srcVertexCount) {
      status = "Mesh has less texcoords than positions.";
      return false;
    }
  }
  else {
    status = "Mesh does not have texcoords stream, ignoring";
    return false;
  }
  MappedStreamReadOnly<glm::vec2> srcTex = srcMesh->mapTexCoordsReadOnly(0, srcVertexCount);
  if (!srcTex.isValid()) {
    status = "Failed to map mesh normals as float3, ignoring.";
    return false;
  }
 
  // Calculate precise bounds
  glm::vec3 posMin(std::numeric_limits<float>::max());
  glm::vec3 posMax(-std::numeric_limits<float>::max());
  for (size_t i = 0; i < srcVertexCount; i++) {
    posMin = glm::min(posMin, srcPos[i]);
    posMax = glm::max(posMax, srcPos[i]);
  }
  glm::vec3 extent = posMax - posMin;
  float scale = std::max(std::max(extent.x, extent.y), extent.z);
  if (srcVertexCount == 0) {
    LOG_WARNING(logger, "No vertices - skipping scaling");
    scale = 1.0f;
  }
  else if (scale <= 0.0f) {
    LOG_WARNING(logger, "Zero extent(%f) - skipping scaling", scale);
    scale = 1.0f;
  }
  
  const float invScale = 1.f / scale;
 
  // Figure out how many bits needed for id. Negative ids is encoded as zero.
  uint32_t idMax = 0;
  uint32_t idMin = 0;
  if (0 < srcVertexCount) {
    idMin = std::numeric_limits<uint32_t>::max();
    for (size_t i = 0; i < srcVertexCount; i++) {
      uint32_t id = uint32_t(std::max(0.f, srcTex[i].x));
      idMin = std::min(idMin, id);
      idMax = std::max(idMax, id);
    }
  }
 
  // Choose packing format
  uint32_t idOffset = 0;
  PackFormat packFormat;
  switch (options.target)
  {
  case Target::WebGL1_Low:
    packFormat = PackFormat::Pos16un_Nrm8sn_Id32f;
    break;
 
  case Target::WebGL2_Low:
    // Use id offset if allowed and it maks sense
    if (options.allowIdOffset && (0xFFFFu <= idMax) && ((idMax - idMin) < 0xFFFFu)) {
      idOffset = idMin;
    }
 
    // If we are not splitting out ids later, and it is possible, use 16 bits for ids.
    if (((idMax - idOffset) < 0xFFFFu)) {
      // pos + normal is 6 bytes, but webgl cannot represent this format, it has to be 4 or 8 bytes, so we can just use padding for id.
      packFormat = PackFormat::Pos10un_Nrm8sn_Id16ui;
    }
    else {
      // If Id32 gets stripped out, so we are left with Pos16Un_Nrm8sn, which is 8 bytes.
      packFormat = PackFormat::Pos16un_Nrm8sn_Id32ui;
    }
    break;
 
  case Target::WebGL2_Med:
 
    // Use id offset if allowed and it makes sense
    if (options.allowIdOffset && (0xFFFFu <= idMax) && ((idMax - idMin) < 0xFFFFu)) {
      idOffset = idMin;
    }
 
    // If we are not splitting out ids later, and it is possible, use 16 bits for ids.
    if (!options.allowSeparateIdStream && ((idMax - idOffset) < 0xFFFFu)) {
      packFormat = PackFormat::Pos16un_Nrm16sn_Id16ui;
    }
    else {
      packFormat = PackFormat::Pos16un_Nrm8sn_Id32ui;
    }
    break;
 
  default:
    assert(false && "Invalid enum");
    return false;
  }
 
  Cogs::Memory::MemoryBuffer tmp;
 
  std::vector<uint32_t> remapping;
  Cogs::Memory::MemoryBuffer posData;
 
  VertexFormatHandle posFmtHandle;
  if (!buildPackedVertices(posFmtHandle,
                           posData,
                           remapping,
                           packFormat,
                           srcPos, srcNrm, srcTex, srcVertexCount, posMin, invScale, idOffset)) return false;
 
  size_t posFmtSize = Cogs::getSize(posFmtHandle);
  const size_t vertexCount = posData.size() / posFmtSize;
 
  std::vector<uint32_t> indices;
  if (!buildIndices(indices, remapping, srcMesh)) return false;
  const size_t indexCount = indices.size();
 
  // Reorder indices to reduce the number of GPU vertex shader invocations
  if (options.optimizeTriangleOrder) {
    std::vector<uint32_t> indices2(indexCount);
    meshopt_optimizeVertexCache(indices2.data(), indices.data(), indexCount, vertexCount);
    indices.swap(indices2);
  }
 
  // Reorder vertices to optimize for vertex fetch cache
  if (options.optimizeVertexOrder) {
    tmp.resize(posData.size(), false);
    meshopt_optimizeVertexFetch(tmp.data(), indices.data(), indexCount, posData.data(), vertexCount, posFmtSize);
    posData.swap(tmp);
  }
 
  // Move id/texcoord into its own stream where reasonable.
  size_t interleavedIdsOffset = 0;        // Offset of id into the interleaved vertex format. 
  VertexFormatHandle newPosFmtHandle;
  VertexFormatHandle idsFmtHandle;
  if (options.allowSeparateIdStream) {
    switch (packFormat) {
    case PackFormat::Pos10un_Nrm8sn_Id16ui:
    case PackFormat::Pos16un_Nrm16sn_Id16ui:
      break;
 
    case PackFormat::Pos16un_Nrm8sn_Id32ui:
      interleavedIdsOffset = offsetof(Pos16un_Nrm8sn_Id32ui::Vtx, tex);
      newPosFmtHandle = Cogs::VertexFormats::createVertexFormat(Pos16un_Nrm8sn::Fmt, std::size(Pos16un_Nrm8sn::Fmt));
      if (idMax - idOffset < 0xFFFFu) {
        idsFmtHandle = Cogs::VertexFormats::createVertexFormat(Id16ui::Fmt, std::size(Id16ui::Fmt));
      }
      else {
        idsFmtHandle = Cogs::VertexFormats::createVertexFormat(Id32ui::Fmt, std::size(Id32ui::Fmt));
      }
      break;
 
    case PackFormat::Pos16un_Nrm8sn_Id32f:
      interleavedIdsOffset = offsetof(Pos16un_Nrm8sn_Id32f::Vtx, tex);
      newPosFmtHandle = Cogs::VertexFormats::createVertexFormat(Pos16un_Nrm8sn::Fmt, std::size(Pos16un_Nrm8sn::Fmt));
      idsFmtHandle = Cogs::VertexFormats::createVertexFormat(Id32f::Fmt, std::size(Id32f::Fmt));
      break;
 
    default:
      assert(false && "Invalid enum value");
      break;
    }
  }
 
  // If we are to split out ids into its own stream, we have a new position format and a id format
  size_t idsFmtSize = 0;
  Cogs::Memory::MemoryBuffer idsData;
  if (newPosFmtHandle && idsFmtHandle) {
    const size_t newPosFmtSize = Cogs::getSize(newPosFmtHandle);
    idsFmtSize = Cogs::getSize(idsFmtHandle);
 
    tmp.resize(newPosFmtSize * vertexCount, false);
    idsData.resize(idsFmtSize * vertexCount, false);
 
    char* newPosDataPtr = static_cast<char*>(tmp.data());
    char* idsDataPtr = static_cast<char*>(idsData.data());
    const char* posDataPtr = static_cast<char*>(posData.data());
    for (size_t i = 0; i < vertexCount; i++) {
      std::memcpy(newPosDataPtr + newPosFmtSize * i, posDataPtr + posFmtSize * i, newPosFmtSize);
      std::memcpy(idsDataPtr + idsFmtSize * i, posDataPtr + posFmtSize * i + interleavedIdsOffset, idsFmtSize);
    }
 
    // Swap in new pos/normal data without id
    posData.swap(tmp);
    posFmtHandle = newPosFmtHandle;
    posFmtSize = newPosFmtSize;
  }
 
  const uint32_t indexTypeSize = vertexCount < 0xffffu ? 2 : 4;
  const size_t indexedSize = (posFmtSize + idsFmtSize) * vertexCount + indexTypeSize * indexCount;
  const size_t unindexedSize = (posFmtSize + idsFmtSize) * indexCount;
 
  bool indexed = indexedSize < unindexedSize;
  //LOG_DEBUG(logger, "fmt=%s indexed size: %zu, unindexed size: %zu", PackFormatName[size_t(packFormat)], indexedSize, unindexedSize);
 
  const Cogs::Geometry::BoundingBox bbox{ glm::vec3(0.f), invScale * extent };
  if (indexed) {
    h = buildIndexedMesh(context, srcMesh, bbox, posFmtHandle, posData, idsFmtHandle, idsData, vertexCount, indices, indexTypeSize, idOffset);
  }
  else {
    h = buildUnindexedMesh(context, srcMesh, bbox, posFmtHandle, posData, idsFmtHandle, idsData, vertexCount, indices, idOffset);
  }
 
  glm::vec3 shift = posMin;
  transform = glm::mat4(scale, 0.f, 0.f, 0.f,
                        0.f, scale, 0.f, 0.f,
                        0.f, 0.f, scale, 0.f,
                        shift.x, shift.y, shift.z, 1.f);
 
  return true;
}