twist2d.h

#pragma once
 
#include "math/eigen_interface.h"
 
#include <cmath>
#include <iostream>
#include <vector>

class Twist2d {
  public:
    constexpr Twist2d();

    Twist2d(const double &dx, const double &dy, const double &dtheta);

    Twist2d(const Eigen::Vector3d &twist_vector);

    double dx() const;

    double dy() const;

    double dtheta() const;

    bool operator==(const Twist2d &other) const;

    Twist2d operator*(const double &scalar) const;

    Twist2d operator/(const double &scalar) const;

    friend std::ostream &operator<<(std::ostream &os, const Twist2d &twist);
 
  private:
    double m_dx;
    double m_dy;
    double m_dtheta;
};