Pose.h

#pragma once
 
#include <algorithm>
#include <initializer_list>
#include <limits>
 
#include <frc/geometry/Translation2d.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/base.h>
#include <units/dimensionless.h>
#include <units/length.h>
#include <units/math.h>
#include <units/time.h>
#include <units/velocity.h>
 
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Pose3d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Transform3d.h"
#include "frc/geometry/Translation3d.h"
#include "valkyrie/util/EigenUnit.h"
 
namespace valor {
namespace geometry {
 
// Forward declarations
class Translation2d;
class Translation3d;
class Rotation2d;
class Rotation3d;
class Pose2d;
class Pose3d;
class Transform2d;
class Transform3d;
 
// ============================================================
//  Translation2d
// ============================================================

class Translation2d : public EigenUnit::NamedPhysicalVector<Translation2d, EigenUnit::VectorN<units::meter, 2>> {
   public:
    using NamedPhysicalVector::NamedPhysicalVector;
 
    Translation2d(const EigenUnit::UnitVector<units::meter, units::meter>& b) : NamedPhysicalVector(b) {}

    explicit Translation2d(frc::Translation2d frcTranslation) : Translation2d(frcTranslation.X(), frcTranslation.Y()) {};

    explicit Translation2d(frc::Translation3d frcTranslation) : Translation2d(frcTranslation.X(), frcTranslation.Y()) {};

    explicit Translation2d(const Translation3d& t);

    Translation2d RotateBy(const Rotation2d& rot) const;

    units::meter_t Distance(const Translation2d& other) const { return (*this - other).Norm(); }
};
 
// ============================================================
//  Rotation2d
// ============================================================

class Rotation2d
      : public EigenUnit::NamedPhysicalMatrix<Rotation2d, EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::dimensionless::scalar, 2>,
                                                                                    EigenUnit::VectorN<units::dimensionless::scalar, 2>>> {
    using BaseMatrix =
        EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::dimensionless::scalar, 2>, EigenUnit::VectorN<units::dimensionless::scalar, 2>>;
 
   public:
    using NamedPhysicalMatrix::NamedPhysicalMatrix;
 
    Rotation2d(const BaseMatrix& b) : NamedPhysicalMatrix(b) {}

    template <class AngleUnit, std::enable_if_t<units::traits::is_angle_unit<AngleUnit>::value, int> = 0>
    Rotation2d(units::unit_t<AngleUnit> theta) : Rotation2d(units::radian_t{theta}) {}
 
    Rotation2d(units::radian_t theta)
          : NamedPhysicalMatrix(BaseMatrix::FromRows(
                EigenUnit::VectorN<units::dimensionless::scalar, 2>{units::math::cos(theta), -units::math::sin(theta)},
                EigenUnit::VectorN<units::dimensionless::scalar, 2>{units::math::sin(theta), units::math::cos(theta)})) {}

    explicit Rotation2d(frc::Rotation2d frcRotation) : Rotation2d(frcRotation.Radians()) {};

    explicit Rotation2d(frc::Rotation3d frcRotation) : Rotation2d(frcRotation.Z()) {};

    explicit Rotation2d(const Rotation3d& rot);

    units::radian_t GetAngle() const { return units::math::atan2(Get<1, 0>(), Get<0, 0>()); }

    Rotation2d operator-() const { return Rotation2d(-GetAngle()); }

    Rotation2d operator*(double scalar) const { return Rotation2d(GetAngle() * scalar); }

    static Rotation2d Exp(units::radian_t theta) { return Rotation2d(theta); }

    units::radian_t Log() const { return GetAngle(); }
};
 
// ============================================================
//  HomogeneousVector2d / Pose2d internals
// ============================================================

class HomogeneousVector2d : public EigenUnit::UnitVector<units::meter, units::meter, units::dimensionless::scalar> {
   public:
    using UnitVector::UnitVector;
};
 
// ============================================================
//  Transform2d
// ============================================================

class Transform2d
      : public EigenUnit::NamedPhysicalMatrix<Transform2d, EigenUnit::PhysicalMatrix<HomogeneousVector2d, HomogeneousVector2d>> {
    using BaseMatrix = EigenUnit::PhysicalMatrix<HomogeneousVector2d, HomogeneousVector2d>;
 
   public:
    using NamedPhysicalMatrix::NamedPhysicalMatrix;
 
    Transform2d(const BaseMatrix& b) : NamedPhysicalMatrix(b) {}

    Transform2d(Translation2d trans, Rotation2d rot)
          : NamedPhysicalMatrix{
                BaseMatrix::FromBlocks(
                    rot,
                    EigenUnit::UnitMatrix<std::tuple<units::meter, units::meter>, std::tuple<units::dimensionless::scalar>>::FromCols(
                        trans),
                    EigenUnit::UnitMatrix<std::tuple<units::dimensionless::scalar>, std::tuple<units::meter, units::meter>>::Zero(),
                    EigenUnit::UnitMatrix<std::tuple<units::dimensionless::scalar>, std::tuple<units::dimensionless::scalar>>::Identity()),
            } {}

    explicit Transform2d(frc::Translation2d frcTrans, frc::Rotation2d frcRot) : Transform2d(Translation2d(frcTrans), Rotation2d(frcRot)) {}

    explicit Transform2d(frc::Transform2d frcTransform) : Transform2d(frcTransform.Translation(), frcTransform.Rotation()) {};

    static Transform2d Identity() { return Transform2d(Translation2d(0_m, 0_m), Rotation2d(0_rad)); }
 
    inline Translation2d Translation() const { return this->TopRight<2, 1>().Col<0>(); }
 
    inline Rotation2d Rotation() const { return this->TopLeft<2, 2>(); }

    Transform2d Inverse() const {
        // Correct inversion: T^{-1} = [ R^T  -R^T * t ]
        //                              [  0       1   ]
        auto invR = Rotation().Inverse();
        auto invT = Translation2d(invR * (-Translation()));
        return Transform2d(invT, invR);
    }

    Transform2d operator*(double scalar) const { return Transform2d(Translation() * scalar, Rotation2d(Rotation().GetAngle() * scalar)); }
};
 
// ============================================================
//  Pose2d
// ============================================================

class Pose2d : public EigenUnit::NamedPhysicalMatrix<Pose2d, EigenUnit::PhysicalMatrix<HomogeneousVector2d, HomogeneousVector2d>> {
    using BaseMatrix = EigenUnit::PhysicalMatrix<HomogeneousVector2d, HomogeneousVector2d>;
 
