TrajectorySystem.cpp

#include "TrajectorySystem.h"
 
#include "Context.h"
 
#include "Components/Core/LodComponent.h"
#include "Components/Data/TrajectoryComponent.h"
 
#include "Systems/Core/CameraSystem.h"
#include "Systems/Core/TransformSystem.h"
#include "Systems/Core/LodSystem.h"
 
#include "Utilities/Math.h"
#include "Utilities/TransformVertices.h"
 
#include "Foundation/ComponentModel/Entity.h"
#include "Foundation/Logging/Logger.h"
 
#define _USE_MATH_DEFINES
#include <cmath>
 
using namespace Cogs::Core;
 
namespace {
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("TrajectorySystem");
 
  glm::vec3 removeLinearDepenence(const glm::vec3& vec, const glm::vec3& normalizedRef)
  {
    return vec - glm::dot(vec, normalizedRef)*normalizedRef;
  }
 
  glm::vec3 pickAnyDirectionBut(const glm::vec3& but)
  {
    glm::vec3 t = glm::abs(but);
    glm::vec3 r;
    if ((t.x > t.y) && (t.x > t.z)) {
      r = glm::vec3(0.f, 0.f, 1.f);  // x is smallest largest, choose z
    }
    else if (t.x > t.y) {
      r = glm::vec3(0.f, 1.f, 0.f); // t.z is largest component, choose y
    }
    else {
      r = glm::vec3(1.f, 0.f, 0.f); // otherwise choose x
    }
    return glm::normalize(removeLinearDepenence(r, but));
  }
}
 
namespace {
  template<GeometricErrorKind K> struct GeometricErrorKindTraits;
 
  template<> struct GeometricErrorKindTraits < GeometricErrorKind::DistanceBased >
  {
    inline static float bakedTolerance(float sourceTolerance) { return sourceTolerance; }
    inline static float error(const glm::vec2& /*ab*/, const glm::vec2&ac, const glm::vec2&n)
    { return glm::abs(glm::dot(ac, n));}
  };
 
  template<> struct GeometricErrorKindTraits < GeometricErrorKind::AreaBased >
  {
    inline static float bakedTolerance(float sourceTolerance) { return sourceTolerance*sourceTolerance; }
    inline static float error(const glm::vec2&ab, const glm::vec2&ac, const glm::vec2& /*n*/)
    { return 0.5f*glm::abs(ab.x*ac.y - ac.x*ab.y); }
  };
}
 
