transform.hpp

 
#ifndef UFO_NUMERIC_TRANSFORM_HPP
#define UFO_NUMERIC_TRANSFORM_HPP
 
// UFO
#include <ufo/numeric/mat.hpp>
#include <ufo/numeric/quat.hpp>
#include <ufo/numeric/vec.hpp>
 
// STL
#include <cmath>
#include <concepts>
#include <cstddef>
#include <format>
#include <ostream>
 
namespace ufo
{
 
/**************************************************************************************
|                                                                                     |
|                                   Transform<Dim, T>                                 |
|                                                                                     |
**************************************************************************************/
 
template <std::size_t Dim, std::floating_point T = float>
  requires(Dim == 2 || Dim == 3)
struct Transform {
    using value_type = T;
    using size_type  = std::size_t;
 
    Mat<Dim, Dim, T> rotation = Mat<Dim, Dim, T>::identity();
    Vec<Dim, T> translation{};
 
    /**************************************************************************************
    |                                                                                     |
    |                                    Constructors                                     |
    |                                                                                     |
    **************************************************************************************/
 
    constexpr Transform() noexcept = default;
 
    constexpr Transform(Transform const&) noexcept = default;
 
    constexpr Transform(Mat<Dim, Dim, T> const& rotation, Vec<Dim, T> const& translation)
        : rotation(rotation), translation(translation)
    {
    }
 
    constexpr explicit Transform(Mat<Dim, Dim, T> const& rotation) : rotation(rotation) {}
 
    template <std::floating_point T1, std::floating_point T2>
    constexpr Transform(Mat<Dim, Dim, T1> const& rotation, Vec<Dim, T2> const& translation)
        : rotation(rotation), translation(translation)
    {
    }
 
    template <std::floating_point U>
    constexpr explicit Transform(Mat<Dim, Dim, U> const& rotation) : rotation(rotation)
    {
    }
 
    template <std::floating_point T1, std::floating_point T2>
      requires(Dim == 2)
    constexpr Transform(T1 const& angle, Vec<2, T2> const& translation)
        : translation(translation)
    {
        auto const s   = std::sin(angle);
        auto const c   = std::cos(angle);
        this->rotation = Mat<2, 2, T>(c, -s, s, c);
    }
 
    template <std::floating_point U>
      requires(Dim == 2)
    constexpr explicit Transform(U const& angle)
    {
        auto const s   = std::sin(angle);
        auto const c   = std::cos(angle);
        this->rotation = Mat<2, 2, T>(c, -s, s, c);
    }
 
    template <std::floating_point T1, std::floating_point T2>
      requires(Dim == 3)
    constexpr Transform(Quat<T1> const& rotation, Vec<3, T2> const& translation)
        : Transform(Mat<3, 3, T1>(rotation), translation)
    {
    }
 
    template <std::floating_point U>
      requires(Dim == 3)
    constexpr explicit Transform(Quat<U> const& rotation)
        : Transform(Mat<3, 3, U>(rotation))
    {
    }
 
    template <std::floating_point U>
    constexpr explicit Transform(Mat<Dim + 1, Dim + 1, U> const& m)
    {
        rotation = Mat<Dim, Dim, U>(m);
        for (std::size_t i = 0; i < Dim; ++i) {
            translation[i] = m[i][Dim];
        }
    }
 
