CubicInterpolator.h

Go to the documentation of this file.

#pragma once
 
#include <TrajColl/CubicHermiteSpline.h>
#include <TrajColl/ElementInterpolation.h>
#include <TrajColl/Interpolator.h>
 
namespace TrajColl
{
template<class T, class U = T>
class CubicInterpolator : public Interpolator<T, U>
{
protected:
  using Vector1d = Eigen::Matrix<double, 1, 1>;
 
public:
  CubicInterpolator(const std::map<double, T> & points = {}) : Interpolator<T, U>(points)
  {
    func_ = std::make_shared<CubicHermiteSpline<Vector1d>>(1, std::map<double, std::pair<Vector1d, Vector1d>>{});
 
    if(this->points_.size() >= 2)
    {
      calcCoeff();
    }
  }
 
  CubicInterpolator(const CubicInterpolator & inst) : Interpolator<T, U>(inst)
  {
    func_ = std::make_shared<CubicHermiteSpline<Vector1d>>(*inst.func_);
  }
 
  virtual std::shared_ptr<Interpolator<T, U>> clone() const override
  {
    return std::shared_ptr<Interpolator<T, U>>(new CubicInterpolator(*this));
  }
 
  virtual void appendPoint(const std::pair<double, T> & point) override
  {
    this->points_.insert(point);
  }
 
  virtual void calcCoeff() override
  {
    if(this->points_.size() < 2)
    {
      throw std::out_of_range("[CubicInterpolator] Number of points should be 2 or more: "
                              + std::to_string(this->points_.size()));
    }
 
    func_->clearPoints();
 
    auto it = this->points_.begin();
    for(size_t i = 0; i < this->points_.size(); i++)
    {
      func_->appendPoint(std::make_pair(
          it->first, std::make_pair((Vector1d() << static_cast<double>(i)).finished(), Vector1d::Zero())));
      it++;
    }
 
    func_->setDomainLowerLimit(this->points_.begin()->first);
    func_->calcCoeff();
  }
 
  virtual T operator()(double t) const override
  {
    size_t idx = func_->index(t);
    double ratio = (*func_)(t)[0] - static_cast<double>(idx);
    return interpolate<T>(std::next(this->points_.begin(), idx)->second,
                          std::next(this->points_.begin(), idx + 1)->second, std::clamp(ratio, 0.0, 1.0));
  }
 
  virtual U derivative(double t, int order = 1) const override
  {
    size_t idx = func_->index(t);
    double ratio = (*func_)(t)[0] - static_cast<double>(idx);
    return func_->derivative(t, order)[0]
           * interpolateDerivative<T, U>(std::next(this->points_.begin(), idx)->second,
                                         std::next(this->points_.begin(), idx + 1)->second, std::clamp(ratio, 0.0, 1.0),
                                         1);
  }
 
protected:
  std::shared_ptr<CubicHermiteSpline<Vector1d>> func_;
};
} // namespace TrajColl