template<GeometricErrorKind K>
void selectLodSubset(std::vector<bool>& currInclude,
                     const std::vector<bool>& prevInclude,
                     const std::vector<glm::vec4> positions,
                     const glm::vec2& viewportSize,
                     const float distanceTolerance,
                     const float previousFrameStickyness)
{
  float tolerance = GeometricErrorKindTraits<K>::bakedTolerance(distanceTolerance);
  float unstickyness = glm::clamp(1.f - previousFrameStickyness, 0.001f, 0.999f);
 
  const size_t N = positions.size();
  std::vector<glm::vec2> screenPosition(N);
  std::vector<std::pair<size_t,size_t>> ranges;
 
  currInclude.clear();
  currInclude.resize(N);
 
  bool prevInsideState = false;
  size_t rangeStart = 0;
  for(size_t i=0; i<=N; i++) {
    // Check if we're behind or in front of the near-plane. We do an
    // extra iteration which fails the inside-test at the end to
    // purge any ongoing intervals.
    bool currInsideState = i < N ? positions[i].z >= -positions[i].w : false;
 
    if (prevInsideState != currInsideState) {
      prevInsideState = currInsideState;
      currInclude[i == 0 ? 0 : i - 1] = true; // Make sure that samples on both sides
      currInclude[i < N ? i : N - 1] = true;  // of the transition zone are included
 
      if (currInsideState) {              // we're entering the frustum
        rangeStart = i;
      }
      else {                              // we're exiting the frustum
        ranges.emplace_back(rangeStart, i - 1);
      }
    }
 
    // And we find the screen-space position for all points behind near.
    if(currInsideState) {
      const glm::vec4& p = positions[i];
      screenPosition[i] = (0.5f / p.w)* viewportSize * glm::vec2(p.x, p.y);
    }
  }
  // Now, all the indices being part of a line segment intersecting the near-
  // plane are tagged for inclusion, as well as all intervals of line segments
  // behind the near plane is stored in in ranges.
 
  // Next step is to subdivide ranges and tag split points until the criteria
  // is met. We find the most offending index in the range, insert this point
  // and refine, until we are happy.
  while (!ranges.empty()) {
 
    const std::pair<size_t, size_t> r = ranges.back();
    ranges.pop_back();
 
    // We run through the interval searching for the most offending index,
    // and if that index is not offending enough, we're done with that
    // interval.
    if (r.first + 1 < r.second) {
      size_t prevSplit = 0;
      size_t currSplit = 0;
      float prevError = -std::numeric_limits<float>::max();
      float currError = -std::numeric_limits<float>::max();
 
      glm::vec2 ab = screenPosition[r.second] - screenPosition[r.first];
      glm::vec2 n = glm::normalize(glm::vec2(-ab.y, ab.x));
      for (size_t i = r.first + 1; i < r.second; i++) {
        glm::vec2 ac = screenPosition[i] - screenPosition[r.first];
 
        float error = GeometricErrorKindTraits<K>::error(ab, ac, n);
 
        if (currError < error) {  // Update overall most offending
          currSplit = i;          // index in interval
          currError = error;
        }
 
        if ((prevError < error) && prevInclude[i]) {  // Update most offending
          prevSplit = i;                              // index that was included
          prevError = error;                          // in the previous frame.
        }
      }
 
      if (tolerance < currError) {  // here we assume that tolerance
        // is a non-negative value
 
        // Now, if the error is almost the same by using an indexed that was
        // included the previous frame, we do that, and reduce popping artifacts.
        if (unstickyness*currError < prevError) {
          currSplit = prevSplit;
        }
 
        // And split. Only add ranges if they have interior indices.
        currInclude[currSplit] = true;
        if (r.first + 1 < currSplit) {
          ranges.emplace_back(r.first, currSplit);
        }
        if (currSplit + 1 < r.second) {
          ranges.emplace_back(currSplit, r.second);
        }
 
      }
    }
  }
}
 
