HighlightRegionPicker.cpp

#include "HighlightRegionPicker.h"
#include "Systems/Core/CameraSystem.h"
#include "Systems/Core/TransformSystem.h"
#include "Utilities/Math.h"
#include "Context.h"
 
#include "HighlightRegionSystem.h"
 
#include <algorithm>
 
namespace {
  using namespace Cogs::Core;
 
  bool pickCameraImpl(Context* context,
                      HighlightRegionSystem& hrSystem,
                      const glm::vec3& rayNear_engine,
                      const glm::vec3& rayFar_engine,
                      const glm::mat4* viewMatrix,
                      const float rayLength,  // ray length in engine frame
                      const RayPicking::RayPickFilter& filter,
                      std::vector<RayPicking::RayPickHit>& hits)
  {
    if (hits.empty()) return false; // No hits to modify
 
    bool hitSomething = false;
    bool haveSorted = false;
    for (const HighlightRegionComponent& hrComp : hrSystem.pool) {
      if (!hrComp.isPickable() || filter.isUnwantedType(hrComp)) { continue; }
 
      const RenderComponent* renderComp = hrComp.getComponent<RenderComponent>();
      if (renderComp == nullptr || !renderComp->isVisibleInLayer(filter.layerMask)) { continue; }
 
      const HighlightRegionData& hrData = hrSystem.getData(&hrComp);
      const TransformComponent* trComp_hr = hrComp.getComponent<TransformComponent>();
      const glm::mat4& engineFromLocal_hr = context->transformSystem->getLocalToWorld(trComp_hr);
      const glm::mat4 localFromEngine_hr = glm::inverse(engineFromLocal_hr);
 
      const glm::vec3 nPos_local = localFromEngine_hr * glm::vec4(rayNear_engine, 1.f);
      const glm::vec3 fPos_local = localFromEngine_hr * glm::vec4(rayFar_engine, 1.f);
      const glm::vec3 dir_local = glm::normalize(fPos_local - nPos_local);
 
      RayPicking::Ordinal ordinal = filter.getOrdinal(*renderComp);
 
      size_t N = std::min(hrComp.position.size(), hrComp.scale.size());
      for (size_t i = 0; i < N; i++) {
        const glm::vec3 a = hrComp.position[i] - nPos_local;
        const float proj_dir_a = dot(a, dir_local);
        const glm::vec3 nearestPointOnLine = proj_dir_a * dir_local;
 
        constexpr float sqrt3 = 1.74f;
        const float d2 = glm::distance2(a, nearestPointOnLine);
        const float s = sqrt3 * std::max(hrComp.scale[i].x, std::max(hrComp.scale[i].y, hrComp.scale[i].z));
        if (s * s < d2) continue;
 
        const HighlightRegionInstanceData& instance = hrData.instanceData[i];
 
        glm::mat4 localFromInstance = glm::transpose(glm::mat4(instance.data0[0],
                                                               instance.data0[1],
                                                               instance.data0[2],
                                                               glm::vec4(0, 0, 0, 1)));
 
        glm::mat4 instanceFromLocal = glm::mat4(glm::transpose(glm::mat3(localFromInstance)));
        instanceFromLocal[3] = glm::vec4(-glm::mat3(instanceFromLocal) * glm::vec3(localFromInstance[3]), 1.f);
 
        glm::vec3 scale = glm::vec3(instance.data1[0]);
 
        glm::vec3 npos_instance = glm::vec3(instanceFromLocal * glm::vec4(nPos_local, 1.f));
        glm::vec3 fpos_instance = glm::vec3(instanceFromLocal * glm::vec4(fPos_local, 1.f));
 
        glm::vec3 dir = fpos_instance - npos_instance;
        glm::vec3 invDir = 1.f / dir;
 
        glm::vec3 nHits = invDir * (-glm::sign(dir) * scale - npos_instance);
        glm::vec3 fHits = invDir * ( glm::sign(dir) * scale - npos_instance);
 
        float t_n = std::max(nHits.x, std::max(nHits.y, nHits.z));
        float t_f = std::min(fHits.x, std::min(fHits.y, fHits.z));
        if (t_f < t_n) continue;
 
 
        float enter, exit;
        if (viewMatrix) {
          glm::vec4 n = *viewMatrix * glm::vec4((1.f - t_n) * rayNear_engine + t_n * rayFar_engine, 1.f);
          glm::vec4 f = *viewMatrix * glm::vec4((1.f - t_f) * rayNear_engine + t_f * rayFar_engine, 1.f);
          enter = -n.z;
          exit = -f.z;
        }
        else {
          enter = rayLength * t_n;
          exit = rayLength * t_f;
        }
 
        // First hit, sort hits so we can do binary search.
        if (!haveSorted) {
          std::sort(hits.begin(), hits.end());
          haveSorted = true;
        }
        if (auto it = std::lower_bound(hits.begin(), hits.end(), enter,
                                         [ordinal](const RayPicking::RayPickHit& hit, const float& distance)
                                         {
                                           // hit.distance < distance;
                                           return hit.isInfront(ordinal, distance);
                                         }); it != hits.end())
        {
          //                         it->distance <= exit
          for (; it != hits.end() && !it->isBehind(ordinal, exit); ++it) {
            it->textureCoords.y = static_cast<float>(hrComp.id[i]);
            hitSomething = true;
          }
        }
      }
    }
 
    return hitSomething;
  }
 
 
}
 
bool Cogs::Core::HighlightRegionPicker::pickCamera(Context* context,
                                                   const CameraComponent& camComp,
                                                   const glm::vec2& normPosition,
                                                   float /*rayLength*/,
                                                   float /*radius*/,
                                                   PickingFlags /*pickingFlags*/,
                                                   PicksReturned /*returnFlag*/,
                                                   const RayPicking::RayPickFilter& filter,
                                                   std::vector<RayPicking::RayPickHit>& hits)
{
  if (!hrSystem) return false;
 
  const CameraData& camData = context->cameraSystem->getData(&camComp);
  glm::vec3 rayNear_engine = euclidean(camData.inverseViewProjectionMatrix * glm::vec4(normPosition.x, normPosition.y, -1.f, 1.f));
  glm::vec3 rayFar_engine  = euclidean(camData.inverseViewProjectionMatrix * glm::vec4(normPosition.x, normPosition.y,  1.f, 1.f));
  float rayLength = glm::length(rayFar_engine - rayNear_engine);
  return pickCameraImpl(context, *hrSystem, rayNear_engine, rayFar_engine, &camData.viewMatrix, rayLength, filter, hits);
}
 
bool Cogs::Core::HighlightRegionPicker::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)
{
  if (!hrSystem) return false;
  const glm::vec3 endPos = startPos - rot * glm::vec3(0, 0, rayLength);
  return pickCameraImpl(context, *hrSystem, startPos, endPos, nullptr, rayLength, filter, hits);
}