MarkerPointSetPicker.cpp

#include "MarkerPointSetPicker.h"
#include "MarkerPointSetSystem.h"
 
#include "Context.h"
#include "Components/Core/SceneComponent.h"
#include "Components/Core/TransformComponent.h"
#include "Scene/RayPick.h"
#include "Services/DPIService.h"
#include "Resources/Mesh.h"
#include "Systems/Core/CameraSystem.h"
#include "Utilities/Math.h"
 
#include "Rendering/IGraphicsDevice.h"
#include "Rendering/ICapabilities.h"
 
#include "Foundation/Logging/Logger.h"
 
namespace
{
  using namespace Cogs::Core;
 
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("MarkerPointSetPicker");
 
 
  struct RecordHit
  {
    std::vector<RayPicking::RayPickHit>& hits;
    const MarkerPointSetComponent& comp;
    const glm::mat4& worldFromLocal;
    const float rayFromLocalScale;
    bool returnClosest;
    bool returnChildEntity;
    bool hitSomething = false;
 
    void operator()(const glm::vec4& posLocal, const float distanceLocal, size_t index) {
      glm::vec4 posWorld = worldFromLocal * posLocal;
      const float distanceWorld = rayFromLocalScale * distanceLocal;
      if (returnClosest && !hits.empty()) {
        if (distanceWorld < hits[0].distance) {
          hits[0] = { comp, returnChildEntity, glm::vec3(posWorld), distanceWorld, glm::vec2(static_cast<float>(index)) };
          hitSomething = true;
        }
        // else, the intersection we found is further, so don't do anything
      }
      else {
        hits.emplace_back(comp, returnChildEntity, glm::vec3(posWorld), distanceWorld, glm::vec2(static_cast<float>(index)));
        hitSomething = true;
      }
    }
 
  };
 
  void pickFixedScreenSizeDisc(RecordHit& recordHit,
                             const MarkerPointSetComponent& comp,
                             const MarkerPointSetData& data,
                             const glm::vec3& rayOriginLocal,
                             const glm::vec3& rayUnitDirectionLocal,
                             const glm::mat4& clipFromLocal,
                             const glm::vec2& queryClip,
                             const glm::vec2 viewportSize,
                             const float rayPixelRadius,
                             const float maxDistanceLocal,
                             const float dpiScale)
  {
    const float nearDistance = comp.nearDistance;
    const float farDistance = std::min(comp.farDistance, maxDistanceLocal);
    const float radius = 0.5f * comp.pointSize;
    const glm::vec4 viewPlaneLocal = data.viewPlaneLocal;
    const glm::vec2 pickPosView = queryClip * viewportSize;
 
    for (size_t i = 0; i < comp.positions.size(); i++) {
      const glm::vec4 posLocal(comp.positions[i], 1.f);
      float viewZ = glm::dot(viewPlaneLocal, posLocal);
 
      const glm::vec3 mainCamViewDirLocal(data.viewPlaneLocal);
      const float rcp = -1.f / glm::dot(mainCamViewDirLocal, rayUnitDirectionLocal);
      const float mp = glm::dot(mainCamViewDirLocal, rayOriginLocal);
      if (nearDistance <= viewZ && viewZ <= farDistance) {
        glm::vec3 pointPosClipSpace = euclidean(clipFromLocal * posLocal);
        glm::vec2 pointPosView = glm::vec2(pointPosClipSpace) * viewportSize;
        const float t = rcp * (mp - glm::dot(mainCamViewDirLocal, glm::vec3(posLocal)));
        float pickableRadius = 2.f * (radius + rayPixelRadius) * dpiScale;           // factor 2 is because we multiply clip pos ([-1,1]) not normed pos ([0,1]).
        if (glm::distance2(pickPosView, pointPosView) <= pickableRadius * pickableRadius) {
          recordHit(posLocal, t, i);
        }
      }
    }
 
  }
 
