RayPick.cpp

#include "RayPick.h"
 
#include "Context.h"
 
#include "Components/Core/CameraComponent.h"
#include "Components/Core/TransformComponent.h"
 
#include "Systems/Core/CameraSystem.h"
#include "Systems/Core/ClipShapeSystem.h"
 
#include "Services/Variables.h"
 
#include "Platform/Instrumentation.h"
 
#include "Resources/Mesh.h"
#include "Resources/Buffer.h"
 
#include "Math/RayIntersection.h"
 
#include "Foundation/ComponentModel/Entity.h"
#include "Foundation/Logging/Logger.h"
 
 
using namespace Cogs::Geometry;
 
namespace
{
  using namespace Cogs;
  using namespace Cogs::Core;
 
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("RayPick");
 
  EntityId getRootEntityId(const ComponentModel::Component* component)
  {
    const Entity* container = component->getContainer();
 
    if (container) {
      const TransformComponent* transformComponent = container->getComponent<TransformComponent>();
 
      if (transformComponent) {
        if (const Component* parentTransform = transformComponent->parent.resolve(); parentTransform) {
          return getRootEntityId(parentTransform);
        }
      }
      return container->getId();
    }
 
    return NoEntity;
  }
 
  void filterHits(std::vector<RayPicking::RayPickHit>& hits, float rayLength, const glm::vec3& rayStartPos, PickingFlags pickingFlags)
  {
    const float rayLengthSq = rayLength * rayLength;
    for (auto it = hits.begin(); it != hits.end();) {
      bool erase = false;
 
      // Erase hits further away than rayLength.
      if (glm::distance2(it->position, rayStartPos) > rayLengthSq) {
        erase = true;
      }
      // Erase duplicate hits, where duplicates means hitting the same entity.
      else if ((pickingFlags & PickingFlags::RemoveDuplicateEntities) == PickingFlags::RemoveDuplicateEntities) {
        erase = std::find_if(std::next(it), hits.end(),
                              [it](const RayPicking::RayPickHit& otherHit) {
                                return it->entity == otherHit.entity;
                              }) != hits.end();
      }
      // Erase duplicate hits, where duplicates means hitting the same point on the same entity.
      else if ((pickingFlags & PickingFlags::RemoveDuplicates) == PickingFlags::RemoveDuplicates) {
        erase = std::find_if(std::next(it), hits.end(),
                             [it](const RayPicking::RayPickHit& otherHit) {
                               return it->entity == otherHit.entity && it->textureCoords == otherHit.textureCoords;
                             }) != hits.end();
      }
 
      if (erase) {
        std::swap(*it, hits.back());
        hits.pop_back();
      }
      else {
        ++it;
      }
    }
  }
}
 
bool Cogs::Core::RayPicking::pickCamera(Context* context,
                                        const CameraComponent& camera,
                                        const glm::vec2& cameraPosition,
                                        float rayLength,
                                        float radius,
                                        PickingFlags pickingFlags,
                                        PicksReturned returnFlag,
                                        const RayPickFilter& filter,
                                        std::vector<RayPicking::RayPickHit>& hits)
{
  CpuInstrumentationScope(SCOPE_RAYPICK, "pickCamera");
 
  if ((camera.flags & CameraFlags::EnablePicking) == 0) {
    LOG_WARNING(logger, "Camera picking is not enabled.");
    return NoEntity;
  }
  const CameraData& cameraData = context->cameraSystem->getData(&camera);
 
  glm::vec2 normPos = 2.f * (cameraPosition + 0.5f) / cameraData.viewportSize - 1.f;
  // We don't want each system to sort the hits, we do it here when we gather them all.
  PicksReturned retFlag = returnFlag == PicksReturned::AllSorted ? PicksReturned::AllUnsorted : returnFlag;
 
  bool hitSomething = false;
  for (IRayPickable* pickable : pickables) {
    hitSomething |= pickable->pickCamera(context, camera, normPos, rayLength, radius, pickingFlags, retFlag, filter, hits);
  }
 
  // Filter results based on various flags.
  // TODO: would be best that these hits were not in the results in the first place, should be each system's job to make sure of that.
  const glm::vec3 cameraPos = glm::vec3(cameraData.inverseViewMatrix[3]);
  filterHits(hits, rayLength, cameraPos, pickingFlags);
 
  if (returnFlag == PicksReturned::AllSorted) {
    std::sort(hits.begin(), hits.end());
  }
 
  // Use a bool insted of returning !hits.emtpy() just in case the function is called with a non empty hits vector.
  return hitSomething;
}
 
