Func.h

Go to the documentation of this file.

#pragma once
 
#include <array>
#include <cmath>
#include <functional>
#include <limits>
#include <map>
#include <memory>
#include <vector>
 
namespace TrajColl
{
template<class T>
class Func
{
public:
  Func() {}
 
  virtual T operator()(double t) const = 0;
 
  virtual T derivative(double t, int order = 1) const = 0;
 
  virtual double domainLowerLimit() const
  {
    return std::numeric_limits<double>::lowest();
  }
 
  virtual double domainUpperLimit() const
  {
    return std::numeric_limits<double>::max();
  }
};
 
template<class T>
class PiecewiseFunc : public Func<T>
{
public:
  PiecewiseFunc() {}
 
  virtual T operator()(double t) const override
  {
    checkArg(t);
    auto funcIt = funcs_.lower_bound(t);
    return (*(funcIt->second))(t);
  }
 
  virtual T derivative(double t, int order = 1) const override
  {
    checkArg(t);
    auto funcIt = funcs_.lower_bound(t);
    return (funcIt->second)->derivative(t, order);
  }
 
  virtual size_t index(double t) const
  {
    checkArg(t);
    return std::distance(funcs_.begin(), funcs_.lower_bound(t));
  }
 
  virtual double domainLowerLimit() const override
  {
    return tLowerLimit_;
  }
 
  virtual double domainUpperLimit() const override
  {
    if(funcs_.empty())
    {
      return std::numeric_limits<double>::max();
    }
    return funcs_.rbegin()->first;
  }
 
  void clearFuncs()
  {
    funcs_.clear();
    tLowerLimit_ = std::numeric_limits<double>::lowest();
  }
 
  void appendFunc(double t, std::shared_ptr<Func<T>> func)
  {
    funcs_.insert(std::pair<double, std::shared_ptr<Func<T>>>(t, func));
  }
 
  void setDomainLowerLimit(double t)
  {
    tLowerLimit_ = t;
  }
 
protected:
  void checkArg(double t) const
  {
    if(t < tLowerLimit_ || funcs_.rbegin()->first < t)
    {
      throw std::runtime_error("[PiecewiseFunc] Argument is out of function range. it should be "
                               + std::to_string(tLowerLimit_) + " <= " + std::to_string(t)
                               + " <= " + std::to_string(funcs_.rbegin()->first));
    }
  }
 
protected:
  std::map<double, std::shared_ptr<Func<T>>> funcs_;
 
  double tLowerLimit_ = std::numeric_limits<double>::lowest();
};
 
template<class T, int Order>
class Polynomial : public Func<T>
{
public:
  Polynomial(const std::array<T, Order + 1> & coeff, double t0 = 0.0) : coeff_(coeff), t0_(t0) {}
 
  int order() const
  {
    return Order;
  }
 
  virtual T operator()(double t) const override
  {
    T ret = coeff_[0];
    for(int i = 0; i < Order; i++)
    {
      ret += coeff_[i + 1] * std::pow(t - t0_, i + 1);
    }
    return ret;
  }
 
  virtual T derivative(double t, int derivativeOrder = 1) const override
  {
    if(derivativeOrder > Order)
    {
      if constexpr(std::is_scalar_v<T>)
      {
        return T(0);
      }
      else
      {
        T ret = coeff_[0]; // Dummy initialization for dynamic size class
        ret.setZero();
        return ret;
      }
    }
 
    T ret = coeff_[derivativeOrder];
    for(int j = 0; j < derivativeOrder - 1; j++)
    {
      ret *= (derivativeOrder - j);
    }
 
    for(int i = 0; i < Order - derivativeOrder; i++)
    {
      T term = coeff_[i + 1 + derivativeOrder] * std::pow(t - t0_, i + 1);
      for(int j = 0; j < derivativeOrder; j++)
      {
        term *= (i + 1 + derivativeOrder - j);
      }
      ret += term;
    }
 
    return ret;
  }
 
protected:
  std::array<T, Order + 1> coeff_;
 
  double t0_;
};
 
template<class T>
class Constant : public Polynomial<T, 0>
{
public:
  Constant(const T & value) : Polynomial<T, 0>(std::array<T, 1>{value}) {}
 
  virtual T operator()(double t = 0.0) const override
  {
    return Polynomial<T, 0>::operator()(t);
  }
};
 
template<class T>
using LinearPolynomial = Polynomial<T, 1>;
 
template<class T>
using QuadraticPolynomial = Polynomial<T, 2>;
 
template<class T>
using CubicPolynomial = Polynomial<T, 3>;
} // namespace TrajColl