SequenceInterpolator.h

Go to the documentation of this file.

/*
 * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rtc/logging.h>
#include <mc_trajectory/LinearInterpolation.h>
 
#include <algorithm>
 
namespace mc_trajectory
{
 
template<typename Value, typename InterpolationFunction = LinearInterpolation<Value>>
struct SequenceInterpolator
{
  using TimedValue = typename std::pair<double, Value>;
  using TimedValueVector = std::vector<TimedValue>;
 
  SequenceInterpolator() noexcept {}
 
  SequenceInterpolator(const TimedValueVector & values) { this->values(values); }
 
  void values(const TimedValueVector & values)
  {
    if(values.empty()) return;
    if(!std::is_sorted(values.begin(), values.end(),
                       [](const TimedValue & a, const TimedValue & b) { return a.first < b.first; }))
    {
      mc_rtc::log::error_and_throw("SequenceInterpolator values must be ordered by strictly ascending time");
    }
    values_ = values;
    prevIndex_ = 0;
    if(values_.size() > 1)
    {
      nextIndex_ = 1;
      intervalDuration_ = values_[nextIndex_].first - values_[prevIndex_].first;
    }
  }
 
  inline const TimedValueVector & values() const noexcept { return values_; }
 
  inline bool hasValues() const noexcept { return values_.size(); }
 
  void clear() { values_.clear(); }
 
  Value compute(double currTime)
  {
    if(values_.empty()) { mc_rtc::log::error_and_throw("SequenceInterpolator requires at least one value"); }
 
    // Check for out-of-bound access
    if(currTime >= values_.back().first) { return values_.back().second; }
    else if(currTime <= values_.front().first) { return values_.front().second; }
 
    /*
     * Efficiently update interval index and associated cached values:
     * 1/ Check whether currTime is in the current interval
     * 2/ Check whether currTime is in the next interval
     * 3/ Perform a binary search
     * Note that we don't need to check indices bounds here as they are valid by
     * construction.
     */
    auto updateIndex = [this, currTime]()
    {
      if(values_[prevIndex_].first < currTime && values_[nextIndex_].first >= currTime) { return; }
      else if(values_[nextIndex_].first < currTime && values_[nextIndex_ + 1].first >= currTime)
      {
        ++prevIndex_;
        ++nextIndex_;
        intervalDuration_ = values_[nextIndex_].first - values_[prevIndex_].first;
      }
      else
      {
        // first element in values_ with time greater or equal to currTime
        auto nextIt = std::lower_bound(std::begin(values_), std::end(values_), currTime,
                                       [](const TimedValue & lhs, const double & rhs) { return lhs.first < rhs; });
        nextIndex_ = static_cast<size_t>(nextIt - values_.begin());
        prevIndex_ = nextIndex_ - 1;
        intervalDuration_ = values_[nextIndex_].first - values_[prevIndex_].first;
      }
    };
 
    updateIndex();
    const auto & prevTime = values_[prevIndex_].first;
    return interpolator_(values_[prevIndex_].second, values_[nextIndex_].second,
                         (currTime - prevTime) / intervalDuration_);
  }
 
protected:
  InterpolationFunction interpolator_; 
  TimedValueVector values_; 
  size_t prevIndex_ = 0; 
  size_t nextIndex_ = 0; 
  double intervalDuration_ = 0; 
};
 
} // namespace mc_trajectory