bool Cogs::Core::RayPicking::pickRay(Context* context,
                                     const glm::vec3 & position,
                                     const glm::quat & rotation,
                                     float rayLength,
                                     float radius,
                                     PickingFlags pickingFlags,
                                     PicksReturned returnFlag,
                                     const RayPickFilter& filter,
                                     std::vector<RayPicking::RayPickHit>& hits)
{
  CpuInstrumentationScope(SCOPE_RAYPICK, "pickRay");
 
  // We don't want each system to sort the hits, we do it here when we gather them all.
  PicksReturned retFlag = returnFlag == PicksReturned::AllSorted ? PicksReturned::AllUnsorted : returnFlag;
 
  bool hitSomething = false;
  for (IRayPickable* pickable : pickables) {
    pickable->pickRay(context, position, rotation, rayLength, radius, pickingFlags, retFlag, filter, hits);
  }
  // Filter results based on various flags.
  // TODO: would be best that these hits were not in the results in the first place, should be each system's job to make sure of that.
  filterHits(hits, rayLength, position, pickingFlags);
 
  if (returnFlag == PicksReturned::AllSorted) {
    std::sort(hits.begin(), hits.end());
  }
 
  // Use a bool instead of returning !hits.emtpy() just in case the function is called with a non empty hits vector.
  return hitSomething;
}
 
void Cogs::Core::RayPicking::addPickable(IRayPickable * pickable)
{
  pickables.push_back(pickable);
  std::sort(pickables.begin(), pickables.end(), [](const IRayPickable* a, const IRayPickable* b) { return a->order < b->order; });
}
 
void Cogs::Core::RayPicking::removePickable(IRayPickable * pickable)
{
  pickables.erase(std::remove(pickables.begin(), pickables.end(), pickable), pickables.end());
}
 
 
Cogs::Core::RayPicking::RayPickHit::RayPickHit(const ComponentModel::Component& comp, bool returnChildEntity, const glm::vec3& pos, float dist, const glm::vec2& texCoords)
  : position(pos)
  , textureCoords(texCoords)
  , distance(dist)
{
  if (returnChildEntity) {
    entity = comp.getContainer()->getId();
    root = getRootEntityId(&comp);
  }
  else {
    entity = getRootEntityId(&comp);
  }
}
 
// ----
 
