CubicHermiteSpline.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
 
#include <TrajColl/Func.h>
 
namespace TrajColl
{
template<class T>
class CubicHermiteSpline : public PiecewiseFunc<T>
{
public:
  CubicHermiteSpline(int dim, const std::map<double, std::pair<T, T>> & points = {}) : dim_(dim), points_(points) {}
 
  const std::map<double, std::pair<T, T>> & points() const noexcept
  {
    return points_;
  }
 
  void clearPoints()
  {
    points_.clear();
  }
 
  void appendPoint(const std::pair<double, std::pair<T, T>> & point)
  {
    points_.insert(point);
  }
 
  void calcCoeff()
  {
    this->funcs_.clear();
 
    // Set piecewise CubicPolynomial
    size_t n = points_.size();
    if(n < 2)
    {
      throw std::runtime_error("[CubicHermiteSpline] Invalid points size: " + std::to_string(n));
    }
 
    {
      auto pointIt = points_.begin();
      for(size_t k = 0; k < n - 1; k++, pointIt++)
      {
        const T & pk = pointIt->second.first;
        const T & pk1 = std::next(pointIt)->second.first;
        const T & vk = pointIt->second.second;
        const T & vk1 = std::next(pointIt)->second.second;
        double deltaT = std::next(pointIt)->first - pointIt->first;
 
        // clang-format off
        std::array<T, 4> coeff = {
          pk,
          deltaT * vk,
          -3 * pk - 2 * deltaT * vk + 3 * pk1 - deltaT * vk1,
          2 * pk + deltaT * vk - 2 * pk1 + deltaT * vk1};
        // clang-format on
        std::shared_ptr<CubicPolynomial<T>> cubicFunc = std::make_shared<CubicPolynomial<T>>(coeff);
        this->funcs_.emplace(std::next(pointIt)->first, cubicFunc);
      }
    }
 
    // Set tLowerLimit_
    this->tLowerLimit_ = points_.begin()->first;
  }
 
  virtual T operator()(double t) const override
  {
    this->checkArg(t);
    auto funcIt = this->funcs_.lower_bound(t);
 
    const auto & seg = argSegment(t);
    double scaledT = (t - seg.first) / (seg.second - seg.first);
    return (*(funcIt->second))(scaledT);
  }
 
  virtual T derivative(double t, int order = 1) const override
  {
    this->checkArg(t);
    auto funcIt = this->funcs_.lower_bound(t);
 
    const auto & seg = argSegment(t);
    double scaledT = (t - seg.first) / (seg.second - seg.first);
    return (funcIt->second)->derivative(scaledT, order) / std::pow(seg.second - seg.first, order);
  }
 
  void calcMonotoneVelocity(bool keepStartVel = false, bool keepEndVel = false)
  {
    std::vector<T> delta;
    std::vector<T> alpha;
    std::vector<T> beta;
 
    // 1.
    for(auto it = points_.begin(); it != std::prev(points_.end()); it++)
    {
      delta.push_back((std::next(it)->second.first - it->second.first) / (std::next(it)->first - it->first));
    }
 
    // 2.
    {
      int i = 0;
      for(auto it = points_.begin(); it != points_.end(); it++)
      {
        if(it == points_.begin())
        {
          if(!keepStartVel)
          {
            it->second.second = delta[i];
          }
        }
        else if(it == std::prev(points_.end()))
        {
          if(!keepEndVel)
          {
            it->second.second = delta[i - 1];
          }
        }
        else
        {
          it->second.second = (delta[i - 1] + delta[i]) / 2;
          for(int j = 0; j < dim_; j++)
          {
            if(delta[i - 1][j] * delta[i][j] < 0)
            {
              it->second.second[j] = 0;
            }
          }
        }
        i++;
      }
    }
 
    // 3.
    {
      int i = 0;
      for(auto it = points_.begin(); it != std::prev(points_.end()); it++)
      {
        for(int j = 0; j < dim_; j++)
        {
          if(std::abs(delta[i][j]) < std::numeric_limits<double>::min())
          {
            it->second.second[j] = 0;
            std::next(it)->second.second[j] = 0;
          }
        }
        i++;
      }
    }
 
    // 4.
    {
      int i = 0;
      for(auto it = points_.begin(); it != std::prev(points_.end()); it++)
      {
        alpha.push_back(it->second.second.cwiseProduct(delta[i].cwiseInverse()));
        beta.push_back(std::next(it)->second.second.cwiseProduct(delta[i].cwiseInverse()));
        for(int j = 0; j < dim_; j++)
        {
          if(alpha[i][j] < 0)
          {
            it->second.second[j] = 0;
          }
          if(beta[i][j] < 0)
          {
            std::next(it)->second.second[j] = 0;
          }
        }
        i++;
      }
    }
 
    // 5.
    {
      int i = 0;
      for(auto it = points_.begin(); it != std::prev(points_.end()); it++)
      {
        for(int j = 0; j < dim_; j++)
        {
          if(std::abs(delta[i][j]) < std::numeric_limits<double>::min())
          {
            continue;
          }
          if(!(alpha[i][j] - std::pow(2 * alpha[i][j] + beta[i][j] - 3, 2) / (3 * (alpha[i][j] + beta[i][j] - 2)) > 0
               || alpha[i][j] + 2 * beta[i][j] - 3 <= 0 || 2 * alpha[i][j] + beta[i][j] - 3 <= 0))
          {
            double tau = 3 / std::sqrt(std::pow(alpha[i][j], 2) + std::pow(beta[i][j], 2));
            it->second.second[j] = tau * alpha[i][j] * delta[i][j];
            std::next(it)->second.second[j] = tau * beta[i][j] * delta[i][j];
          }
        }
        i++;
      }
    }
  }
 
protected:
  std::pair<double, double> argSegment(double t) const
  {
    auto funcIt = points_.lower_bound(t);
    if(funcIt == points_.begin())
    {
      funcIt++;
    }
    return {std::prev(funcIt)->first, funcIt->first};
  }
 
protected:
  int dim_;
 
  std::map<double, std::pair<T, T>> points_;
};
} // namespace TrajColl