quat.hpp

 
#ifndef UFO_NUMERIC_QUAT_HPP
#define UFO_NUMERIC_QUAT_HPP
 
// UFO
#include <ufo/numeric/mat.hpp>
#include <ufo/numeric/vec.hpp>
 
// STL
#include <algorithm>
#include <cassert>
#include <cmath>
#include <concepts>
#include <format>
#include <limits>
#include <numbers>
#include <ostream>
 
namespace ufo
{
 
template <std::floating_point T = float>
struct Quat {
    using value_type = T;
    using size_type  = std::size_t;
 
    T w{1};  // Real (scalar) part.
    T x{};   // First imaginary component.
    T y{};   // Second imaginary component.
    T z{};   // Third imaginary component.
 
    /**************************************************************************************
    |                                                                                     |
    |                                    Constructors                                     |
    |                                                                                     |
    **************************************************************************************/
 
    constexpr Quat() noexcept = default;
 
    constexpr Quat(Quat const&) noexcept = default;
 
    template <std::floating_point U>
    constexpr explicit(!std::is_same_v<T, U>) Quat(Quat<U> const& q) noexcept
        : w(static_cast<T>(q.w))
        , x(static_cast<T>(q.x))
        , y(static_cast<T>(q.y))
        , z(static_cast<T>(q.z))
    {
    }
 
    constexpr Quat(T w, T x, T y, T z) noexcept : w(w), x(x), y(y), z(z) {}
 
    Quat(Vec<3, T> const& u, Vec<3, T> const& v)
    {
        T         norm_u_norm_v = std::sqrt(dot(u, u) * dot(v, v));
        T         real_part     = norm_u_norm_v + dot(u, v);
        Vec<3, T> t;
 
        if (real_part < T(1.e-6) * norm_u_norm_v) {
            real_part = T(0);
            t         = std::abs(u[0]) > std::abs(u[2]) ? Vec<3, T>(-u[1], u[0], T(0))
                                                        : Vec<3, T>(T(0), -u[2], u[1]);
        } else {
            t = cross(u, v);
        }
 
        *this = normalize(Quat<T>(real_part, t[0], t[1], t[2]));
    }
 
    explicit Quat(Vec<3, T> const& euler_angles)
    {
        Vec<3, T> const c{std::cos(euler_angles[0] * T(0.5)),
                          std::cos(euler_angles[1] * T(0.5)),
                          std::cos(euler_angles[2] * T(0.5))};
        Vec<3, T> const s{std::sin(euler_angles[0] * T(0.5)),
                          std::sin(euler_angles[1] * T(0.5)),
                          std::sin(euler_angles[2] * T(0.5))};
 
        w = c[0] * c[1] * c[2] + s[0] * s[1] * s[2];
        x = s[0] * c[1] * c[2] - c[0] * s[1] * s[2];
        y = c[0] * s[1] * c[2] + s[0] * c[1] * s[2];
        z = c[0] * c[1] * s[2] - s[0] * s[1] * c[2];
    }
 
    constexpr Quat(Mat<3, 3, T> const& m) noexcept
    {
        T const trace = m[0][0] + m[1][1] + m[2][2];
 
        if (trace > T(0)) {
            T s = std::sqrt(trace + T(1));
            w   = s * T(0.5);
            s   = T(0.5) / s;
            // Row-major: m[row][col] — x = (R_{2,1} - R_{1,2}) * s
            x = (m[2][1] - m[1][2]) * s;
            y = (m[0][2] - m[2][0]) * s;
            z = (m[1][0] - m[0][1]) * s;
        } else {
            unsigned const i = m[0][0] < m[1][1] ? (m[1][1] < m[2][2] ? 2u : 1u)
                                                 : (m[0][0] < m[2][2] ? 2u : 0u);
            unsigned const j = (i + 1u) % 3u;
            unsigned const k = (i + 2u) % 3u;
 
            T q[4]{};
            T s      = std::sqrt(m[i][i] - m[j][j] - m[k][k] + T(1));
            q[i + 1] = s * T(0.5);
            s        = T(0.5) / s;
            q[0]     = (m[k][j] - m[j][k]) * s;  // row-major: swap j,k vs column-major
            q[j + 1] = (m[j][i] + m[i][j]) * s;  // symmetric — same as column-major
            q[k + 1] = (m[k][i] + m[i][k]) * s;
            w        = q[0];
            x        = q[1];
            y        = q[2];
            z        = q[3];
        }
    }
 