bool Cogs::Core::RayPicking::RayPickFilter::isUnwantedType(const ComponentModel::Component& comp) const
{
  return entityTypeHash != RayPicking::NoPickEntityType &&
    static_cast<const EntityData *>(comp.getContainer()->getUserData())->templateId != entityTypeHash;
}
 
 
bool Cogs::Core::MeshRayPickableBase::pickCamera(Context* context,
                                                const CameraComponent& camera,
                                                const glm::vec2& normPosition,
                                                float /*rayLength*/,
                                                float radius,
                                                PickingFlags pickingFlags,
                                                PicksReturned returnFlag,
                                                const RayPicking::RayPickFilter& filter,
                                                std::vector<RayPicking::RayPickHit>& hits)
{
  const CameraData& cameraData = context->cameraSystem->getData(&camera);
 
  const glm::mat4 rawViewProjection = cameraData.rawViewProjection;
 
  const float sx = cameraData.viewportSize.x / radius;   // scale up so that regionSize wrt viewportSize maps to [-1,1]
  const float sy = cameraData.viewportSize.y / radius;
  const glm::mat4 worldPickMatrix = glm::mat4(sx, 0.f, 0.f, 0.f,
                                              0.f, sy, 0.f, 0.f,
                                              0.f, 0.f, 1.0f, 0.f,
                                              -normPosition.x * sx, -normPosition.y * sy, 0.f, 1.f) * rawViewProjection;
  
  // Use the camera position as origin of the ray (maybe should be the near plane but seems accurate enough)
  const glm::mat4 viewMatrix = cameraData.viewMatrix;
  return pickImpl(context, worldPickMatrix, rawViewProjection, viewMatrix, filter, pickingFlags, returnFlag, hits);
}
 
 
bool Cogs::Core::MeshRayPickableBase::pickRay(Context* context,
                                              const glm::vec3& startPos,
                                              const glm::quat& rot,
                                              float rayLength,
                                              float radius,
                                              PickingFlags pickingFlags,
                                              PicksReturned returnFlag,
                                              const RayPicking::RayPickFilter& filter,
                                              std::vector<RayPicking::RayPickHit>& hits)
{
  const float right = radius;
  // const float left = -radius;
  const float top = radius;
  // const float bottom = -radius;
  // const float fNear = 0;
  const float fFar = rayLength;
  // already simplified some operations (2/right - left) == (1/right) given that right == -left
  // term -(f+n)/(f-n); n == 0 => -f/f = -1.
  const glm::mat4 projectionMatrix(
    1 / right, 0, 0, 0,
    0, 1 / top, 0, 0,
    0, 0, -2 / fFar, 0,
    0, 0, -1, 1
  );
 
  const glm::mat4 worldViewMatrix = glm::toMat4(glm::conjugate(rot)) * glm::translate(glm::mat4(1.f), -startPos);
  const glm::mat4 worldPickMatrix = projectionMatrix * worldViewMatrix;
  
  return pickImpl(context, worldPickMatrix, worldPickMatrix, worldViewMatrix, filter, pickingFlags, returnFlag, hits);
}
 
 
Cogs::Core::MeshRayPickableBase::TextureCoordinateInfo
  Cogs::Core::MeshRayPickableBase::getTextureCoordinateInfo(const Cogs::Core::Mesh& mesh)
{
  TextureCoordinateInfo info{};
  for (size_t vertTypeI = 0; vertTypeI < VertexDataType::LastVertexType; ++vertTypeI) {
    VertexDataType::EVertexDataType vertexType = static_cast<VertexDataType::EVertexDataType>(vertTypeI);
    if (mesh.hasStream(vertexType)) {
      const DataStream& stream = mesh.streams[mesh.streamIndexes[vertexType]];
      const Cogs::VertexFormat* vertexFormat = Cogs::VertexFormats::getVertexFormat(stream.format);
      for (const Cogs::VertexElement& element : vertexFormat->elements) {
        if (element.semantic == Cogs::ElementSemantic::TextureCoordinate && element.semanticIndex == 0) {
          info.ptr = static_cast<const uint8_t*>(stream.buffer->data()) + stream.offset + element.offset;
          info.format = element.format;
          info.stride = stream.stride;
          info.count = stream.numElements;
        }
        else if (element.semantic == Cogs::ElementSemantic::TextureCoordinate && element.semanticIndex == 1 && element.inputType == Cogs::InputType::InstanceData) {
 
          // In some cases, we can store the ids in a smaller type by subtracting a fixed offset. This fixed offset
          // is then stored as a per-instance texture coordinate stream that has one element per mesh. Then the
          // actual id is the sum of the per-vertex id and the per-mesh id.
 
          const uint8_t* ptr = static_cast<const uint8_t*>(stream.buffer->data()) + stream.offset + element.offset;
          switch (element.format) {
          case Cogs::Format::R16_UINT:
            info.idOffset = *reinterpret_cast<const uint16_t*>(ptr);
            info.hasIdOffset = true;
            break;
          case Cogs::Format::R32_UINT:
            info.idOffset = *reinterpret_cast<const uint32_t*>(ptr);
            info.hasIdOffset = true;
            break;
          case Cogs::Format::R32_FLOAT:
            info.idOffset = static_cast<uint32_t>(std::round(*reinterpret_cast<const float*>(ptr)));
            info.hasIdOffset = true;
            break;
          default:
            break;
          }
        }
      }
    }
  }
  return info;
}
 
bool Cogs::Core::MeshRayPickableBase::clippedByClipComp(const ClipShapeData& clipData, const glm::vec4& position)
{
  switch (clipData.shape) {
  case ClipShapeType::Cube:
    if (glm::min(glm::min(glm::min(dot(clipData.planes[0], position), dot(clipData.planes[1], position)),
                          glm::min(dot(clipData.planes[2], position), dot(clipData.planes[3], position))),
                glm::min(dot(clipData.planes[4], position), dot(clipData.planes[5], position))) < 0.f) {
      return true;
    }
    break;
 
  case ClipShapeType::InvertedCube:
    if (0.f <= glm::min(glm::min(glm::min(dot(clipData.planes[0], position), dot(clipData.planes[1], position)),
                                glm::min(dot(clipData.planes[2], position), dot(clipData.planes[3], position))),
                        glm::min(dot(clipData.planes[4], position), dot(clipData.planes[5], position)))) {
      return true;
    }
    break;
 
  default:
    break;
  }
 
  return false;
}
 