   public:
    using NamedPhysicalMatrix::NamedPhysicalMatrix;
 
    Pose2d(const BaseMatrix& b) : NamedPhysicalMatrix(b) {}

    Pose2d(Translation2d trans, Rotation2d rot)
          : NamedPhysicalMatrix{
                BaseMatrix::FromBlocks(
                    rot,
                    EigenUnit::UnitMatrix<std::tuple<units::meter, units::meter>, std::tuple<units::dimensionless::scalar>>::FromCols(
                        trans),
                    EigenUnit::UnitMatrix<std::tuple<units::dimensionless::scalar>, std::tuple<units::meter, units::meter>>::Zero(),
                    EigenUnit::UnitMatrix<std::tuple<units::dimensionless::scalar>, std::tuple<units::dimensionless::scalar>>::Identity()),
            } {}

    explicit Pose2d(frc::Translation2d frcTrans, frc::Rotation2d frcRot) : Pose2d(Translation2d(frcTrans), Rotation2d(frcRot)) {};

    explicit Pose2d(frc::Pose2d frcPose) : Pose2d(frcPose.Translation(), frcPose.Rotation()) {};

    explicit Pose2d(frc::Pose3d frcPose) : Pose2d(Translation2d(frcPose.Translation()), Rotation2d(frcPose.Rotation())) {};

    explicit Pose2d(const Pose3d& pose);
 
    inline Translation2d Translation() const { return this->TopRight<2, 1>().Col<0>(); }
 
