pose.hpp

Go to the documentation of this file.

 
#pragma once
 
#include <cstddef>
 
#include "hermite/thirdparty/simplevectors.hpp"
 
namespace hermite {
using svector::Vector;
 
template <std::size_t D> class Pose {
public:
  Pose() : m_time{0} {}
 
  Pose(const double time, const Vector<D> pos, const Vector<D> vel)
      : m_time{time}, m_pos{pos}, m_vel{vel} {}
 
  Pose(const Pose<D> &other)
      : m_time{other.m_time}, m_pos{other.m_pos}, m_vel{other.m_vel} {}
 
  Pose<D> &operator=(const Pose<D> &other) {
    // check if assigning to self
    if (this == &other) {
      return *this;
    }
 
    m_pos = other.m_pos;
    m_vel = other.m_vel;
    m_time = other.m_time;
 
    return *this;
  }
 
  ~Pose() = default;
 
  Vector<D> getPos() const { return m_pos; }
 
  Vector<D> getVel() const { return m_vel; }
 
  double getTime() const { return m_time; }
 
  void setPos(const Vector<D> &pos) { m_pos = pos; }
 
  void setVel(const Vector<D> &vel) { m_vel = vel; }
 
  void setTime(const double &time) { m_time = time; }
 
private:
  double m_time;
  Vector<D> m_pos;
  Vector<D> m_vel;
};
} // namespace hermite