  void pickViewAlignedDisc(RecordHit& recordHit,
                           const MarkerPointSetComponent& comp,
                           const MarkerPointSetData& data,
                           const glm::vec3& rayOriginLocal,
                           const glm::vec3& rayUnitDirectionLocal,
                           const float rayRadiusBiasLocal,
                           const float rayRadiusSlopeLocal,
                           const float maxDistanceLocal)
  {
    const float nearDistance = comp.nearDistance;
    const float farDistance = std::min(comp.farDistance, maxDistanceLocal);
    const float radius = 0.5f * comp.pointSize + rayRadiusBiasLocal;
    const glm::vec3 mainCamViewDirLocal(data.viewPlaneLocal);
    const float rcp = -1.f / glm::dot(mainCamViewDirLocal, rayUnitDirectionLocal);
    const float mp = glm::dot(mainCamViewDirLocal, rayOriginLocal);
    if (std::isfinite(rcp)) {
      for (size_t i = 0; i < comp.positions.size(); i++) {
        const glm::vec4 posLocal(comp.positions[i], 1.f);
        float viewZ = glm::dot(data.viewPlaneLocal, posLocal);
        if (nearDistance <= viewZ && viewZ <= farDistance) {
          const float t = rcp * (mp - glm::dot(mainCamViewDirLocal, glm::vec3(posLocal)));
          glm::vec3 r = (rayOriginLocal - glm::vec3(posLocal)) + t * rayUnitDirectionLocal;
          const float adjustedRadius = radius + t * rayRadiusSlopeLocal;
          if (dot(r, r) <= adjustedRadius * adjustedRadius) {
            recordHit(posLocal, t, i);
          }
        }
      }
    }
  }
 