    inline Rotation2d Rotation() const { return this->TopLeft<2, 2>(); }
 
    inline units::meter_t X() const { return Translation().X(); }
 
    inline units::meter_t Y() const { return Translation().Y(); }

    inline Pose2d TransformBy(const Transform2d& transform) const { return Pose2d((*this) * transform); }

    inline Pose2d RelativeTo(const Pose2d& other) const { return Pose2d(other.Inverse() * (*this)); }

    Pose2d RotateBy(const Rotation2d& rot) const { return Pose2d(Translation().RotateBy(rot), Rotation2d(rot * Rotation())); }

    Pose2d RotateAround(const Translation2d& point, const Rotation2d& rot) const {
        Translation2d newTrans = point + Translation2d(rot * (Translation() - point));
        return Pose2d(newTrans, Rotation2d(rot * Rotation()));
    }

    Pose2d Nearest(std::initializer_list<Pose2d> poses) const {
        Pose2d best = *poses.begin();
        units::meter_t bestDist = Translation().Distance(best.Translation());
        for (const valor::geometry::Pose2d& p : poses) {
            units::meter_t d = Translation().Distance(p.Translation());
            if (d < bestDist) {
                bestDist = d;
                best = p;
            }
        }
        return best;
    }

    Pose2d operator+(const Transform2d& transform) const { return TransformBy(transform); }

    Transform2d operator-(const Pose2d& other) const {
        Pose2d rel = RelativeTo(other);
        return Transform2d(rel.Translation(), rel.Rotation());
    }

    Pose2d operator*(double scalar) const { return Pose2d(Translation() * scalar, Rotation2d(Rotation().GetAngle() * scalar)); }

    Pose2d operator/(double scalar) const { return (*this) * (1.0 / scalar); }
 
    using Twist2d = EigenUnit::UnitVector<units::meter, units::meter, units::radian>;

    static Pose2d Exp(Twist2d twist) {
        units::radian_t theta = twist.Get<2>();
        EigenUnit::UnitVector<units::meter, units::meter> linearDisplacement = twist.Head<2>();
 
        if (units::math::abs(theta) < 1e-9_rad) {
            return {Translation2d(linearDisplacement), Rotation2d(0_rad)};
        }
 
        using VMatrix = EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::meter, 2>, EigenUnit::VectorN<units::meter, 2>>;
 
        units::scalar_t s_over_t = units::math::sin(theta) * 1_rad / theta;
        units::scalar_t c_over_t = (units::scalar_t{1.0} - units::math::cos(theta)) * 1_rad / theta;
 
        VMatrix V = VMatrix::FromElems(s_over_t, -c_over_t, c_over_t, s_over_t);
 
        return {Translation2d(V * linearDisplacement), Rotation2d::Exp(theta)};
    }

