translation2d.h

#pragma once
 
#include <cmath>
#include <vector>
 
#include "cevalm.hpp"
#include "core/math/eigen_interface.h"
#include "core/math/geometry/rotation2d.h"
#include "core/math/units.h"
 
class Rotation2d;
 
template <typename Q>
concept LinearKinematicQuantity =
        units::KinematicQuantity<Q> && std::ratio_equal_v<typename Q::length, std::ratio<1>> &&
        std::ratio_equal_v<typename Q::angle, std::ratio<0>>;

template <LinearKinematicQuantity Q>
class LinearVector2d {
   private:
    Q x_;
    Q y_;
 
   public:
    constexpr LinearVector2d() : x_{0}, y_{0} {}

    constexpr LinearVector2d(Q x, Q y) : x_{x}, y_{y} {}

    constexpr LinearVector2d(Q r, Rotation2d theta)
        : x_{r * theta.f_cos()}, y_{r * theta.f_sin()} {}

    constexpr LinearVector2d(Eigen::Vector2d vector, Q unit)
        : x_{vector[0], unit}, y_{vector[1], unit} {}

    constexpr Q x() const { return x_; }

    constexpr double x(Q unit) const { return x_.to(unit); }

    constexpr void set_x(Q val) { x_ = val; }

    constexpr Q y() const { return y_; }

    constexpr double y(Q unit) const { return y_.to(unit); }

    constexpr void set_y(Q val) { y_ = val; }

    constexpr Rotation2d theta() const { return Rotation2d(x_.internal(), y_.internal()); }

    constexpr Eigen::Vector2d as_vector(Q unit) const { return EVec<2>{x_.to(unit), y_.to(unit)}; }

    constexpr Q norm() const { return units::hypot(x_, y_); }

    constexpr LinearVector2d normalize(Q magnitude = Q(1.0)) const {
        return LinearVector2d(magnitude, theta());
    }

    constexpr Q distance(LinearVector2d other) const {
        return units::hypot(x_ - other.x_, y_ - other.y_);
    }

    constexpr LinearVector2d interpolate(LinearVector2d end, double fraction) const {
        if (fraction <= 0) {
            return *this;
        }
        if (fraction >= 1) {
            return end;
        }
        return *this + (end - *this) * fraction;
    }

    constexpr LinearVector2d rotate_by(Rotation2d rotation) const {
        return {x_ * rotation.f_cos() - y_ * rotation.f_sin(),
                x_ * rotation.f_sin() + y_ * rotation.f_cos()};
    }

    constexpr LinearVector2d rotate_around(LinearVector2d other, Rotation2d rotation) const {
        LinearVector2d diff = *this - other;
        return diff.rotate_by(rotation) + other;
    }

    constexpr LinearVector2d inverse() const { return LinearVector2d(-x_, -y_); }

    constexpr units::Multiplied<Q, Q> dot(LinearVector2d other) const {
        return (x_ * other.x_) + (y_ * other.y_);
    }

    constexpr LinearVector2d operator+(LinearVector2d other) const {
        return {x_ + other.x_, y_ + other.y_};
    }

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

    constexpr LinearVector2d operator-(LinearVector2d other) const {
        return {x_ - other.x_, y_ - other.y_};
    }

    constexpr LinearVector2d& operator-=(LinearVector2d other) { return *this = *this - other; }

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

    constexpr LinearVector2d operator*(double scalar) const { return {x_ * scalar, y_ * scalar}; }

    friend constexpr LinearVector2d operator*(double scalar, LinearVector2d vector) {
        return vector * scalar;
    }

    template <units::IsQuantity S>
        requires LinearKinematicQuantity<units::Multiplied<Q, S>>
    constexpr LinearVector2d<units::Multiplied<Q, S>> operator*(S scalar) const {
        return LinearVector2d<units::Multiplied<Q, S>>{x_ * scalar, y_ * scalar};
    }

    template <units::IsQuantity S>
        requires LinearKinematicQuantity<units::Multiplied<Q, S>>
    friend constexpr auto operator*(S scalar, LinearVector2d vector) {
        return vector * scalar;
    }

    constexpr units::Multiplied<Q, Q> operator*(LinearVector2d other) const { return dot(other); }

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

    constexpr LinearVector2d operator/(double scalar) const { return {x_ / scalar, y_ / scalar}; }
 
    template <units::IsQuantity S>
        requires LinearKinematicQuantity<units::Divided<Q, S>>
    constexpr LinearVector2d<units::Divided<Q, S>> operator/(S scalar) const {
        return LinearVector2d<units::Divided<Q, S>>{x_ / scalar, y_ / scalar};
    }

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

    constexpr bool operator==(LinearVector2d other) const {
        return cevalm::abs(x_.internal() - other.x_.internal()) < 1e-6 &&
               cevalm::abs(y_.internal() - other.y_.internal()) < 1e-6;
    }

    constexpr bool is_near(LinearVector2d other, Q tolerance = Q(1e-6)) const {
        return distance(other) <= tolerance;
    }

    static constexpr LinearVector2d mean(const std::vector<LinearVector2d>& list) {
        if (list.size() == 0) {
            return LinearVector2d{};
        }
 
        Q sumx;
        Q sumy;
 
        for (LinearVector2d& vec : list) {
            sumx += vec.x_;
            sumy += vec.y_;
        }
 
        return LinearVector2d(sumx / list.size(), sumy / list.size());
    }
};

using Translation2d = LinearVector2d<units::Length>;
using Velocity2d = LinearVector2d<units::Velocity>;
using Acceleration2d = LinearVector2d<units::Acceleration>;
using Jerk2d = LinearVector2d<units::Jerk>;