TwinCadModelRayPickExtension.cpp

#include "Systems/Core/RenderSystem.h"
#include "Systems/Core/ClipShapeSystem.h"
 
#include "Components/Core/MeshComponent.h"
 
#include "Services/TaskManager.h"
 
#include "Utilities/FrustumClassification.h"
 
#include "Resources/Mesh.h"
 
#include "Math/RayIntersection.h"
 
#include "Foundation/Logging/Logger.h"
#include "Foundation/Platform/Threads.h"
#include "Foundation/Geometry/BoundingBox.hpp"
 
#include "Context.h"
 
#include "TwinCadModelSystem.h"
 
namespace {
 
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("TwinCadModelRayPick");
 
}
 
Cogs::Core::TwinCadModelRayPickExtension::TwinCadModelRayPickExtension(RenderSystem* renderSystem, TwinCadModelSystem* cadModelSystem)
: renderSystem(renderSystem),
  cadModelSystem(cadModelSystem)
{}
 
 
bool Cogs::Core::TwinCadModelRayPickExtension::pickImpl(Context* context,
                                                        const glm::mat4& worldPickMatrix,
                                                        const glm::mat4& rawViewProjection,
                                                        const glm::mat4& viewMatrix,
                                                        const RayPicking::RayPickFilter& filter,
                                                        PickingFlags pickingFlags,
                                                        PicksReturned returnFlag,
                                                        std::vector<RayPicking::RayPickHit>& hits)
{
  const bool returnChildEntity = (pickingFlags & PickingFlags::ReturnChildEntity) == PickingFlags::ReturnChildEntity;
  const bool addInstanceTexcoords = (pickingFlags & PickingFlags::AddInstanceTexcoords) == PickingFlags::AddInstanceTexcoords;
 
  const size_t hitsBefore = hits.size();
 
  const Cogs::SizeType itemCount = renderSystem->pool.size();
  const Cogs::SizeType batchSize = 1024;
 
  TaskId group = context->taskManager->createGroup();
  Mutex batchLock;
 
  for (Cogs::SizeType start = 0; start < itemCount; start += batchSize) {
    const Cogs::SizeType end = std::min(start + batchSize, itemCount);
 
    context->taskManager->enqueueChild(group,
                                       [&, start, end]() {
      CpuInstrumentationScope(SCOPE_RAYPICK, "executePickingQueryTask");
 
      std::vector<RayPicking::RayPickHit> batchPickResults;
      if (returnFlag != PicksReturned::Closest) {
        batchPickResults.reserve(10); // Reserve space for 10 hits in the current batch based purely on guessing.
      }
 
      for (Cogs::SizeType i = start; i < end; i++) {
        const MeshRenderComponent& comp = renderSystem->pool[i];
 
        // Here, we just let the custom handling pass.
        if (!comp.customRayPickHandling) { continue; }
 
        const TwinCadModelComponent* cadModelComp = comp.customRayPickHandling.resolveComponent<TwinCadModelComponent>();
        if (cadModelComp == nullptr) continue;
 
        const TwinCadModelData& cadModelData = cadModelSystem->getData(cadModelComp);
 
        // Filter out unwanted, invisible or disabled entities early to speed up queries.
        if (filter.isUnwantedType(comp) || comp.lod.currentLod != comp.lod.selectedLod) { continue; }
 
        // ForcePickable flag overrides visibility and flags.
        if (!comp.isRenderFlagSet(RenderFlags::ForcePickable)) {
          if (!comp.isVisible() || !comp.isVisibleInLayer(filter.layerMask) || !comp.isPickable()) {
            continue;
          }
        }
 
        const RayPicking::Ordinal ordinal = filter.getOrdinal(comp);
 
        const MeshRenderData& renderData = renderSystem->getData<MeshRenderData>(&comp);
        if (!renderData.meshComponent) {
          continue;
        }
 
        Geometry::BoundingBox bboxWorld = renderSystem->getWorldBounds(&comp);
        const Mesh* mesh = renderData.meshComponent.resolveComponent<MeshComponent>()->meshHandle.resolve();
        if (!mesh) {
          continue;
        }
        TextureCoordinateInfo texcoordInfo = getTextureCoordinateInfo(*mesh);
 
        if (cadModelComp->allowClipping) {
          bboxWorld.min = glm::max(bboxWorld.min, cadModelComp->minClipping);
          bboxWorld.max = glm::min(bboxWorld.max, cadModelComp->maxClipping);
        }
 
        // Note that the bbox test must be 'fuzzy' (including tolerance) for
        // line picking not to prematurely fail in bbox test (bbox of single
        // line is a single line).
        if (Geometry::isEmpty(bboxWorld)) {
          continue;
        }
 
        if (frustumClassifyBoundingBox(worldPickMatrix, bboxWorld.min, bboxWorld.max) != FrustumPlanes::InsideAll) {
          continue;
        }
 
        // Create transform from local frame to pick frustum, where pick ray
        // is negative Z and ray fuzziness is +/- w.
        glm::mat4 localPickMatrix = worldPickMatrix * renderData.localToWorld;
 
        std::vector<RayIntersectionHit> meshHits;
        std::vector<FrustumPlanes> scratch;
        if (!intersectMesh(meshHits,
                           scratch,
                           localPickMatrix,
                           *mesh,
                           comp.startIndex,
                           comp.vertexCount,
                           static_cast<uint32_t>(-1))) {
          continue;
        }
 
        if (meshHits.empty()) {
          continue;
        }
 
        for (const RayIntersectionHit& meshHit : meshHits) {
 
          uint32_t objectId = 0;
          switch (texcoordInfo.format) {
          case Cogs::DataFormat::R32_FLOAT:
            objectId = static_cast<uint32_t>( * reinterpret_cast<const float*>(texcoordInfo.ptr + texcoordInfo.stride * meshHit.index.x));
            break;
          case Cogs::DataFormat::R32G32_FLOAT:
            objectId = static_cast<uint32_t>( * reinterpret_cast<const float*>(texcoordInfo.ptr + texcoordInfo.stride * meshHit.index.x));
            break;
          case Cogs::DataFormat::R8_UINT:
            objectId = * reinterpret_cast<const uint8_t *>(texcoordInfo.ptr + texcoordInfo.stride * meshHit.index.x);
            break;
          case Cogs::DataFormat::R16_UINT:
            objectId = * reinterpret_cast<const uint16_t *>(texcoordInfo.ptr + texcoordInfo.stride * meshHit.index.x);
            break;
          case Cogs::DataFormat::R32_UINT:
            objectId = * reinterpret_cast<const uint32_t *>(texcoordInfo.ptr + texcoordInfo.stride * meshHit.index.x);
            break;
          case Cogs::DataFormat::Unknown: // No texcoords
            break;
          default:
            LOG_WARNING_ONCE(logger, "Unhandled texcoord format 0x%x", static_cast<uint32_t>(texcoordInfo.format));
            break;
          }
          if (addInstanceTexcoords && texcoordInfo.hasIdOffset) {
            objectId += texcoordInfo.idOffset;
          }
 
          // Skip hidden object Ids
          size_t objectIdIx = 4 * objectId + 3;
          if ((cadModelData.attributeMapData.size() >= objectIdIx) && ((cadModelData.attributeMapData[objectIdIx] >> 2u) == 3u)) {
            continue;
          }
 
          const glm::vec4 position = renderData.localToWorld * glm::vec4(meshHit.pos_, 1.f);
 
          if (cadModelComp->allowClipping) {
            // We assume localToWorld is affine, i.e. position.w is 1.
            const glm::vec3 minClipping = cadModelComp->minClipping;
            const glm::vec3 maxClipping = cadModelComp->maxClipping;
            if ((position.x < minClipping.x) || (maxClipping.x < position.x) ||
                (position.y < minClipping.y) || (maxClipping.y < position.y) ||
                (position.z < minClipping.z) || (maxClipping.z < position.z))
            {
              continue;
            }
          }
 
          if (const ClipShapeComponent* clipComp = comp.clipShapeComponent.resolveComponent<ClipShapeComponent>(); clipComp) {
            const ClipShapeData& clipData = context->clipShapeSystem->getData(clipComp);
            if (clippedByClipComp(clipData, position)) {
              continue;
            }
          }
 
          const glm::vec4 clipPos = rawViewProjection * glm::vec4(position.x, position.y, position.z, 1.f);
          // use clip space to see if the point falls within the view frustum
          if ((-clipPos.w < clipPos.z) && (clipPos.z < clipPos.w)) {
 
            glm::vec4 viewPos = viewMatrix * position;
            float viewDist = -viewPos.z;
 
            if (returnFlag == PicksReturned::Closest && !batchPickResults.empty()) {
              if (batchPickResults[0].isBehind(ordinal, viewDist)) {
                batchPickResults[0] = {comp, returnChildEntity, position, ordinal, viewDist, glm::vec2(objectId, objectId)};
              }
              // else, the intersection we found is further, so don't do anything
            }
            else {
              batchPickResults.emplace_back(comp, returnChildEntity, position, ordinal, viewDist, glm::vec2(objectId, objectId));
            }
          }
        }
      }
 
      if (!batchPickResults.empty()) {
        LockGuard guard(batchLock);
        if (returnFlag == PicksReturned::Closest) {
          if (hits.empty()) {
            hits = std::move(batchPickResults);
          }
          else if (batchPickResults[0] < hits[0]) {
            hits[0] = batchPickResults[0];
          }
          //else don't insert anything because we found a hit further away than what we already had
        }
        else {
          hits.insert(hits.end(),
                      std::make_move_iterator(batchPickResults.begin()),
                      std::make_move_iterator(batchPickResults.end()));
        }
      }
    }
    );
  }
  context->taskManager->destroy(group);
 
  return hitsBefore != hits.size();
}