    Twist2d Log() const {
        units::radian_t theta = Rotation().GetAngle();
 
        if (units::math::abs(theta) < 1e-9_rad) {
            Translation2d t = Translation();
            return {t.X(), t.Y(), 0_rad};
        }
 
        using VInvMatrix = EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::meter, 2>, EigenUnit::VectorN<units::meter, 2>>;
 
        units::scalar_t a = (theta * units::math::sin(theta)) / (2.0_rad * (units::scalar_t{1.0} - units::math::cos(theta)));
        units::scalar_t b = theta / 2.0_rad;
 
        VInvMatrix V_inv = VInvMatrix::FromElems(a, b, -b, a);
 
        auto trans = V_inv * Translation();
        return {trans.X(), trans.Y(), theta};
    }
};
 
// ============================================================
//  Translation3d
// ============================================================

class Translation3d
      : public EigenUnit::NamedPhysicalVector<Translation3d, EigenUnit::UnitVector<units::meter, units::meter, units::meter>> {
   public:
    using NamedPhysicalVector::NamedPhysicalVector;
 
    Translation3d(const EigenUnit::UnitVector<units::meter, units::meter, units::meter>& b) : NamedPhysicalVector(b) {}

    explicit Translation3d(const Translation2d& t) : Translation3d(t.X(), t.Y(), 0_m) {}

    Translation3d(units::meter_t distance, const Rotation3d& direction);

    explicit Translation3d(frc::Translation3d frcTrans) : Translation3d(frcTrans.X(), frcTrans.Y(), frcTrans.Z()) {};

    explicit Translation3d(frc::Translation2d frcTrans) : Translation3d(frcTrans.X(), frcTrans.Y(), 0_m) {};

    Translation2d ToTranslation2d() const { return Translation2d(X(), Y()); }

    Translation3d RotateBy(const Rotation3d& rot) const;

    units::meter_t Distance(const Translation3d& other) const { return (*this - other).Norm(); }

    Translation3d operator*(double scalar) const {
        return Translation3d(EigenUnit::UnitVector<units::meter, units::meter, units::meter>(Raw() * scalar));
    }

    Translation3d operator/(double scalar) const {
        return Translation3d(EigenUnit::UnitVector<units::meter, units::meter, units::meter>(Raw() / scalar));
    }
};
 
// ============================================================
//  Rotation3d
// ============================================================

class Rotation3d
      : public EigenUnit::NamedPhysicalMatrix<Rotation3d, EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::dimensionless::scalar, 3>,
                                                                                    EigenUnit::VectorN<units::dimensionless::scalar, 3>>> {
    using BaseMatrix =
        EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::dimensionless::scalar, 3>, EigenUnit::VectorN<units::dimensionless::scalar, 3>>;
 
   public:
    using NamedPhysicalMatrix::NamedPhysicalMatrix;
 
    Rotation3d(const BaseMatrix& b) : NamedPhysicalMatrix(b) {}
 
    Rotation3d(units::radian_t pitch, units::radian_t roll, units::radian_t yaw)
          : Rotation3d{
                BaseMatrix::FromRows(
                    EigenUnit::VectorN<units::dimensionless::scalar, 3>{units::math::cos(pitch), 0, units::math::sin(pitch)},
                    EigenUnit::VectorN<units::dimensionless::scalar, 3>{0, 1, 0},
                    EigenUnit::VectorN<units::dimensionless::scalar, 3>{-units::math::sin(pitch), 0, units::math::cos(pitch)}),
                BaseMatrix::FromRows(
                    EigenUnit::VectorN<units::dimensionless::scalar, 3>{1, 0, 0},
                    EigenUnit::VectorN<units::dimensionless::scalar, 3>{0, units::math::cos(roll), -units::math::sin(roll)},
                    EigenUnit::VectorN<units::dimensionless::scalar, 3>{0, units::math::sin(roll), units::math::cos(roll)}),
                BaseMatrix::FromRows(EigenUnit::VectorN<units::dimensionless::scalar, 3>{units::math::cos(yaw), -units::math::sin(yaw), 0},
                                     EigenUnit::VectorN<units::dimensionless::scalar, 3>{units::math::sin(yaw), units::math::cos(yaw), 0},
                                     EigenUnit::VectorN<units::dimensionless::scalar, 3>{0, 0, 1}),
            } {}

    explicit Rotation3d(const Rotation2d& rot) : Rotation3d(0_rad, 0_rad, rot.GetAngle()) {}

    explicit Rotation3d(frc::Rotation3d frcRot) : Rotation3d(frcRot.Y(), frcRot.X(), frcRot.Z()) {};

    explicit Rotation3d(frc::Rotation2d frcRot) : Rotation3d(0_rad, 0_rad, frcRot.Radians()) {};

    Rotation3d operator*(double scalar) const { return Rotation3d::Exp(Log() * scalar); }

    static Rotation3d Exp(EigenUnit::VectorN<units::radian, 3> w) {
        units::radian_t theta = w.Norm();
        if (theta < 1e-9_rad) {
            return Identity();
        }
        EigenUnit::VectorN<units::dimensionless::scalar, 3> axis = w / theta;
        BaseMatrix K = EigenUnit::VectorN<units::dimensionless::scalar, 3>::Skew(axis);
        auto R = BaseMatrix::Identity() + units::math::sin(theta) * K + (1.0 - units::math::cos(theta)) * (K * K);
        return Rotation3d(R);
    }

    EigenUnit::VectorN<units::radian, 3> Log() const {
        units::dimensionless::scalar_t cos_theta = (this->Trace() - 1.0) / 2.0;
        cos_theta = units::dimensionless::scalar_t{std::clamp(cos_theta.value(), -1.0, 1.0)};
        units::radian_t theta = units::math::acos(cos_theta);
        if (theta < 1e-9_rad) {
            return EigenUnit::VectorN<units::radian, 3>::Zero();
        }
        BaseMatrix K = ((*this) - this->Transpose()) / (2.0 * units::math::sin(theta));
        return EigenUnit::VectorN<units::radian, 3>{
            K.Get<2, 1>() * theta,
            K.Get<0, 2>() * theta,
            K.Get<1, 0>() * theta,
        };
    }

    Rotation3d Interpolate(const Rotation3d& other, double t) const {
        return Rotation3d((*this) * Rotation3d::Exp(Rotation3d(other * this->Inverse()).Log() * t));
    }
 
    units::radian_t Pitch() const { return units::math::asin(-Get<2, 0>()); }
 
    units::radian_t Roll() const { return units::math::atan2(Get<2, 1>(), Get<2, 2>()); }
 
    units::radian_t Yaw() const { return units::math::atan2(Get<1, 0>(), Get<0, 0>()); }
 
   private:
    Rotation3d(
        EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::dimensionless::scalar, 3>, EigenUnit::VectorN<units::dimensionless::scalar, 3>>
            p,
        EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::dimensionless::scalar, 3>, EigenUnit::VectorN<units::dimensionless::scalar, 3>>
            r,
        EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::dimensionless::scalar, 3>, EigenUnit::VectorN<units::dimensionless::scalar, 3>>
            y)
          : NamedPhysicalMatrix{y * p * r}, pitchMatrix(p), rollMatrix(r), yawMatrix(y) {}
 
    EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::dimensionless::scalar, 3>, EigenUnit::VectorN<units::dimensionless::scalar, 3>>
        pitchMatrix, rollMatrix, yawMatrix;
};
 
inline Translation2d Translation2d::RotateBy(const Rotation2d& rot) const {
    return rot * (*this);
}
 
inline Translation2d::Translation2d(const Translation3d& t) : Translation2d(t.X(), t.Y()) {}
 
inline Rotation2d::Rotation2d(const Rotation3d& rot) : Rotation2d(rot.Yaw()) {}

class HomogeneousVector3d : public EigenUnit::UnitVector<units::meter, units::meter, units::meter, units::dimensionless::scalar> {
   public:
    using UnitVector::UnitVector;
};
 
// ============================================================
//  Transform3d
// ============================================================

class Transform3d
      : public EigenUnit::NamedPhysicalMatrix<Transform3d, EigenUnit::PhysicalMatrix<HomogeneousVector3d, HomogeneousVector3d>> {
    using BaseMatrix = EigenUnit::PhysicalMatrix<HomogeneousVector3d, HomogeneousVector3d>;
 
   public:
    using NamedPhysicalMatrix::NamedPhysicalMatrix;
 
    Transform3d(const BaseMatrix& b) : NamedPhysicalMatrix(b) {}

    Transform3d(Translation3d trans, Rotation3d rot)
          : NamedPhysicalMatrix{
                FromBlocks(
                    rot,
                    EigenUnit::UnitMatrix<std::tuple<units::meter, units::meter, units::meter>,
                                          std::tuple<units::dimensionless::scalar>>::FromCols(trans),
                    EigenUnit::UnitMatrix<std::tuple<units::dimensionless::scalar>,
                                          std::tuple<units::meter, units::meter, units::meter>>::Zero(),
                    EigenUnit::UnitMatrix<std::tuple<units::dimensionless::scalar>, std::tuple<units::dimensionless::scalar>>::Identity()),
            } {}

    explicit Transform3d(const Transform2d& t) : Transform3d(Translation3d(t.Translation()), Rotation3d(t.Rotation())) {}

    explicit Transform3d(frc::Translation3d frcTrans, frc::Rotation3d frcRot) : Transform3d(Translation3d(frcTrans), Rotation3d(frcRot)) {};

    explicit Transform3d(frc::Transform3d frcTransform) : Transform3d(frcTransform.Translation(), frcTransform.Rotation()) {};

    explicit Transform3d(frc::Transform2d frcTransform)
          : Transform3d(Translation3d(frcTransform.Translation()), Rotation3d(frcTransform.Rotation())) {};

    static Transform3d Identity() { return Transform3d(Translation3d(0_m, 0_m, 0_m), Rotation3d(0_rad, 0_rad, 0_rad)); }
 
    inline Translation3d Translation() const { return this->TopRight<3, 1>().Col<0>(); }
 
    inline Rotation3d Rotation() const { return this->TopLeft<3, 3>(); }

    Transform3d Inverse() const {
        Rotation3d invR = Rotation().Inverse();
        Translation3d invT = invR * (-Translation());
        return Transform3d(invT, invR);
    }
};
 
// ============================================================
//  Pose3d
// ============================================================

class Pose3d : public EigenUnit::NamedPhysicalMatrix<Pose3d, EigenUnit::PhysicalMatrix<HomogeneousVector3d, HomogeneousVector3d>> {
    using BaseMatrix = EigenUnit::PhysicalMatrix<HomogeneousVector3d, HomogeneousVector3d>;
 
