cubic.hpp

Go to the documentation of this file.

 
#pragma once
 
#include <cstddef>
#include <vector>
 
#include "hermite/base_spline.hpp"
#include "hermite/cubic/cubic_vec.hpp"
#include "hermite/pose.hpp"
#include "hermite/thirdparty/simplevectors.hpp"
 
namespace hermite {
template <std::size_t D> class Cubic : public BaseSpline<D> {
public:
  Cubic() = default;
 
  Cubic(const std::vector<Pose<D>> &waypoints) : m_waypoints{waypoints} {
    std::sort(m_waypoints.begin(), m_waypoints.end(),
              [](const Pose<D> &a, const Pose<D> &b) {
                return a.getTime() < b.getTime();
              });
    m_spl = CubicVec<D>{m_waypoints};
  }
 
  Cubic(const Cubic<D> &other)
      : m_waypoints{other.m_waypoints}, m_spl{other.m_spl} {}
 
  Cubic<D> &operator=(const Cubic<D> &other) {
    if (this == &other) {
      return *this;
    }
 
    m_waypoints = other.m_waypoints;
    m_spl = other.m_spl;
 
    return *this;
  }
 
  ~Cubic() override = default;
 
  std::vector<Pose<D>> getAllWaypoints() const { return m_waypoints; }
 
  double getLowestTime() const override {
    if (m_waypoints.size() == 0) {
      return 0;
    }
 
    return m_waypoints[0].getTime();
  }
 
  double getHighestTime() const override {
    if (m_waypoints.size() == 0) {
      return 0;
    }
 
    return m_waypoints[m_waypoints.size() - 1].getTime();
  }
 
  Vector<D> getPos(const double t) const override {
    Vector<D> res;
    if (m_waypoints.size() < 2) {
      return res;
    }
 
    return m_spl.splpos(t);
  }
 
  Vector<D> getVel(const double t) const override {
    Vector<D> res;
    if (m_waypoints.size() < 2) {
      return res;
    }
 
    return m_spl.splvel(t);
  }
 
  Vector<D> getAcc(const double t) const override {
    Vector<D> res;
    if (m_waypoints.size() < 2) {
      return res;
    }
 
    return m_spl.splacc(t);
  }
 
  double getLength(const double timeStep) const override {
    double res = 0.0;
 
    if (m_waypoints.size() < 2) {
      return res;
    }
 
    double time = getLowestTime() + timeStep;
    const double timeEnd = getHighestTime();
    while (time <= timeEnd) {
      auto vel = getVel(time);
      auto speed = magn(vel);
      res += speed * timeStep;
 
      time += timeStep;
    }
 
    return res;
  }
 
private:
  std::vector<Pose<D>> m_waypoints;
  CubicVec<D> m_spl;
};
} // namespace hermite