void Cogs::Core::TrajectorySystem::generateAnchoredFramesOfReference(std::vector<glm::quat>& orientations,
                                                                     const std::vector<float>& indices,
                                                                     const std::vector<glm::vec3>& positions,
                                                                     const glm::vec3& startAnchor,
                                                                     const glm::vec3& stopAnchor,
                                                                     const float anchorStickyness)
{
  const size_t N = positions.size();
  orientations.resize(N);
  if (N < 2) {
    return;
  }
 
  // calculate directions for each position
  std::vector<glm::vec3> directions(N);
  for (size_t i = 0; i < N - 1; i++) {                                  // calculate tangents
    directions[i] = positions[i + 1] - positions[i];
  }
  directions[N - 1] = directions[N - 2];                                        // duplicate at end
  for (size_t i = N - 2; i > 0; i--) {                                  // average at inner indices backwards
    directions[i] = glm::normalize(directions[i] + directions[i - 1]);
  }
  directions[0] = glm::normalize(directions[0]);                // normalize end points
  directions[N - 1] = glm::normalize(directions[N - 1]);
 
  std::vector<float> debug(N, -43.f);
  std::vector<glm::vec3> anchors(N);
  anchors[0] = glm::normalize(removeLinearDepenence(startAnchor, directions[0]));
  anchors[N-1] = glm::normalize(removeLinearDepenence(stopAnchor, directions[N - 1]));
 
  float to = indices[0];
  float ts = 1.f / (indices[N - 1] - indices[0]);
  float linearDepenenceTolerance = glm::clamp(1.f - anchorStickyness, 0.01f, 1.f);
 
  size_t prevIndependent = 0;
  for (size_t i = 1; i < N; ) {
    glm::vec3 interpolatedAnchor = glm::mix(startAnchor, stopAnchor, ts*(indices[i] - to));
    glm::vec3 orthogonalAnchor = removeLinearDepenence(interpolatedAnchor, directions[i]);
 
    if (linearDepenenceTolerance < glm::length(orthogonalAnchor)) {
      // All well with the interpolated anchor
      anchors[i] = glm::normalize(orthogonalAnchor);
      prevIndependent = i;
      //debug[i] = -42.f;
      // And skip forward
      i++;
    }
    else {
      // Anchor is or is close linear dependent on tangent, search forward
      // until we get something decent.
      size_t nextIndependent = N - 1;
      for (size_t k = i + 1; k < N - 1; k++) {
        glm::vec3 interpolatedAnchor_ = glm::mix(startAnchor, stopAnchor, ts*(indices[k] - to));
        glm::vec3 orthogonalAnchor_ = removeLinearDepenence(interpolatedAnchor_, directions[k]);
        if (linearDepenenceTolerance < glm::length(orthogonalAnchor_)) {
          anchors[k] = glm::normalize(orthogonalAnchor_);
          //debug[i] = -44.f;
          nextIndependent = k;
          break;
        }
      }
 
 
      // Try to create a plane in which the curve lies. We use the normal (or the flip
      // of the normal) of this plane to define and small detour for the anchor.
      size_t halfWay = (prevIndependent + nextIndependent) / 2;
      glm::vec3 va = glm::normalize(positions[nextIndependent] - positions[prevIndependent]);
      glm::vec3 detour = glm::cross(positions[halfWay] - positions[prevIndependent], va);
      if (glm::length(detour) < std::numeric_limits<float>::epsilon()) {
        // Failed to create a plane, we settle for anything orthogonal to the midpoint tangent.
        detour = pickAnyDirectionBut(directions[halfWay]);
      }
      else {
        // We do have a candidate...!
        detour = glm::normalize(detour);
 
        // Then we check if the twist (if any) of the unconstrained rotation
        // aligns better with n or -n.
        glm::quat unconstrainedRotation = Cogs::Core::getRotation(directions[prevIndependent], directions[halfWay]);
        if (glm::dot(unconstrainedRotation * anchors[prevIndependent], detour) < 0.f) {
          detour = -detour;
        }
      }
 
      // And do a quadratic interpolation through the detour
      for (size_t k = prevIndependent+1; k < nextIndependent; k++) {
        float t_k = ts*(indices[k] - indices[prevIndependent]) / (indices[nextIndependent] - indices[prevIndependent]);
        glm::vec3 twistAnchor = ((1.f - t_k)*(1.f - t_k)*anchors[prevIndependent] +
                                 2.f*(1.f - t_k)*t_k*detour +
                                 t_k*t_k*anchors[nextIndependent]);
        anchors[k] = glm::normalize(removeLinearDepenence(twistAnchor, directions[k]));
        debug[k] = t_k;
      }
 
      // and finally skip forward
      prevIndependent = nextIndependent;
      i = nextIndependent+1;
    }
  }
 
  // Create rotations
  for (size_t i = 0; i < N; i++) {
 
    // Rotation for aligning local Z with tangent
    glm::quat alignZ;
    if (i == 0) {
      alignZ = Cogs::Core::getRotation(glm::vec3(0.f, 0.f, 1.f), directions[0]);
    }
    else {
      alignZ = Cogs::Core::getRotation(directions[i - 1], directions[i])*orientations[i - 1];
    }
 
    // Roll-correction to align local Y with predefined anchor
    glm::quat alignY = Cogs::Core::getRotation(alignZ * glm::vec3(0.f, 1.f, 0.f), anchors[i]);
 
    orientations[i] = alignY*alignZ;
  }
}
 
 
void Cogs::Core::TrajectorySystem::generateFramesOfReference(std::vector<glm::quat>& orientations,
                                                             const std::vector<glm::vec3>& positions)
{
  const size_t N = positions.size();
  orientations.resize(N);
  if (N < 2) {
    return;
  }
 
 
  // calculate directions for each position
  std::vector<glm::vec3> directions(N);
  for (size_t i = 0; i < N - 1; i++) {                                  // calcualte tangents
    directions[i] = positions[i + 1] - positions[i];
  }
  directions[N - 1] = directions[N - 2];                                        // duplicate at end
  for (size_t i = N - 2; i > 0; i--) {                                  // average at inner indices backwards
    directions[i] = glm::normalize(directions[i] + directions[i - 1]);
  }
  directions[0] = glm::normalize(directions[0]);                // normalize end points
  directions[N - 1] = glm::normalize(directions[N - 1]);
 
  // incrementally create a sequence of orientations.
  orientations[0] = Cogs::Core::getRotation(glm::vec3(0.f, 0.f, 1.f), directions[0]);
  for (size_t i = 1; i < N; i++) {
    orientations[i] = Cogs::Core::getRotation(directions[i - 1], directions[i])*orientations[i - 1];
  }
}
 