   public:
    using NamedPhysicalMatrix::NamedPhysicalMatrix;
 
    Pose3d(const BaseMatrix& b) : NamedPhysicalMatrix(b) {}

    Pose3d(Translation3d trans, Rotation3d rot)
          : NamedPhysicalMatrix{
                FromBlocks(
                    rot,
                    EigenUnit::UnitMatrix<std::tuple<units::meter, units::meter, units::meter>,
                                          std::tuple<units::dimensionless::scalar>>::FromCols(trans),
                    EigenUnit::UnitMatrix<std::tuple<units::dimensionless::scalar>,
                                          std::tuple<units::meter, units::meter, units::meter>>::Zero(),
                    EigenUnit::UnitMatrix<std::tuple<units::dimensionless::scalar>, std::tuple<units::dimensionless::scalar>>::Identity()),
            } {}

    explicit Pose3d(const Pose2d& pose) : Pose3d(Translation3d(pose.Translation()), Rotation3d(pose.Rotation())) {}

    explicit Pose3d(frc::Translation3d frcTrans, frc::Rotation3d frcRot) : Pose3d(Translation3d(frcTrans), Rotation3d(frcRot)) {};

    explicit Pose3d(frc::Pose3d frcPose) : Pose3d(frcPose.Translation(), frcPose.Rotation()) {};

