twist2d.h

#pragma once
 
#include "cevalm.hpp"
#include "core/math/eigen_interface.h"
#include "core/math/units.h"

class Twist2d {
   private:
    units::Length dx_;
    units::Length dy_;
    units::Angle dtheta_;
 
   public:
    constexpr Twist2d() : dx_(0), dy_(0), dtheta_(0) {}

    constexpr Twist2d(units::Length dx, units::Length dy, units::Angle dtheta)
        : dx_{dx}, dy_{dy}, dtheta_{dtheta} {}

    constexpr Twist2d(
            const Eigen::Vector3d& twist_vector, units::Length length_unit,
            units::Angle angle_unit = units::radians
    )
        : dx_{twist_vector[0], length_unit},
          dy_{twist_vector[1], length_unit},
          dtheta_{twist_vector[2], angle_unit} {}

    constexpr units::Length dx() const { return dx_; }

    constexpr double dx(units::Length unit) const { return dx_.to(unit); }

    constexpr void set_dx(units::Length val) { dx_ = val; }

    constexpr units::Length dy() const { return dy_; }

    constexpr double dy(units::Length unit) const { return dy_.to(unit); }

    constexpr void set_dy(units::Length val) { dy_ = val; }

    constexpr units::Angle dtheta() const { return dtheta_; }

    constexpr double dtheta(units::Angle unit) const { return dtheta_.to(unit); }

    constexpr void set_dtheta(units::Angle val) { dtheta_ = val; }

    EVec<3> as_vector(units::Length length_unit, units::Angle angle_unit = units::radians) const {
        return EVec<3>{dx_.to(length_unit), dy_.to(length_unit), dtheta_.to(angle_unit)};
    }

    constexpr Twist2d operator*(double scalar) const {
        return Twist2d{dx_ * scalar, dy_ * scalar, dtheta_ * scalar};
    }

    friend constexpr Twist2d operator*(double scalar, const Twist2d& twist) {
        return twist * scalar;
    }

    constexpr Twist2d& operator*=(double scalar) { return *this = *this * scalar; }

    constexpr Twist2d operator/(double scalar) const { return *this * (1.0 / scalar); }

    constexpr Twist2d& operator/=(double scalar) { return *this = *this / scalar; }

    constexpr bool operator==(const Twist2d& other) const {
        return dx_ == other.dx_ && dy_ == other.dy_ && dtheta_ == other.dtheta_;
    }

    constexpr bool is_near(
            const Twist2d& other, units::Length distance_tolerance = units::Length(1e-6),
            units::Angle angle_tolerance = units::Angle(1e-6)
    ) const {
        return units::hypot(dx_ - other.dx_, dy_ - other.dy_) <= distance_tolerance &&
               units::abs(dtheta_ - other.dtheta_) <= angle_tolerance;
    }
};