void Cogs::Core::TrajectorySystem::update(Context* context)
{
  const auto & cameraComponent = context->cameraSystem->getMainCamera();
 
  for (TrajectoryComponent& trajComp : pool) {
    TrajectoryData& trajData = getData(&trajComp);
 
    if (trajComp.hasChanged()) {
 
      float l2a = glm::dot(trajComp.startRadialAnchor, trajComp.startRadialAnchor);
      float l2b = glm::dot(trajComp.startRadialAnchor, trajComp.stopRadialAnchor);
      if (std::isnormal(l2a) && std::isnormal(l2b)) {
        generateAnchoredFramesOfReference(trajData.frames, trajComp.indexes, trajComp.positions,
                                          trajComp.startRadialAnchor, trajComp.stopRadialAnchor,
                                          trajComp.anchorStickyness);
      }
      else {
        generateFramesOfReference(trajData.frames, trajComp.positions);
      }
    }
 
    
    LodComponent* lodComp = trajComp.getComponent<LodComponent>();
 
    if (lodComp) {
      if (lodComp->policy == LodPolicy::GeometricTolerance) {
        const LodData & lodData = context->lodSystem->getData<LodData>(lodComp);
 
        // Transform to clip space
        trajData.clipSpacePos.resize(trajComp.positions.size());
        Cogs::Core::transformVertices(context,
                                      trajData.clipSpacePos,
                                      lodData.localToClip,
                                      trajComp.positions);
 
        bool hasChanged = false;
        std::vector<bool> lodSelection(trajComp.positions.size(), false);
        if (trajComp.hasChanged() || (trajData.lodSelection.size() != trajComp.positions.size())) {
          trajData.lodSelection.clear();
          trajData.lodSelection.resize(trajComp.positions.size());
          hasChanged = true;
        }
 
        if (true) { // if anything that concerns the projection has changed or lod'ing
 
          switch (context->lodSystem->geometricErrorKind) {
          case GeometricErrorKind::AreaBased:
            selectLodSubset<GeometricErrorKind::AreaBased>(lodSelection, trajData.lodSelection, trajData.clipSpacePos,
                                                           cameraComponent->viewportSize, lodComp->geometricTolerance,
                                                           context->lodSystem->previousFrameStickyness);
            break;
          case GeometricErrorKind::DistanceBased:
            selectLodSubset<GeometricErrorKind::DistanceBased>(lodSelection, trajData.lodSelection, trajData.clipSpacePos,
                                                               cameraComponent->viewportSize, lodComp->geometricTolerance,
                                                               context->lodSystem->previousFrameStickyness);
            break;
          }
 
          if (!hasChanged) {
            hasChanged = !std::equal(lodSelection.begin(), lodSelection.end(),
                                    trajData.lodSelection.begin());
          }
 
          if (hasChanged) {
            trajData.lodSelection.swap(lodSelection);
            trajComp.setChangedTransient(); // notify rest of systems down the chain, but don't ping-pong
          }
        }
      }
      else {
        if (lodComp->hasChanged() || (trajData.lodSelection.size() != trajData.frames.size())) {
          trajData.lodSelection.clear();
          trajData.lodSelection.resize(trajData.frames.size(), true);
          trajComp.setChangedTransient();
        }
      }
    }
  }
}