hermite.hpp

Go to the documentation of this file.

 
#pragma once
 
#include <algorithm>
#include <cstddef>
#include <cstdint>
#include <map>
#include <vector>
 
#include "hermite/base_spline.hpp"
#include "hermite/hermite/hermite_sub.hpp"
#include "hermite/pose.hpp"
#include "hermite/thirdparty/simplevectors.hpp"
 
namespace hermite {
using svector::magn;
 
template <std::size_t D> class Hermite : public BaseSpline<D> {
public:
  Hermite() : m_multiplier{10LL} {}
 
  Hermite(const double multiplier) : m_multiplier{multiplier} {}
 
  Hermite(const Hermite<D> &other)
      : m_multiplier{other.m_multiplier}, m_waypoints{other.m_waypoints} {}
 
  Hermite<D> &operator=(const Hermite<D> &other) {
    // check if assigning to self
    if (this == &other) {
      return *this;
    }
 
    m_multiplier = other.m_multiplier;
    m_waypoints = other.m_waypoints;
 
    return *this;
  }
 
  ~Hermite() override = default;
 
  void insert(const Pose<D> &waypoint) {
    if (exists(waypoint)) {
      return;
    }
 
    auto tRounded = roundTime(waypoint.getTime());
    m_waypoints[tRounded] = waypoint;
  }
 
  void replace(const Pose<D> &waypoint) {
    if (!exists(waypoint)) {
      return;
    }
 
    auto tRounded = roundTime(waypoint.getTime());
    m_waypoints[tRounded] = waypoint;
  }
 
  void insertOrReplace(const Pose<D> &waypoint) {
    auto tRounded = roundTime(waypoint.getTime());
    m_waypoints[tRounded] = waypoint;
  }
 
  bool exists(const Pose<D> &waypoint) const {
    return exists(waypoint.getTime());
  }
 
  bool exists(const double time) const {
    const auto tRounded = roundTime(time);
    return m_waypoints.find(tRounded) != m_waypoints.end();
  }
 
  void erase(const Pose<D> &waypoint) { erase(waypoint.getTime()); }
 
  void erase(const double time) {
    const auto tRounded = roundTime(time);
 
    // check if exists
    auto it = m_waypoints.find(tRounded);
    if (it == m_waypoints.end()) {
      return;
    }
 
    m_waypoints.erase(it);
  }
 
  std::vector<Pose<D>> getAllWaypoints() const {
    std::vector<Pose<D>> res;
    for (const auto &it : m_waypoints) {
      res.push_back(it.second);
    }
 
    return res;
  }
 
  double getLowestTime() const override {
    if (m_waypoints.size() == 0) {
      return 0;
    }
 
    auto firstIt = m_waypoints.begin();
    return firstIt->second.getTime();
  }
 
  double getHighestTime() const override {
    if (m_waypoints.size() == 0) {
      return 0;
    }
 
    auto lastIt = m_waypoints.rbegin();
    return lastIt->second.getTime();
  }
 
  Vector<D> getPos(const double t) const override {
    Vector<D> res;
    if (m_waypoints.size() < 2) {
      return res;
    }
 
    auto func = getSub(t);
    return func.getPos(t);
  }
 
  Vector<D> getVel(const double t) const override {
    Vector<D> res;
    if (m_waypoints.size() < 2) {
      return res;
    }
 
    auto func = getSub(t);
    return func.getVel(t);
  }
 
  Vector<D> getAcc(const double t) const override {
    Vector<D> res;
    if (m_waypoints.size() < 2) {
      return res;
    }
 
    auto func = getSub(t);
    return func.getAcc(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:
  double m_multiplier;
  std::map<std::int64_t, Pose<D>> m_waypoints;
 
  std::int64_t roundTime(double t) const {
    t *= m_multiplier;
    return static_cast<std::int64_t>(t);
  }
 
  HermiteSub<D> getSub(const double t) const {
    const auto tRound = roundTime(t);
    auto itLower = m_waypoints.upper_bound(tRound);
 
    if (itLower == m_waypoints.begin()) {
      itLower++;
    }
 
    if (itLower == m_waypoints.end()) {
      itLower--;
    }
 
    auto itUpper = itLower;
    itLower--;
 
    const auto objUpper = itUpper->second;
    const auto objLower = itLower->second;
 
    const auto p0 = objLower.getPos();
    const auto pf = objUpper.getPos();
    const auto v0 = objLower.getVel();
    const auto vf = objUpper.getVel();
    const auto lowerT = objLower.getTime();
    const auto upperT = objUpper.getTime();
 
    HermiteSub<D> res{p0, pf, v0, vf, lowerT, upperT};
    return res;
  }
};
} // namespace hermite