  void pickObjectSpaceFloorDisc(RecordHit& recordHit,
                                const MarkerPointSetComponent& comp,
                                const MarkerPointSetData& data,
                                const glm::vec3& rayOriginLocal,
                                const glm::vec3& rayUnitDirectionLocal,
                                const float rayRadiusBiasLocal,
                                const float rayRadiusSlopeLocal,
                                const float maxDistanceLocal)
  {
    const float nearDistance = comp.nearDistance;
    const float farDistance = std::min(comp.farDistance, maxDistanceLocal);
    const float pointRadius = 0.5f * comp.pointSize;
    const float rcp = -1.f / rayUnitDirectionLocal.z;
    if (std::isfinite(rcp)) {
      for (size_t i = 0; i < comp.positions.size(); i++) {
        const glm::vec4 posLocal(comp.positions[i], 1.f);
        float viewZ = glm::dot(data.viewPlaneLocal, posLocal);
        if (nearDistance <= viewZ && viewZ <= farDistance) {
 
          // Calculate ray radius at point
          const glm::vec3 o3 = rayOriginLocal - glm::vec3(posLocal);
          const float rayRadiusAtPoint = rayRadiusBiasLocal + rayRadiusSlopeLocal * glm::dot(rayUnitDirectionLocal, -o3);
          const float testRadius = pointRadius + rayRadiusAtPoint;
 
          if (rayRadiusAtPoint < 0.1f * pointRadius) {
            // If ray radius is really small or zero, intersect directly with disc
 
            const float t = rcp * (rayOriginLocal.z - posLocal.z);
            const glm::vec2 r = (glm::vec2(rayOriginLocal) - glm::vec2(posLocal)) + t * glm::vec2(rayUnitDirectionLocal);
 
            if (dot(r, r) <= testRadius * testRadius) {
              recordHit(posLocal, t, i);
            }
          }
          else {
 
            // Otherwise we extrude the disc to a capped cylinder to make it easier to hit gracing discs.
            const glm::vec2 r(rayUnitDirectionLocal);
            const glm::vec2 o = glm::vec2(rayOriginLocal) - glm::vec2(posLocal);
 
            // Intersect with infinite cylinder
            const float a = glm::dot(r, r);
            const float b = 2.f * glm::dot(o, r);
            const float c = glm::dot(o, o) - testRadius * testRadius;
            const float desc = b * b - 4.f * a * c;
            if (desc < 0.f) {
              continue; // No solutions
            }
 
            // We have a pair of solutions
            const float r0 = (-b - std::copysign(std::sqrt(desc), b)) / (2.f * a);
            const float r1 = c / (a * r0);
            float t0 = std::min(r0, r1);
            float t1 = std::max(r0, r1);
 
            // Intersect with two planes that cap the infinite cylinder
            if (float p_rcp = 1.f / rayUnitDirectionLocal.z; std::isfinite(p_rcp)) {
              float p_r0 = -p_rcp * (o3.z - rayRadiusAtPoint);
              float p_r1 = -p_rcp * (o3.z + rayRadiusAtPoint);
              t0 = std::max(t0, std::min(p_r0, p_r1));
              t1 = std::min(t1, std::max(p_r0, p_r1));
            }
            else if (o3.z < -rayRadiusAtPoint || rayRadiusAtPoint < o3.z) {
              // Ray dir lies in cap plane and ray origin is outside of slab
              continue;
            }
            if (t0 < t1) {
              recordHit(posLocal, t0, i);
            }
          }
        }
      }
    }
  }
 
}
 
 
bool Cogs::Core::MarkerPointSetPicker::pickCamera(Context* context,
                                                  const CameraComponent& camera,
                                                  const glm::vec2& queryClip,
                                                  float rayLength,
                                                  float rayPixelRadius,
                                                  PickingFlags pickingFlags,
                                                  PicksReturned returnFlag,
                                                  const RayPicking::RayPickFilter& filter,
                                                  std::vector<RayPicking::RayPickHit>& hits)
{
  const CameraData& cameraData = context->cameraSystem->getData(&camera);
  const float dpiScale = context->getDefaultView()->dpiService->getScaleFactor();
 
  bool hitSomething = false;
  for (const MarkerPointSetComponent& comp : system->pool) {
    if (filter.isUnwantedType(comp)) {
      continue;
    }
 
    const SceneComponent* sceneComp = comp.getComponent<SceneComponent>();
    if (sceneComp && (!sceneComp->visible || !sceneComp->pickable)) {
      continue;
    }
 
    const RenderComponent* renderComp = comp.getComponent<RenderComponent>();
    if (renderComp && !renderComp->isVisibleInLayer(filter.layerMask)) {
      continue;
    }
 
    const MarkerPointSetData& data = system->getData(&comp);
 
    const TransformComponent* transformComp = comp.getComponent<TransformComponent>();
    const glm::mat4 worldFromLocal = context->transformSystem->getLocalToWorld(transformComp);
    const glm::mat4 clipFromLocal = cameraData.rawViewProjection * worldFromLocal;
    const glm::mat4 localFromClip = glm::inverse(clipFromLocal);
    const glm::vec2 sigma = glm::vec2(2.f * rayPixelRadius) / cameraData.viewportSize;
    const glm::vec3 rayOriginLocal = euclidean(localFromClip * glm::vec4(queryClip, -1.f, 1.f));
    const glm::vec3 rayOriginFarLocal = euclidean(localFromClip * glm::vec4(queryClip, 1.f, 1.f));
    const glm::vec3 rayDirLocal = rayOriginFarLocal - rayOriginLocal;
    const glm::vec3 rayOriginSigmaLocal = euclidean(localFromClip * glm::vec4(queryClip + sigma, -1.f, 1.f));
    const glm::vec3 rayOriginSigmaFarLocal = euclidean(localFromClip * glm::vec4(queryClip + sigma, 1.f, 1.f));
    const float rayNearSpread = glm::distance(rayOriginLocal, rayOriginSigmaLocal);
    const float rayFarSpread = glm::distance(rayOriginFarLocal, rayOriginSigmaFarLocal);
    const float rayLengthRcp = 1.f / glm::length(rayDirLocal);
    const float rayRadiusBiasLocal = rayNearSpread;
    const float rayRadiusSlopeLocal = rayLengthRcp * (rayFarSpread - rayNearSpread);
    const glm::vec3 rayUnitDirLocal = rayLengthRcp * rayDirLocal;
    const float worldFromLocalScale = glm::determinant(glm::mat3(worldFromLocal));
 
    // The min below is to work-around local scale becoming infinity if raylength is max_float.
    const float maxDistanceLocal = std::min(std::numeric_limits<float>::max(), 2.f * worldFromLocalScale * rayLength);
 
    RecordHit recordHit{
      .hits = hits,
      .comp = comp,
      .worldFromLocal = worldFromLocal,
      .rayFromLocalScale = worldFromLocalScale,
      .returnClosest = returnFlag == PicksReturned::Closest,
      .returnChildEntity = (pickingFlags & PickingFlags::ReturnChildEntity) == PickingFlags::ReturnChildEntity
    };
 
    switch (comp.shape) {
    case MarkerPointSetShape::ViewAlignedDisc:
      pickViewAlignedDisc(recordHit, comp, data, rayOriginLocal, rayUnitDirLocal, rayRadiusBiasLocal, rayRadiusSlopeLocal, maxDistanceLocal);
      break;
 
    case MarkerPointSetShape::ObjectSpaceFloorDisc:
      pickObjectSpaceFloorDisc(recordHit, comp, data, rayOriginLocal, rayUnitDirLocal, rayRadiusBiasLocal, rayRadiusSlopeLocal, maxDistanceLocal);
      break;
 
    case MarkerPointSetShape::FixedScreenSizeDisc:
      pickFixedScreenSizeDisc(recordHit, comp, data, rayOriginLocal, rayUnitDirLocal, clipFromLocal, queryClip, cameraData.viewportSize, rayPixelRadius, maxDistanceLocal, dpiScale);
      break;
 
    default:
      assert(false && "Illegal enum value");
    }
 
    hitSomething |= recordHit.hitSomething;
  }
 
  if (returnFlag == PicksReturned::AllSorted) {
    std::sort(hits.begin(), hits.end());
  }
 
  return hitSomething;
}
 
