cubic_vec.hpp

Go to the documentation of this file.

 
#pragma once
 
#include <array>
#include <cstddef>
#include <vector>
 
#include "hermite/cubic/cubic_impl.hpp"
#include "hermite/pose.hpp"
#include "hermite/thirdparty/simplevectors.hpp"
 
namespace hermite {
using svector::Vector;
 
template <std::size_t D> class CubicVec {
public:
  CubicVec() = default;
 
  CubicVec(const std::vector<Pose<D>> &waypoints) {
    const std::size_t n = waypoints.size();
    m_ts.resize(n);
 
    // solve each dimension independently
    for (std::size_t dim = 0; dim < D; dim++) {
      m_ys[dim].resize(n);
      m_accs[dim].resize(n);
 
      for (std::size_t i = 0; i < n; i++) {
        m_ts[i] = waypoints[i].getTime();
        m_ys[dim][i] = waypoints[i].getPos()[dim];
      }
 
      const double yp1 = waypoints[0].getVel()[dim];
      const double ypn = waypoints[n - 1].getVel()[dim];
 
      spline(m_ts.data(), m_ys[dim].data(), static_cast<int>(n), yp1, ypn,
             m_accs[dim].data());
    }
  }
 
  CubicVec(const CubicVec<D> &other)
      : m_ts{other.m_ts}, m_ys{other.m_ys}, m_accs{other.m_accs} {}
 
  CubicVec<D> &operator=(const CubicVec<D> &other) {
    // check if assigning to self
    if (this == &other) {
      return *this;
    }
 
    m_ts = other.m_ts;
    m_ys = other.m_ys;
    m_accs = other.m_accs;
 
    return *this;
  }
 
  ~CubicVec() = default;
 
  Vector<D> splpos(const double t) const {
    const std::size_t n = m_ts.size();
    Vector<D> res;
 
    // solve each dimension independently
    for (std::size_t dim = 0; dim < D; dim++) {
      double output;
      hermite::splpos(m_ts.data(), m_ys[dim].data(), m_accs[dim].data(),
                      static_cast<int>(n), t, &output);
      res[dim] = output;
    }
 
    return res;
  }
 
  Vector<D> splvel(const double t) const {
    const std::size_t n = m_ts.size();
    Vector<D> res;
 
    // solve each dimension independently
    for (std::size_t dim = 0; dim < D; dim++) {
      double output;
      hermite::splvel(m_ts.data(), m_ys[dim].data(), m_accs[dim].data(),
                      static_cast<int>(n), t, &output);
      res[dim] = output;
    }
 
    return res;
  }
 
  Vector<D> splacc(const double t) const {
    const std::size_t n = m_ts.size();
    Vector<D> res;
 
    // solve each dimension independently
    for (std::size_t dim = 0; dim < D; dim++) {
      double output;
      hermite::splacc(m_ts.data(), m_ys[dim].data(), m_accs[dim].data(),
                      static_cast<int>(n), t, &output);
      res[dim] = output;
    }
 
    return res;
  }
 
private:
  std::vector<double> m_ts;
  std::array<std::vector<double>, D> m_ys;
  std::array<std::vector<double>, D> m_accs;
};
} // namespace hermite