hermite_sub.hpp

Go to the documentation of this file.

 
#pragma once
 
#include <algorithm>
#include <cstddef>
 
#include "hermite/base_interpol.hpp"
#include "hermite/hermite/constants.hpp"
#include "hermite/hermite/hermite_unit.hpp"
 
namespace hermite {
template <std::size_t D> class HermiteSub : public BaseInterpol<D> {
public:
  HermiteSub() : m_lower{0}, m_upper{1} {};
 
  HermiteSub(const Vector<D> p0, const Vector<D> pf, const Vector<D> v0,
             const Vector<D> vf, const double lower, const double upper)
      : m_lower{lower}, m_upper{upper}, m_unit{p0, pf, v0 * (upper - lower),
                                               vf * (upper - lower)} {};
 
  HermiteSub(const HermiteSub<D> &other)
      : m_lower{other.m_lower}, m_upper{other.m_upper}, m_unit{other.m_unit} {}
 
  HermiteSub<D> &operator=(const HermiteSub<D> &other) {
    // check if assigning to self
    if (this == &other) {
      return *this;
    }
 
    m_unit = other.m_unit;
    m_lower = other.m_lower;
    m_upper = other.m_upper;
 
    return *this;
  }
 
  ~HermiteSub() override = default;
 
  Vector<D> getPos(const double t) const override {
    Vector<D> res;
    const double tNew = (t - m_lower) / (m_upper - m_lower);
    res = m_unit.getPos(tNew);
    return res;
  }
 
  Vector<D> getVel(const double t) const override {
    Vector<D> res;
    const double tNew = (t - m_lower) / (m_upper - m_lower);
    res = m_unit.getVel(tNew) * (1 / (m_upper - m_lower)); // multiply by chain
    return res;
  }
 
  Vector<D> getAcc(const double t) const override {
    Vector<D> res;
    const double tNew = (t - m_lower) / (m_upper - m_lower);
    res = m_unit.getAcc(tNew) * (1 / (m_upper - m_lower)) *
          (1 / (m_upper -
                m_lower)); // multiply by chain; don't need to add product rule
                           // because second derivative of tNew(t) = 0
    return res;
  }
 
private:
  double m_lower;
  double m_upper;
  HermiteUnit<D> m_unit;
};
} // namespace hermite