glm::vec2 Cogs::Core::MeshRayPickableBase::getTextureCoords(const TextureCoordinateInfo& textureInfo, const RayIntersectionHit& hit, const PickingFlags pickingFlags)
{
  bool flatInterpolation = (pickingFlags & PickingFlags::FlatTexcoordInterpolation) == PickingFlags::FlatTexcoordInterpolation;
  bool addInstanceTexcoords = (pickingFlags & PickingFlags::AddInstanceTexcoords) == PickingFlags::AddInstanceTexcoords;
 
  glm::vec2 t0, t1, t2;
  switch (textureInfo.format) {
  case Cogs::DataFormat::R32_FLOAT:
    t0 = glm::vec2(*reinterpret_cast<const float*>(textureInfo.ptr + textureInfo.stride * hit.index.x));
    if (!flatInterpolation) {
      t1 = glm::vec2(*reinterpret_cast<const float*>(textureInfo.ptr + textureInfo.stride * hit.index.y));
      t2 = glm::vec2(*reinterpret_cast<const float*>(textureInfo.ptr + textureInfo.stride * hit.index.z));
    }
    break;
  case Cogs::DataFormat::R32G32_FLOAT:
    t0 = *reinterpret_cast<const glm::vec2*>(textureInfo.ptr + textureInfo.stride * hit.index.x);
    if (!flatInterpolation) {
      t1 = *reinterpret_cast<const glm::vec2*>(textureInfo.ptr + textureInfo.stride * hit.index.y);
      t2 = *reinterpret_cast<const glm::vec2*>(textureInfo.ptr + textureInfo.stride * hit.index.z);
    }
    break;
  case Cogs::DataFormat::R8_UINT:
    t0 = glm::vec2(*reinterpret_cast<const uint8_t*>(textureInfo.ptr + textureInfo.stride * hit.index.x));
    if (!flatInterpolation) {
      t1 = glm::vec2(*reinterpret_cast<const uint8_t*>(textureInfo.ptr + textureInfo.stride * hit.index.y));
      t2 = glm::vec2(*reinterpret_cast<const uint8_t*>(textureInfo.ptr + textureInfo.stride * hit.index.z));
    }
    break;
  case Cogs::DataFormat::R16_UINT:
    t0 = glm::vec2(*reinterpret_cast<const uint16_t*>(textureInfo.ptr + textureInfo.stride * hit.index.x));
    if (!flatInterpolation) {
      t1 = glm::vec2(*reinterpret_cast<const uint16_t*>(textureInfo.ptr + textureInfo.stride * hit.index.y));
      t2 = glm::vec2(*reinterpret_cast<const uint16_t*>(textureInfo.ptr + textureInfo.stride * hit.index.z));
    }
    break;
  case Cogs::DataFormat::R32_UINT:
    t0 = glm::vec2(static_cast<float>(*reinterpret_cast<const uint32_t*>(textureInfo.ptr + textureInfo.stride * hit.index.x)));
    if (!flatInterpolation) {
      t1 = glm::vec2(static_cast<float>(*reinterpret_cast<const uint32_t*>(textureInfo.ptr + textureInfo.stride * hit.index.y)));
      t2 = glm::vec2(static_cast<float>(*reinterpret_cast<const uint32_t*>(textureInfo.ptr + textureInfo.stride * hit.index.z)));
    }
    break;
  case Cogs::DataFormat::Unknown: // No texcoords
    if (addInstanceTexcoords && textureInfo.hasIdOffset) {
      // If we lack per-vertex ids, but per-mesh id has been requested and is available, return that.
      return glm::vec2(static_cast<float>(textureInfo.idOffset));
    }
    return RayPicking::NoPickTextureCoords;
  default:
    LOG_WARNING_ONCE(logger, "Unhandled texcoord format 0x%x", static_cast<uint32_t>(textureInfo.format));
    return RayPicking::NoPickTextureCoords;
    break;
  }
 
  glm::vec2 rv;
  if (flatInterpolation) {
    rv = t0;
  }
  else {
    rv = hit.bary.x * t0 + hit.bary.y * t1 + hit.bary.z * t2;
  }
 
  if (addInstanceTexcoords && textureInfo.hasIdOffset) {
    rv.x =  float(uint32_t(rv.x) + textureInfo.idOffset);
  }
  return rv;
}