bool Cogs::Core::MarkerPointSetPicker::pickRay(Context* context,
                                               const glm::vec3& startPos,
                                               const glm::quat& rot,
                                               float rayLength,
                                               float rayRadius,
                                               PickingFlags pickingFlags,
                                               PicksReturned returnFlag,
                                               const RayPicking::RayPickFilter& filter,
                                               std::vector<RayPicking::RayPickHit>& hits)
{
  glm::mat4 worldFromRay = glm::mat4(1.f, 0.f, 0.f, 0.f,
                                     0.f, 1.f, 0.f, 0.f,
                                     0.f, 0.f, 1.f, 0.f,
                                     startPos.x, startPos.y, startPos.z, 1.f) * glm::toMat4(rot);
 
  bool hitSomething = false;
  for (const MarkerPointSetComponent& comp : system->pool) {
    if (filter.isUnwantedType(comp)) {
      continue;
    }
 
    const SceneComponent* sceneComp = comp.getComponent<SceneComponent>();
    if (sceneComp && (!sceneComp->visible || !sceneComp->pickable)) {
      continue;
    }
 
    const RenderComponent* renderComp = comp.getComponent<RenderComponent>();
    if (renderComp && !renderComp->isVisibleInLayer(filter.layerMask)) {
      continue;
    }
 
    const MarkerPointSetData& data = system->getData(&comp);
 
    const TransformComponent* transformComp = comp.getComponent<TransformComponent>();
    const glm::mat4 worldFromLocal = context->transformSystem->getLocalToWorld(transformComp);
    const glm::mat4 localFromWorld = glm::inverse(worldFromLocal);
    const float worldFromLocalScale = glm::determinant(glm::mat3(worldFromLocal));
 
    const glm::vec3 rayOriginLocal = euclidean(localFromWorld * (worldFromRay * glm::vec4(0, 0, 0, 1)));
    const glm::vec3 rayOriginFarLocal = euclidean(localFromWorld * (worldFromRay * glm::vec4(0, 0, -1, 1)));
    const glm::vec3 rayUnitDirLocal = normalize(rayOriginFarLocal - rayOriginLocal);
    const float rayRadiusBiasLocal = rayRadius / worldFromLocalScale;
    const float rayRadiusSlopeLocal = 0.f;
 
    // The min below is to work-around local scale becoming infinity if raylength is max_float.
    const float maxDistanceLocal = std::min(std::numeric_limits<float>::max(), 2.f * worldFromLocalScale * rayLength);
 
    RecordHit recordHit{
      .hits = hits,
      .comp = comp,
      .worldFromLocal = worldFromLocal,
      .rayFromLocalScale = worldFromLocalScale,
      .returnClosest = returnFlag == PicksReturned::Closest,
      .returnChildEntity = (pickingFlags & PickingFlags::ReturnChildEntity) == PickingFlags::ReturnChildEntity
    };
 
    switch (comp.shape) {
    case MarkerPointSetShape::ViewAlignedDisc:
      pickViewAlignedDisc(recordHit, comp, data, rayOriginLocal, rayUnitDirLocal,  rayRadiusBiasLocal, rayRadiusSlopeLocal, maxDistanceLocal);
      break;
 
    case MarkerPointSetShape::ObjectSpaceFloorDisc:
      pickObjectSpaceFloorDisc(recordHit, comp, data, rayOriginLocal, rayUnitDirLocal, rayRadiusBiasLocal, rayRadiusSlopeLocal, maxDistanceLocal);
      break;
 
    case MarkerPointSetShape::FixedScreenSizeDisc:
      break;
 
    default:
      assert(false && "Illegal enum value");
    }
 
    hitSomething |= recordHit.hitSomething;
}
 
  if (returnFlag == PicksReturned::AllSorted) {
    std::sort(hits.begin(), hits.end());
  }
 
  return hitSomething;
}