libs/vr/libposepredictor/predictor.cpp

#include <linear_predictor.h>
#include <polynomial_predictor.h>
#include <predictor.h>

namespace posepredictor {

vec3 Predictor::AngularVelocity(const quat& a, const quat& b, real delta_time) {
  const auto delta_q = a.inverse() * b;
  // Check that delta_q.w() == 1, Eigen doesn't respect this convention. If
  // delta_q.w() == -1, we'll get the opposite velocity.
  return 2.0 * (delta_q.w() < 0 ? static_cast<vec3>(-delta_q.vec()) : delta_q.vec()) / delta_time;
}

Velocity Predictor::PredictVelocity(int64_t time_ns) const {
  const auto a = Predict(time_ns - kFiniteDifferenceNs);
  const auto b = Predict(time_ns + kFiniteDifferenceNs);
  const auto delta_time = NsToSeconds(2 * kFiniteDifferenceNs);

  return {(b.position - a.position) / delta_time,
          AngularVelocity(a.orientation, b.orientation, delta_time)};
}

// The factory method.
std::unique_ptr<Predictor> Predictor::Create(PredictorType type) {
  switch (type) {
    case PredictorType::Linear:
      return std::make_unique<LinearPosePredictor>();
    case PredictorType::Quadric:
      return std::make_unique<QuadricPosePredictor>();
    case PredictorType::Cubic:
      return std::make_unique<CubicPosePredictor>();
  }
}
}  // namespace posepredictor