    explicit Pose3d(frc::Pose2d frcPose) : Pose3d(Translation3d(frcPose.Translation()), Rotation3d(frcPose.Rotation())) {};
 
    inline Translation3d Translation() const { return this->TopRight<3, 1>().Col<0>(); }
 
    inline Rotation3d Rotation() const { return this->TopLeft<3, 3>(); }
 
    inline units::meter_t X() const { return Translation().X(); }
 
    inline units::meter_t Y() const { return Translation().Y(); }
 
    inline units::meter_t Z() const { return Translation().Z(); }

    Pose2d ToPose2d() const { return Pose2d(*this); }

    inline Pose3d TransformBy(const Transform3d& transform) const { return Pose3d{(*this) * transform}; }

    inline Pose3d RelativeTo(const Pose3d& other) const { return Pose3d{other.Inverse() * (*this)}; }

    Pose3d RotateBy(const Rotation3d& rot) const { return Pose3d{Translation().RotateBy(rot), Rotation3d(rot * Rotation())}; }

    Pose3d RotateAround(const Translation3d& point, const Rotation3d& rot) const {
        Translation3d newTrans = point + Translation3d(rot * (Translation() - point));
        return Pose3d{newTrans, Rotation3d{rot * Rotation()}};
    }

    Pose3d Nearest(std::initializer_list<Pose3d> poses) const {
        Pose3d best = *poses.begin();
        units::meter_t bestDist = Translation().Distance(best.Translation());
        for (const valor::geometry::Pose3d& p : poses) {
            units::meter_t d = Translation().Distance(p.Translation());
            if (d < bestDist) {
                bestDist = d;
                best = p;
            }
        }
        return best;
    }

