transform2d.h

#pragma once
 
#include "core/math/geometry/rotation2d.h"
#include "core/math/geometry/translation2d.h"
#include "core/math/units.h"

class Transform2d {
   private:
    Translation2d translation_;
    Rotation2d rotation_;
 
   public:
    constexpr Transform2d() = default;

    constexpr Transform2d(Translation2d translation, Rotation2d rotation)
        : translation_(translation), rotation_(rotation) {}

    constexpr Transform2d(units::Length x, units::Length y, Rotation2d rotation)
        : translation_(x, y), rotation_(rotation) {}

    constexpr Transform2d(units::Length x, units::Length y, units::Angle angle)
        : translation_(x, y), rotation_(angle) {}

    constexpr Transform2d(Translation2d translation, units::Angle angle)
        : translation_(translation), rotation_(angle) {}

    constexpr Transform2d(
            const Eigen::Vector3d& transform_vector, units::Length unit,
            units::Angle angle_unit = units::radians
    )
        : translation_({transform_vector(0), transform_vector(1)}, unit),
          rotation_(units::Angle(transform_vector(2), angle_unit)) {}

    constexpr units::Length x() const { return translation_.x(); }

    constexpr double x(units::Length unit) const { return translation_.x(unit); }

    constexpr void set_x(units::Length val) { translation_.set_x(val); }

    constexpr units::Length y() const { return translation_.y(); }

    constexpr double y(units::Length unit) const { return translation_.y(unit); }

    constexpr void set_y(units::Length val) { translation_.set_y(val); }

    constexpr Translation2d translation() const { return translation_; }

    constexpr void set_translation(Translation2d val) { translation_ = val; }

    constexpr Rotation2d rotation() const { return rotation_; }

    constexpr void set_rotation(Rotation2d val) { rotation_ = val; }

    constexpr units::Angle angle() const { return rotation_.angle(); }

    constexpr double angle(units::Angle unit) const { return rotation_.angle(unit); }

    constexpr void set_angle(units::Angle val) { rotation_ = Rotation2d(val); }

    EVec<3> as_vector(units::Length length_unit, units::Angle angle_unit = units::radians) const {
        return {translation_.x().to(length_unit), translation_.y().to(length_unit),
                rotation_.angle().to(angle_unit)};
    }

    constexpr Transform2d inverse() const {
        return Transform2d(-translation_.rotate_by(-rotation_), -rotation_);
    }

    constexpr Transform2d operator+(const Transform2d& other) const {
        return {translation_ + other.translation_.rotate_by(rotation_),
                rotation_ + other.rotation_};
    }

    constexpr Transform2d& operator+=(const Transform2d& other) { return *this = *this + other; }

    constexpr Transform2d operator-() const { return inverse(); }

    constexpr Transform2d operator*(double scalar) const {
        return Transform2d(translation_ * scalar, rotation_ * scalar);
    }

    friend constexpr Transform2d operator*(double scalar, const Transform2d& transform) {
        return transform * scalar;
    }

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

    constexpr Transform2d operator/(double scalar) const {
        return Transform2d(translation_ / scalar, rotation_ / scalar);
    }

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

    constexpr bool operator==(const Transform2d& other) const {
        return (translation_ == other.translation_) && (rotation_ == other.rotation_);
    }

    constexpr bool is_near(
            const Transform2d& other, units::Length distance_tolerance = units::Length(1e-6),
            units::Angle angle_tolerance = units::Angle(1e-6)
    ) const {
        return translation_.is_near(other.translation_, distance_tolerance) &&
               rotation_.is_near(other.rotation_, angle_tolerance);
    }
};