    constexpr Quat(Mat<4, 4, T> const& m) noexcept : Quat(Mat<3, 3, T>(m)) {}
 
    /**************************************************************************************
    |                                                                                     |
    |                                 Assignment operator                                 |
    |                                                                                     |
    **************************************************************************************/
 
    constexpr Quat& operator=(Quat const&) noexcept = default;
 
    template <std::floating_point U>
    constexpr Quat& operator=(Quat<U> const& rhs) noexcept
    {
        w = static_cast<T>(rhs.w);
        x = static_cast<T>(rhs.x);
        y = static_cast<T>(rhs.y);
        z = static_cast<T>(rhs.z);
        return *this;
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                                 Conversion operator                                 |
    |                                                                                     |
    **************************************************************************************/
 
    [[nodiscard]] constexpr explicit operator Mat<3, 3, T>() const noexcept
    {
        T const xx(x * x), yy(y * y), zz(z * z);
        T const xz(x * z), xy(x * y), yz(y * z);
        T const wx(w * x), wy(w * y), wz(w * z);
 
        return Mat<3, 3, T>(
            Vec<3, T>(T(1) - T(2) * (yy + zz), T(2) * (xy - wz), T(2) * (xz + wy)),
            Vec<3, T>(T(2) * (xy + wz), T(1) - T(2) * (xx + zz), T(2) * (yz - wx)),
            Vec<3, T>(T(2) * (xz - wy), T(2) * (yz + wx), T(1) - T(2) * (xx + yy)));
    }
 
    [[nodiscard]] constexpr explicit operator Mat<4, 4, T>() const noexcept
    {
        // TODO: Should we add a constructor for this?
        Mat<4, 4, T> m(Mat<3, 3, T>(*this));
        m[3][3] = T(1);
        return m;
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                                   Element access                                    |
    |                                                                                     |
    **************************************************************************************/
 
    [[nodiscard]] constexpr auto& operator[](this auto& self, size_type pos) noexcept
    {
        assert(4 > pos);
        switch (pos) {
            case 0: return self.w;
            case 1: return self.x;
            case 2: return self.y;
            default: return self.z;
        }
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                            Compound assignment operator                             |
    |                                                                                     |
    **************************************************************************************/
 
    constexpr Quat& operator+=(Quat const& q) noexcept
    {
        w += q.w;
        x += q.x;
        y += q.y;
        z += q.z;
        return *this;
    }
 
    constexpr Quat& operator-=(Quat const& q) noexcept
    {
        w -= q.w;
        x -= q.x;
        y -= q.y;
        z -= q.z;
        return *this;
    }
 
    constexpr Quat& operator*=(Quat const& q) noexcept
    {
        Quat const p(*this);
        w = p.w * q.w - p.x * q.x - p.y * q.y - p.z * q.z;
        x = p.w * q.x + p.x * q.w + p.y * q.z - p.z * q.y;
        y = p.w * q.y + p.y * q.w + p.z * q.x - p.x * q.z;
        z = p.w * q.z + p.z * q.w + p.x * q.y - p.y * q.x;
        return *this;
    }
 
    constexpr Quat& operator*=(T s) noexcept
    {
        w *= s;
        x *= s;
        y *= s;
        z *= s;
        return *this;
    }
 
    constexpr Quat& operator/=(T s) noexcept
    {
        w /= s;
        x /= s;
        y /= s;
        z /= s;
        return *this;
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                                      Capacity                                       |
    |                                                                                     |
    **************************************************************************************/
 
    [[nodiscard]] static constexpr size_type size() noexcept { return 4; }
 
    /**************************************************************************************
    |                                                                                     |
    |                                     Operations                                      |
    |                                                                                     |
    **************************************************************************************/
 
    void swap(Quat& other) noexcept
    {
        using std::swap;
        swap(w, other.w);
        swap(x, other.x);
        swap(y, other.y);
        swap(z, other.z);
    }
 
    constexpr bool operator==(Quat const&) const noexcept = default;
};
 
/**************************************************************************************
|                                                                                     |
|                                     Aliases                                         |
|                                                                                     |
**************************************************************************************/
 
using Quatf = Quat<float>;
using Quatd = Quat<double>;
 
/**************************************************************************************
|                                                                                     |
|                                   Unary operators                                   |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> operator+(Quat<T> const& q) noexcept
{
    return q;
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> operator-(Quat<T> const& q) noexcept
{
    return {-q.w, -q.x, -q.y, -q.z};
}
 
/**************************************************************************************
|                                                                                     |
|                                  Binary operators                                   |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> operator+(Quat<T> lhs, Quat<T> const& rhs) noexcept
{
    lhs += rhs;
    return lhs;
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> operator-(Quat<T> lhs, Quat<T> const& rhs) noexcept
{
    lhs -= rhs;
    return lhs;
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> operator*(Quat<T> lhs, Quat<T> const& rhs) noexcept
{
    lhs *= rhs;
    return lhs;
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Vec<3, T> operator*(Quat<T> const& q, Vec<3, T> const& v) noexcept
{
    Vec<3, T> const qvec(q.x, q.y, q.z);
    Vec<3, T> const uv(cross(qvec, v));
    Vec<3, T> const uuv(cross(qvec, uv));
    return v + ((uv * q.w) + uuv) * T(2);
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Vec<3, T> operator*(Vec<3, T> const& v, Quat<T> const& q) noexcept
{
    return inverse(q) * v;
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Vec<4, T> operator*(Quat<T> const& q, Vec<4, T> const& v) noexcept
{
    Vec<3, T> const r = q * Vec<3, T>(v[0], v[1], v[2]);
    return Vec<4, T>(r[0], r[1], r[2], v[3]);
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Vec<4, T> operator*(Vec<4, T> const& v, Quat<T> const& q) noexcept
{
    Vec<3, T> const r = inverse(q) * Vec<3, T>(v[0], v[1], v[2]);
    return Vec<4, T>(r[0], r[1], r[2], v[3]);
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> operator*(Quat<T> lhs, T s) noexcept
{
    lhs *= s;
    return lhs;
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> operator*(T s, Quat<T> rhs) noexcept
{
    rhs *= s;
    return rhs;
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> operator/(Quat<T> lhs, T s) noexcept
{
    lhs /= s;
    return lhs;
}
 
/**************************************************************************************
|                                                                                     |
|                                     Operations                                      |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
void swap(Quat<T>& lhs, Quat<T>& rhs) noexcept
{
    lhs.swap(rhs);
}
 
/**************************************************************************************
|                                                                                     |
|                                      Functions                                      |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
[[nodiscard]] constexpr T dot(Quat<T> const& a, Quat<T> const& b) noexcept
{
    return a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
}
 
template <std::floating_point T>
[[nodiscard]] constexpr T normSquared(Quat<T> const& q) noexcept
{
    return dot(q, q);
}
 
template <std::floating_point T>
[[nodiscard]] constexpr T norm(Quat<T> const& q) noexcept
{
    return std::sqrt(normSquared(q));
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> normalize(Quat<T> const& q) noexcept
{
    T const len = norm(q);
    if (len <= T(0)) {
        return {T(1), T(0), T(0), T(0)};
    }
    return q / len;
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> conjugate(Quat<T> const& q) noexcept
{
    return {q.w, -q.x, -q.y, -q.z};
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> inverse(Quat<T> const& q) noexcept
{
    return conjugate(q) / normSquared(q);
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> cross(Quat<T> const& q1, Quat<T> const& q2) noexcept
{
    return {q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
            q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
            q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z,
            q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x};
}
 
/**************************************************************************************
|                                                                                     |
|                                    Interpolation                                    |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
[[nodiscard]] Quat<T> mix(Quat<T> const& x, Quat<T> const& y, T a)
{
    T const cos_theta = dot(x, y);
 
    if (cos_theta > T(1) - std::numeric_limits<T>::epsilon()) {
        return {std::lerp(x.w, y.w, a), std::lerp(x.x, y.x, a), std::lerp(x.y, y.y, a),
                std::lerp(x.z, y.z, a)};
    }
 
    T const angle = std::acos(cos_theta);
    return (std::sin((T(1) - a) * angle) * x + std::sin(a * angle) * y) / std::sin(angle);
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> lerp(Quat<T> const& x, Quat<T> const& y, T a) noexcept
{
    assert(a >= T(0) && a <= T(1));
    return x * (T(1) - a) + y * a;
}
 
template <std::floating_point T>
[[nodiscard]] Quat<T> slerp(Quat<T> const& x, Quat<T> const& y, T a)
{
    Quat<T> z         = y;
    T       cos_theta = dot(x, y);
 
    if (cos_theta < T(0)) {
        z         = -y;
        cos_theta = -cos_theta;
    }
 
    if (cos_theta > T(1) - std::numeric_limits<T>::epsilon()) {
        return {std::lerp(x.w, z.w, a), std::lerp(x.x, z.x, a), std::lerp(x.y, z.y, a),
                std::lerp(x.z, z.z, a)};
    }
 
    T const angle = std::acos(cos_theta);
    return (std::sin((T(1) - a) * angle) * x + std::sin(a * angle) * z) / std::sin(angle);
}
 
// k-th extra spin slerp (Graphics Gems III, page 96)
template <std::floating_point T, std::floating_point S>
[[nodiscard]] Quat<T> slerp(Quat<T> const& x, Quat<T> const& y, T a, S k)
{
    Quat<T> z         = y;
    T       cos_theta = dot(x, y);
 
    if (cos_theta < T(0)) {
        z         = -y;
        cos_theta = -cos_theta;
    }
 
    if (cos_theta > T(1) - std::numeric_limits<T>::epsilon()) {
        return {std::lerp(x.w, z.w, a), std::lerp(x.x, z.x, a), std::lerp(x.y, z.y, a),
                std::lerp(x.z, z.z, a)};
    }
 
    T const angle = std::acos(cos_theta);
    T const phi   = angle + T(k) * std::numbers::pi_v<T>;
    return (std::sin(angle - a * phi) * x + std::sin(a * phi) * z) / std::sin(angle);
}
 
/**************************************************************************************
|                                                                                     |
|                                    Trigonometric                                    |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
[[nodiscard]] T angle(Quat<T> const& q) noexcept
{
    constexpr T cos_one_over_two = T(0.877582561890372716130286068203503191);
    if (std::abs(q.w) > cos_one_over_two) {
        T const a = std::asin(std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z)) * T(2);
        return q.w < T(0) ? std::numbers::pi_v<T> * T(2) - a : a;
    }
    return std::acos(q.w) * T(2);
}
 
template <std::floating_point T>
[[nodiscard]] Vec<3, T> axis(Quat<T> const& q) noexcept
{
    T const tmp = T(1) - q.w * q.w;
    if (tmp <= T(0)) {
        return Vec<3, T>(T(0), T(0), T(1));
    }
    return Vec<3, T>(q.x, q.y, q.z) / std::sqrt(tmp);
}
 
template <std::floating_point T>
[[nodiscard]] constexpr Quat<T> angleAxis(T angle, Vec<3, T> const& a) noexcept
{
    T const s = std::sin(angle * T(0.5));
    return Quat<T>(std::cos(angle * T(0.5)), a[0] * s, a[1] * s, a[2] * s);
}
 
/**************************************************************************************
|                                                                                     |
|                                    Euler angles                                     |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
[[nodiscard]] T roll(Quat<T> const& q) noexcept
{
    return T(std::atan2(T(2) * (q.x * q.y + q.w * q.z),
                        q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z));
}
 
template <std::floating_point T>
[[nodiscard]] T pitch(Quat<T> const& q) noexcept
{
    return T(std::atan2(T(2) * (q.y * q.z + q.w * q.x),
                        q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z));
}
 
template <std::floating_point T>
[[nodiscard]] T yaw(Quat<T> const& q) noexcept
{
    return std::asin(std::clamp(T(-2) * (q.x * q.z - q.w * q.y), T(-1), T(1)));
}
 
template <std::floating_point T>
[[nodiscard]] Vec<3, T> eulerAngles(Quat<T> const& q) noexcept
{
    return Vec<3, T>{pitch(q), yaw(q), roll(q)};
}
 
/**************************************************************************************
|                                                                                     |
|                                     Transform                                       |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
[[nodiscard]] Quat<T> rotate(Quat<T> const& q, T angle, Vec<3, T> axis)
{
    T const len = length(axis);
    if (std::abs(len - T(1)) > T(0.001)) {
        axis /= len;
    }
    T const s = std::sin(angle * T(0.5));
    return q * Quat<T>(std::cos(angle * T(0.5)), axis[0] * s, axis[1] * s, axis[2] * s);
}
 
template <std::floating_point T>
[[nodiscard]] Quat<T> quatLookAt(Vec<3, T> const& direction, Vec<3, T> const& up)
{
    Vec<3, T> const fwd   = -direction;
    Vec<3, T>       right = cross(up, fwd);
    right /= std::max(std::sqrt(T(0.00001)), norm(right));
    Vec<3, T> const up_c = cross(fwd, right);
 
    // Build rotation matrix in row-major where columns are right, up_c, fwd
    Mat<3, 3, T> const m(Vec<3, T>(right[0], up_c[0], fwd[0]),
                         Vec<3, T>(right[1], up_c[1], fwd[1]),
                         Vec<3, T>(right[2], up_c[2], fwd[2]));
    return Quat<T>(m);
}
 
/**************************************************************************************
|                                                                                     |
|                                    isnan / isinf                                    |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
[[nodiscard]] Vec<4, bool> isnan(Quat<T> const& q) noexcept
{
    return Vec<4, bool>(std::isnan(q.w), std::isnan(q.x), std::isnan(q.y), std::isnan(q.z));
}
 
template <std::floating_point T>
[[nodiscard]] Vec<4, bool> isinf(Quat<T> const& q) noexcept
{
    return Vec<4, bool>(std::isinf(q.w), std::isinf(q.x), std::isinf(q.y), std::isinf(q.z));
}
 
/**************************************************************************************
|                                                                                     |
|                                       Print                                         |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
std::ostream& operator<<(std::ostream& out, Quat<T> const& q)
{
    return out << "qw: " << q.w << " qx: " << q.x << " qy: " << q.y << " qz: " << q.z;
}
 
}  // namespace ufo
 
template <std::floating_point T>
  requires std::formattable<T, char>
struct std::formatter<ufo::Quat<T>> {
    constexpr auto parse(std::format_parse_context& ctx) { return ctx.begin(); }
 
    auto format(ufo::Quat<T> const& q, std::format_context& ctx) const
    {
        return std::format_to(ctx.out(), "qw: {} qx: {} qy: {} qz: {}", q.w, q.x, q.y, q.z);
    }
};
 
#endif  // UFO_NUMERIC_QUAT_HPP