    Pose3d operator+(const Transform3d& transform) const { return TransformBy(transform); }

    Transform3d operator-(const Pose3d& other) const {
        Pose3d rel = RelativeTo(other);
        return Transform3d{rel.Translation(), rel.Rotation()};
    }

    Pose3d operator*(double scalar) const { return Pose3d(Translation() * scalar, Rotation() * scalar); }

    Pose3d operator/(double scalar) const { return (*this) * (1.0 / scalar); }
 
    using Twist3d = EigenUnit::UnitVector<units::meter, units::meter, units::meter, units::radian, units::radian, units::radian>;

    static Pose3d Exp(Twist3d twist) {
        auto w = twist.Tail<3>();
        units::radian_t theta = w.Norm();
        auto v = twist.Head<3>();
 
        if (theta < 1e-9_rad) {
            return Pose3d{Translation3d(v), Rotation3d::Identity()};
        }
 
        double s = units::math::sin(theta).value();
        double c = units::math::cos(theta).value();
        double t = theta.value();
 
        using VMatrix = EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::meter, 3>, EigenUnit::VectorN<units::meter, 3>>;
        VMatrix K{EigenUnit::VectorN<units::dimensionless::scalar, 3>::Skew(w / theta).Raw()};
 
        VMatrix V = VMatrix::Identity() + ((1.0 - c) / t) * K + ((t - s) / t) * (K * K);
 
        return Pose3d{Translation3d(V * v), Rotation3d::Exp(w)};
    }

    Twist3d Log() const {
        valor::EigenUnit::VectorN<units::radian, 3> w = Rotation().Log();
        units::radian_t theta = w.Norm();
        Translation3d T = Translation();
 
        if (theta < 1e-9_rad) {
            return Twist3d{
                T.X(), T.Y(), T.Z(), 0_rad, 0_rad, 0_rad,
            };
        }
 
        double s = units::math::sin(theta).value();
        double c = units::math::cos(theta).value();
        double t = theta.value();
 
        using VInvMatrix = EigenUnit::PhysicalMatrix<EigenUnit::VectorN<units::meter, 3>, EigenUnit::VectorN<units::meter, 3>>;
        VInvMatrix K{EigenUnit::VectorN<units::dimensionless::scalar, 3>::Skew(w / theta).Raw()};
 
        VInvMatrix V_inv = VInvMatrix::Identity() - (t / 2.0) * K + (1.0 - (t * s) / (2.0 * (1.0 - c))) * (K * K);
 
        EigenUnit::UnitVector<units::meter, units::meter, units::meter> v = V_inv * T;
        return {v.X(), v.Y(), v.Z(), w.X(), w.Y(), w.Z()};
    }
};
 
// ============================================================
//  Deferred Pose2d / Translation3d / Rotation3d method bodies
// ============================================================
 
inline Pose2d::Pose2d(const Pose3d& pose) : Pose2d{Translation2d{pose.Translation()}, Rotation2d{pose.Rotation()}} {}
 
inline Translation3d Translation3d::RotateBy(const Rotation3d& rot) const {
    return Translation3d{rot * (*this)};
}
 
inline Translation3d::Translation3d(units::meter_t distance, const Rotation3d& direction)
      : Translation3d{
            distance * units::math::cos(direction.Pitch()) * units::math::cos(direction.Yaw()),
            distance * units::math::cos(direction.Pitch()) * units::math::sin(direction.Yaw()),
            distance * units::math::sin(direction.Pitch()),
        } {}
 
}  // namespace geometry
}  // namespace valor