    template <std::floating_point U>
    constexpr explicit Transform(Transform<Dim, U> const& other) noexcept
        : rotation(other.rotation), translation(other.translation)
    {
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                                 Assignment operator                                 |
    |                                                                                     |
    **************************************************************************************/
 
    constexpr Transform& operator=(Transform const&) noexcept = default;
 
    template <std::floating_point U>
    constexpr Transform& operator=(Transform<Dim, U> const& rhs) noexcept
    {
        rotation    = rhs.rotation;
        translation = rhs.translation;
        return *this;
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                                 Conversion operators                                |
    |                                                                                     |
    **************************************************************************************/
 
    template <std::floating_point U>
    constexpr explicit operator Mat<Dim, Dim, U>() const
    {
        return Mat<Dim, Dim, U>(rotation);
    }
 
    template <std::floating_point U>
    constexpr explicit operator Mat<Dim + 1, Dim + 1, U>() const
    {
        Mat<Dim + 1, Dim + 1, U> m{Mat<Dim, Dim, U>(rotation)};
        for (std::size_t i = 0; i < Dim; ++i) {
            m[i][Dim] = static_cast<U>(translation[i]);
        }
        m[Dim][Dim] = U(1);
        return m;
    }
 
    template <std::floating_point U>
      requires(Dim == 3)
    constexpr explicit operator Quat<U>() const
    {
        return Quat<U>(rotation);
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                                      Operations                                     |
    |                                                                                     |
    **************************************************************************************/
 
    template <std::floating_point U>
    [[nodiscard]] constexpr Vec<Dim, U> operator()(Vec<Dim, U> const& v) const
    {
        return Mat<Dim, Dim, U>(rotation) * v + Vec<Dim, U>(translation);
    }
 
    template <std::floating_point U>
      requires(Dim == 3)
    [[nodiscard]] constexpr Quat<U> operator()(Quat<U> const& q) const
    {
        return Quat<U>(*this) * q;
    }
 
    [[nodiscard]] constexpr T theta() const
      requires(Dim == 2)
    {
        return std::atan2(rotation[1][0], rotation[0][0]);
    }
 
    constexpr Transform& operator*=(Transform const& t)
    {
        auto tmp = (*this)(t.translation);
        rotation *= t.rotation;
        translation = tmp;
        return *this;
    }
 
    [[nodiscard]] constexpr bool operator==(Transform const& other) const noexcept =
        default;
};
 
/**************************************************************************************
|                                                                                     |
|                                     Aliases                                         |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T = float>
using Transform2 = Transform<2, T>;
template <std::floating_point T = float>
using Transform3 = Transform<3, T>;
 
using Transform2f = Transform2<float>;
using Transform2d = Transform2<double>;
using Transform3f = Transform3<float>;
using Transform3d = Transform3<double>;
 
/**************************************************************************************
|                                                                                     |
|                                  Binary operators                                   |
|                                                                                     |
**************************************************************************************/
 
template <std::size_t Dim, std::floating_point T>
[[nodiscard]] constexpr Transform<Dim, T> operator*(Transform<Dim, T>        t1,
                                                    Transform<Dim, T> const& t2)
{
    t1 *= t2;
    return t1;
}
 
template <std::size_t Dim, std::floating_point T>
[[nodiscard]] constexpr Vec<Dim, T> operator*(Transform<Dim, T> const& t,
                                              Vec<Dim, T> const&       v)
{
    return t(v);
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> operator*(Transform<3, T> const& t, Quat<T> const& q)
{
    return t(q);
}
 
template <std::size_t Dim, std::floating_point T>
std::ostream& operator<<(std::ostream& out, Transform<Dim, T> const& t)
{
    out << "Translation: " << t.translation << " ";
    if constexpr (Dim == 2) {
        out << "Theta: " << t.theta();
    } else {
        out << "Rotation: " << Quat<T>(t);
    }
    return out;
}
 
/**************************************************************************************
|                                                                                     |
|                                    Free functions                                   |
|                                                                                     |
**************************************************************************************/
 
template <std::size_t Dim, std::floating_point T>
[[nodiscard]] constexpr Transform<Dim, T> inverse(Transform<Dim, T> const& t)
{
    Mat<Dim, Dim, T> const inv_r = transpose(t.rotation);
    return {inv_r, inv_r * -t.translation};
}
}  // namespace ufo
 
/**************************************************************************************
|                                                                                     |
|                               std::formatter support                                |
|                                                                                     |
**************************************************************************************/
 
template <std::size_t Dim, std::floating_point T>
  requires(std::formattable<T, char>)
struct std::formatter<ufo::Transform<Dim, T>> {
    constexpr auto parse(std::format_parse_context& ctx) { return ctx.begin(); }
 
    auto format(ufo::Transform<Dim, T> const& t, std::format_context& ctx) const
    {
        if constexpr (Dim == 2) {
            return std::format_to(ctx.out(), "translation: {} Theta: {}", t.translation,
                                  t.theta());
        } else {
            return std::format_to(ctx.out(), "translation: {} Rotation: {}", t.translation,
                                  ufo::Quat<T>(t));
        }
    }
};
 
#endif  // UFO_NUMERIC_TRANSFORM_HPP