mat.hpp

 
#ifndef UFO_NUMERIC_MAT_HPP
#define UFO_NUMERIC_MAT_HPP
 
// UFO
#include <ufo/numeric/vec.hpp>
 
// STL
#include <algorithm>
#include <array>
#include <cmath>
#include <concepts>
#include <cstddef>
#include <format>
#include <functional>
#include <limits>
#include <numbers>
#include <optional>
#include <ostream>
#include <ranges>
#include <span>
#include <type_traits>
#include <utility>
 
namespace ufo
{
// Forward declaration
template <std::size_t Rows, std::size_t Cols, class T>
struct Mat;
 
/**************************************************************************************
|                                                                                     |
|                                      Concepts                                       |
|                                                                                     |
**************************************************************************************/
 
template <class T>
concept Matrix = requires(T const& t) {
    []<std::size_t R, std::size_t C, class U>(Mat<R, C, U> const&) {}(t);
};
 
template <class M>
concept SquareMatrix = Matrix<M> && (M::rows() == M::cols());
 
template <class M>
concept InvertibleMatrix = SquareMatrix<M> && std::floating_point<typename M::value_type>;
 
template <std::size_t Rows, std::size_t Cols, class T>
struct Mat {
    using value_type = T;
    using size_type  = std::size_t;
    using row_type   = Vec<Cols, T>;
    using col_type   = Vec<Rows, T>;
 
    std::array<row_type, Rows> fields{};
 
    constexpr Mat() noexcept = default;
 
    template <std::convertible_to<T>... Args>
      requires(sizeof...(Args) == Rows * Cols)
    explicit(sizeof...(Args) == 1) constexpr Mat(Args... args) noexcept
    {
        auto elements = std::array<T, Rows * Cols>{static_cast<T>(args)...};
        for (std::size_t r = 0; r < Rows; ++r) {
            for (std::size_t c = 0; c < Cols; ++c) {
                fields[r][c] = elements[r * Cols + c];
            }
        }
    }
 
    [[nodiscard]] static consteval Mat zeros() noexcept { return {}; }
 
    [[nodiscard]] static consteval Mat identity() noexcept
      requires(Rows == Cols)
    {
        Mat m{};
        for (std::size_t i = 0; i < Rows; ++i) {
            m[i][i] = T(1);
        }
        return m;
    }
 
    [[nodiscard]] static consteval Mat ones() noexcept
    {
        Mat m{};
        for (auto& row : m.fields) {
            std::ranges::fill(row.fields, T(1));
        }
        return m;
    }
 
    template <std::convertible_to<row_type>... Rs>
      requires(sizeof...(Rs) == Rows)
    constexpr Mat(Rs&&... rs) noexcept
        : fields{static_cast<row_type>(std::forward<Rs>(rs))...}
    {
    }
 
    template <class U>
    constexpr explicit(!std::is_same_v<T, U>) Mat(Mat<Rows, Cols, U> const& other) noexcept
    {
        std::ranges::transform(other.fields, fields.begin(),
                               [](auto const& row) { return row_type(row); });
    }
 
    template <std::size_t R2, std::size_t C2>
      requires(R2 != Rows || C2 != Cols)
    constexpr explicit Mat(Mat<R2, C2, T> const& other) noexcept
    {
        // Copy overlapping region, leave rest as zero
        auto const max_r = std::min(Rows, R2);
        auto const max_c = std::min(Cols, C2);
        for (std::size_t r = 0; r < max_r; ++r) {
            for (std::size_t c = 0; c < max_c; ++c) {
                fields[r][c] = other.fields[r][c];
            }
        }
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                                     Accessors                                      |
    |                                                                                     |
    **************************************************************************************/
 
    constexpr auto& operator[](this auto& self, size_type r, size_type c) noexcept
    {
        return self.fields[r][c];
    }
 
    constexpr auto& operator[](this auto& self, size_type i) noexcept
    {
        return self.fields[i];
    }
 
    constexpr auto& at(this auto& self, size_type r, size_type c)
    {
        if (r >= Rows || c >= Cols) [[unlikely]] {
            throw std::out_of_range("Matrix index out of bounds");
        }
        return self.fields[r][c];
    }
 
    [[nodiscard]] constexpr row_type const& row(size_type r) const noexcept
    {
        return fields[r];
    }
 
    [[nodiscard]] constexpr col_type col(size_type c) const noexcept
    {
        col_type result{};
        for (std::size_t r = 0; r < Rows; ++r) {
            result[r] = fields[r][c];
        }
        return result;
    }
 
    [[nodiscard]] constexpr auto begin(this auto& self) noexcept
    {
        return self.fields.begin();
    }
 
    [[nodiscard]] constexpr auto end(this auto& self) noexcept { return self.fields.end(); }
 
    [[nodiscard]] constexpr auto data(this auto& self) noexcept
    {
        return self.fields.data();
    }
 
    [[nodiscard]] static constexpr std::size_t rows() noexcept { return Rows; }
 
    [[nodiscard]] static constexpr std::size_t cols() noexcept { return Cols; }
 
    [[nodiscard]] static constexpr std::size_t size() noexcept { return Rows * Cols; }
 
    constexpr bool operator==(Mat const&) const = default;
 
    /**************************************************************************************
    |                                                                                     |
    |                                Essential Operations                                  |
    |                                                                                     |
    **************************************************************************************/
 
    [[nodiscard]] constexpr T trace() const noexcept
      requires(Rows == Cols)
    {
        T result{};
        for (std::size_t i = 0; i < Rows; ++i) {
            result += fields[i][i];
        }
        return result;
    }
 
    [[nodiscard]] constexpr T frobenius_norm() const noexcept
    {
        T sum{};
        for (auto const& row : fields) {
            for (auto const& elem : row.fields) {
                sum += elem * elem;
            }
        }
        return std::sqrt(sum);
    }
 
    [[nodiscard]] constexpr bool isNearZero(T epsilon = std::numeric_limits<T>::epsilon() *
                                                        T(100)) const noexcept
      requires std::floating_point<T>
    {
        return frobenius_norm() < epsilon;
    }
 
    [[nodiscard]] constexpr T conditionNumber() const noexcept
      requires InvertibleMatrix<Mat>
    {
        // Estimate using Frobenius norm (rough approximation)
        auto inv = inverse(*this);
        return frobenius_norm() * inv.frobenius_norm();
    }
 
    [[nodiscard]] constexpr std::optional<Mat> safe_inverse(
        T epsilon = std::numeric_limits<T>::epsilon() * T(1000)) const noexcept
      requires InvertibleMatrix<Mat>
    {
        auto det = determinant(*this);
        if (std::abs(det) < epsilon) {
            return std::nullopt;
        }
        return inverse(*this);
    }
 
    [[nodiscard]] constexpr auto flat_view(this auto& self) noexcept
    {
        return std::span{self.data()->data(), Rows * Cols};
    }
 
    template <std::size_t R2>
    [[nodiscard]] constexpr auto block(std::size_t start_row,
                                       std::size_t start_col) const noexcept
        -> Mat<R2, R2, T>
      requires(R2 <= Rows && R2 <= Cols)
    {
        Mat<R2, R2, T> result{};
        for (std::size_t r = 0; r < R2; ++r) {
            for (std::size_t c = 0; c < R2; ++c) {
                result[r][c] = fields[start_row + r][start_col + c];
            }
        }
        return result;
    }
 
    template <std::predicate<T> Pred>
    [[nodiscard]] constexpr bool all_of(Pred&& pred) const noexcept
    {
        return std::ranges::all_of(flat_view(), std::forward<Pred>(pred));
    }
 
    template <std::predicate<T> Pred>
    [[nodiscard]] constexpr bool any_of(Pred&& pred) const noexcept
    {
        return std::ranges::any_of(flat_view(), std::forward<Pred>(pred));
    }
 
    template <class UnaryOp>
    [[nodiscard]] constexpr Mat transform(UnaryOp&& op) const noexcept
    {
        Mat result{};
        std::ranges::transform(flat_view(), result.flat_view().begin(),
                               std::forward<UnaryOp>(op));
        return result;
    }
 
    [[nodiscard]] static constexpr bool is_column_major() noexcept { return false; }
 
    [[nodiscard]] static constexpr bool is_row_major() noexcept { return true; }
 
    [[nodiscard]] static constexpr std::size_t memory_footprint() noexcept
    {
        return sizeof(Mat);
    }
 
    [[nodiscard]] constexpr bool isDiagonal(
        T epsilon = std::numeric_limits<T>::epsilon()) const noexcept
      requires SquareMatrix<Mat>
    {
        for (std::size_t r = 0; r < Rows; ++r) {
            for (std::size_t c = 0; c < Cols; ++c) {
                if (r != c && std::abs(fields[r][c]) > epsilon) {
                    return false;
                }
            }
        }
        return true;
    }
 
    [[nodiscard]] constexpr bool isSymmetric(
        T epsilon = std::numeric_limits<T>::epsilon()) const noexcept
      requires SquareMatrix<Mat>
    {
        for (std::size_t r = 0; r < Rows; ++r) {
            for (std::size_t c = r + 1; c < Cols; ++c) {
                if (std::abs(fields[r][c] - fields[c][r]) > epsilon) {
                    return false;
                }
            }
        }
        return true;
    }
 
    [[nodiscard]] constexpr bool isUpperTriangular(
        T epsilon = std::numeric_limits<T>::epsilon()) const noexcept
      requires SquareMatrix<Mat>
    {
        for (std::size_t r = 1; r < Rows; ++r) {
            for (std::size_t c = 0; c < r; ++c) {
                if (std::abs(fields[r][c]) > epsilon) {
                    return false;
                }
            }
        }
        return true;
    }
 
    [[nodiscard]] constexpr bool isLowerTriangular(
        T epsilon = std::numeric_limits<T>::epsilon()) const noexcept
      requires SquareMatrix<Mat>
    {
        for (std::size_t r = 0; r < Rows - 1; ++r) {
            for (std::size_t c = r + 1; c < Cols; ++c) {
                if (std::abs(fields[r][c]) > epsilon) {
                    return false;
                }
            }
        }
        return true;
    }
 
    [[nodiscard]] constexpr std::size_t rank(T epsilon = std::numeric_limits<T>::epsilon() *
                                                         T(1000)) const noexcept
      requires std::floating_point<T>
    {
        auto        temp = *this;
        std::size_t rank = 0;
 
        for (std::size_t r = 0; r < std::min(Rows, Cols); ++r) {
            // Find pivot
            std::size_t pivot_row = r;
            for (std::size_t i = r + 1; i < Rows; ++i) {
                if (std::abs(temp[i][r]) > std::abs(temp[pivot_row][r])) {
                    pivot_row = i;
                }
            }
 
            if (std::abs(temp[pivot_row][r]) <= epsilon) continue;
 
            if (pivot_row != r) {
                std::ranges::swap(temp[r], temp[pivot_row]);
            }
 
            rank++;
 
            // Eliminate below
            for (std::size_t i = r + 1; i < Rows; ++i) {
                if (std::abs(temp[i][r]) <= epsilon) continue;
 
                T factor = temp[i][r] / temp[r][r];
                for (std::size_t j = r; j < Cols; ++j) {
                    temp[i][j] -= factor * temp[r][j];
                }
            }
        }
 
        return rank;
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                              Unary arithmetic operator                              |
    |                                                                                     |
    **************************************************************************************/
 
    constexpr Mat operator+() const noexcept { return *this; }
 
    constexpr Mat operator-() const noexcept
    {
        Mat result;
        std::ranges::transform(fields, result.fields.begin(), std::negate{});
        return result;
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                            Compound assignment operator                             |
    |                                                                                     |
    **************************************************************************************/
 
    constexpr Mat& operator+=(Mat const& rhs) noexcept
    {
        std::ranges::transform(fields, rhs.fields, fields.begin(), std::plus{});
        return *this;
    }
 
    constexpr Mat& operator-=(Mat const& rhs) noexcept
    {
        std::ranges::transform(fields, rhs.fields, fields.begin(), std::minus{});
        return *this;
    }
 
    constexpr Mat& operator*=(Mat const& rhs) noexcept
      requires(Rows == Cols)
    {
        *this = *this * rhs;
        return *this;
    }
 
    /**************************************************************************************
    |                                                                                     |
    |                             Scalar compound assignment operator                     |
    |                                                                                     |
    **************************************************************************************/
 
    constexpr Mat& operator+=(T rhs) noexcept
    {
        for (auto& row : fields) row += rhs;
        return *this;
    }
 
    constexpr Mat& operator-=(T rhs) noexcept
    {
        for (auto& row : fields) row -= rhs;
        return *this;
    }
 
    constexpr Mat& operator*=(T rhs) noexcept
    {
        for (auto& row : fields) row *= rhs;
        return *this;
    }
 
    constexpr Mat& operator/=(T rhs) noexcept
    {
        for (auto& row : fields) row /= rhs;
        return *this;
    }
};
 
/**************************************************************************************
|                                                                                     |
|                                   Type Aliases                                      |
|                                                                                     |
**************************************************************************************/
 
// Primary aliases
template <class T = float>
using Mat2 = Mat<2, 2, T>;
template <class T = float>
using Mat3 = Mat<3, 3, T>;
template <class T = float>
using Mat4 = Mat<4, 4, T>;
 
// Common type instantiations
using Mat2f = Mat2<float>;
using Mat2d = Mat2<double>;
using Mat3f = Mat3<float>;
using Mat3d = Mat3<double>;
using Mat4f = Mat4<float>;
using Mat4d = Mat4<double>;
 
// Legacy compatibility - can be removed eventually
using Mat2x2f = Mat2f;
using Mat2x2d = Mat2d;
using Mat3x3f = Mat3f;
using Mat3x3d = Mat3d;
using Mat4x4f = Mat4f;
using Mat4x4d = Mat4d;
 
/**************************************************************************************
|                                                                                     |
|                                  Binary operators                                   |
|                                                                                     |
**************************************************************************************/
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator+(
    Mat<Rows, Cols, T> lhs, Mat<Rows, Cols, T> const& rhs) noexcept
{
    lhs += rhs;
    return lhs;
}
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator-(
    Mat<Rows, Cols, T> lhs, Mat<Rows, Cols, T> const& rhs) noexcept
{
    lhs -= rhs;
    return lhs;
}
 
/**************************************************************************************
|                                                                                     |
|                              Scalar binary operators                                |
|                                                                                     |
**************************************************************************************/
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator+(Mat<Rows, Cols, T> lhs,
                                                     T                  rhs) noexcept
{
    lhs += rhs;
    return lhs;
}
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator-(Mat<Rows, Cols, T> lhs,
                                                     T                  rhs) noexcept
{
    lhs -= rhs;
    return lhs;
}
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator*(Mat<Rows, Cols, T> lhs,
                                                     T                  rhs) noexcept
{
    lhs *= rhs;
    return lhs;
}
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator/(Mat<Rows, Cols, T> lhs,
                                                     T                  rhs) noexcept
{
    lhs /= rhs;
    return lhs;
}
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator+(T                  lhs,
                                                     Mat<Rows, Cols, T> rhs) noexcept
{
    rhs += lhs;
    return rhs;
}
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator-(
    T lhs, Mat<Rows, Cols, T> const& rhs) noexcept
{
    Mat<Rows, Cols, T> result;
    std::ranges::transform(rhs.fields, result.fields.begin(),
                           [lhs](auto const& row) { return lhs - row; });
    return result;
}
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator*(T                  lhs,
                                                     Mat<Rows, Cols, T> rhs) noexcept
{
    rhs *= lhs;
    return rhs;
}
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator/(
    T lhs, Mat<Rows, Cols, T> const& rhs) noexcept
{
    Mat<Rows, Cols, T> result;
    std::ranges::transform(rhs.fields, result.fields.begin(),
                           [lhs](auto const& row) { return lhs / row; });
    return result;
}
 
/**************************************************************************************
|                                                                                     |
|                             Matrix × vector product                                 |
|                                                                                     |
**************************************************************************************/
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Vec<Rows, T> operator*(Mat<Rows, Cols, T> const& m,
                                               Vec<Cols, T> const&       v) noexcept
{
    Vec<Rows, T> result{};
    for (std::size_t r = 0; r < Rows; ++r) {
        result[r] = dot(m.fields[r], v);
    }
    return result;
}
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Vec<Cols, T> operator*(Vec<Rows, T> const&       v,
                                               Mat<Rows, Cols, T> const& m) noexcept
{
    Vec<Cols, T> result{};
    for (std::size_t c = 0; c < Cols; ++c) {
        for (std::size_t r = 0; r < Rows; ++r) {
            result[c] += v[r] * m.fields[r][c];
        }
    }
    return result;
}
 
/**************************************************************************************
|                                                                                     |
|                             Matrix × matrix product                                 |
|                                                                                     |
**************************************************************************************/
 
// Mat<Rows, Shared, T> * Mat<Shared, Cols, T> -> Mat<Rows, Cols, T>
template <std::size_t Rows, std::size_t Shared, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Rows, Cols, T> operator*(
    Mat<Rows, Shared, T> const& a, Mat<Shared, Cols, T> const& b) noexcept
{
    Mat<Rows, Cols, T> result{};
    for (std::size_t r = 0; r < Rows; ++r) {
        for (std::size_t c = 0; c < Cols; ++c) {
            for (std::size_t k = 0; k < Shared; ++k) {
                result.fields[r][c] += a.fields[r][k] * b.fields[k][c];
            }
        }
    }
    return result;
}
 
/**************************************************************************************
|                                                                                     |
|                                      Functions                                      |
|                                                                                     |
**************************************************************************************/
 
template <SquareMatrix M>
[[nodiscard]] constexpr auto determinant(M const& m) noexcept
{
    using T          = typename M::value_type;
    constexpr auto N = M::rows();
 
    if constexpr (2 == N) {
        return m[0][0] * m[1][1] - m[0][1] * m[1][0];
    } else if constexpr (3 == N) {
        return m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) -
               m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) +
               m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
    } else if constexpr (4 == N) {
        // Use row-major indexing: m[row][col]
        T const A2323 = m[2][2] * m[3][3] - m[2][3] * m[3][2];
        T const A1323 = m[2][1] * m[3][3] - m[2][3] * m[3][1];
        T const A1223 = m[2][1] * m[3][2] - m[2][2] * m[3][1];
        T const A0323 = m[2][0] * m[3][3] - m[2][3] * m[3][0];
        T const A0223 = m[2][0] * m[3][2] - m[2][2] * m[3][0];
        T const A0123 = m[2][0] * m[3][1] - m[2][1] * m[3][0];
        return m[0][0] * (m[1][1] * A2323 - m[1][2] * A1323 + m[1][3] * A1223) -
               m[0][1] * (m[1][0] * A2323 - m[1][2] * A0323 + m[1][3] * A0223) +
               m[0][2] * (m[1][0] * A1323 - m[1][1] * A0323 + m[1][3] * A0123) -
               m[0][3] * (m[1][0] * A1223 - m[1][1] * A0223 + m[1][2] * A0123);
    } else {
        []<bool flag = false>() {
            static_assert(flag, "determinant only implemented for 2x2, 3x3, and 4x4 matrices");
        }();
    }
}
 
template <InvertibleMatrix M>
[[nodiscard]] constexpr M inverse(M const& m) noexcept
{
    using T          = typename M::value_type;
    constexpr auto N = M::rows();
 
    if constexpr (2 == N) {
        T const det_inv = T(1) / determinant(m);
        M       result{};
        result[0][0] = m[1][1] * det_inv;
        result[0][1] = -m[0][1] * det_inv;
        result[1][0] = -m[1][0] * det_inv;
        result[1][1] = m[0][0] * det_inv;
        return result;
    } else if constexpr (3 == N) {
        T const det_inv = T(1) / determinant(m);
        M       result{};
        result[0][0] = (m[1][1] * m[2][2] - m[1][2] * m[2][1]) * det_inv;
        result[0][1] = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]) * det_inv;
        result[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * det_inv;
        result[1][0] = -(m[1][0] * m[2][2] - m[1][2] * m[2][0]) * det_inv;
        result[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * det_inv;
        result[1][2] = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]) * det_inv;
        result[2][0] = (m[1][0] * m[2][1] - m[1][1] * m[2][0]) * det_inv;
        result[2][1] = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]) * det_inv;
        result[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]) * det_inv;
        return result;
    } else if constexpr (4 == N) {
        T const A2323 = m[2][2] * m[3][3] - m[2][3] * m[3][2];
        T const A1323 = m[2][1] * m[3][3] - m[2][3] * m[3][1];
        T const A1223 = m[2][1] * m[3][2] - m[2][2] * m[3][1];
        T const A0323 = m[2][0] * m[3][3] - m[2][3] * m[3][0];
        T const A0223 = m[2][0] * m[3][2] - m[2][2] * m[3][0];
        T const A0123 = m[2][0] * m[3][1] - m[2][1] * m[3][0];
        T const A2313 = m[1][2] * m[3][3] - m[1][3] * m[3][2];
        T const A1313 = m[1][1] * m[3][3] - m[1][3] * m[3][1];
        T const A1213 = m[1][1] * m[3][2] - m[1][2] * m[3][1];
        T const A2312 = m[1][2] * m[2][3] - m[1][3] * m[2][2];
        T const A1312 = m[1][1] * m[2][3] - m[1][3] * m[2][1];
        T const A1212 = m[1][1] * m[2][2] - m[1][2] * m[2][1];
        T const A0313 = m[1][0] * m[3][3] - m[1][3] * m[3][0];
        T const A0213 = m[1][0] * m[3][2] - m[1][2] * m[3][0];
        T const A0312 = m[1][0] * m[2][3] - m[1][3] * m[2][0];
        T const A0212 = m[1][0] * m[2][2] - m[1][2] * m[2][0];
        T const A0113 = m[1][0] * m[3][1] - m[1][1] * m[3][0];
        T const A0112 = m[1][0] * m[2][1] - m[1][1] * m[2][0];
 
        T const det_inv =
            T(1) / (m[0][0] * (m[1][1] * A2323 - m[1][2] * A1323 + m[1][3] * A1223) -
                    m[0][1] * (m[1][0] * A2323 - m[1][2] * A0323 + m[1][3] * A0223) +
                    m[0][2] * (m[1][0] * A1323 - m[1][1] * A0323 + m[1][3] * A0123) -
                    m[0][3] * (m[1][0] * A1223 - m[1][1] * A0223 + m[1][2] * A0123));
 
        M result{};
        result[0][0] = (m[1][1] * A2323 - m[1][2] * A1323 + m[1][3] * A1223) * det_inv;
        result[0][1] = -(m[0][1] * A2323 - m[0][2] * A1323 + m[0][3] * A1223) * det_inv;
        result[0][2] = (m[0][1] * A2313 - m[0][2] * A1313 + m[0][3] * A1213) * det_inv;
        result[0][3] = -(m[0][1] * A2312 - m[0][2] * A1312 + m[0][3] * A1212) * det_inv;
        result[1][0] = -(m[1][0] * A2323 - m[1][2] * A0323 + m[1][3] * A0223) * det_inv;
        result[1][1] = (m[0][0] * A2323 - m[0][2] * A0323 + m[0][3] * A0223) * det_inv;
        result[1][2] = -(m[0][0] * A2313 - m[0][2] * A0313 + m[0][3] * A0213) * det_inv;
        result[1][3] = (m[0][0] * A2312 - m[0][2] * A0312 + m[0][3] * A0212) * det_inv;
        result[2][0] = (m[1][0] * A1323 - m[1][1] * A0323 + m[1][3] * A0123) * det_inv;
        result[2][1] = -(m[0][0] * A1323 - m[0][1] * A0323 + m[0][3] * A0123) * det_inv;
        result[2][2] = (m[0][0] * A1313 - m[0][1] * A0313 + m[0][3] * A0113) * det_inv;
        result[2][3] = -(m[0][0] * A1312 - m[0][1] * A0312 + m[0][3] * A0112) * det_inv;
        result[3][0] = -(m[1][0] * A1223 - m[1][1] * A0223 + m[1][2] * A0123) * det_inv;
        result[3][1] = (m[0][0] * A1223 - m[0][1] * A0223 + m[0][2] * A0123) * det_inv;
        result[3][2] = -(m[0][0] * A1213 - m[0][1] * A0213 + m[0][2] * A0113) * det_inv;
        result[3][3] = (m[0][0] * A1212 - m[0][1] * A0212 + m[0][2] * A0112) * det_inv;
        return result;
    } else {
        []<bool flag = false>() {
            static_assert(flag, "inverse only implemented for 2x2, 3x3, and 4x4 matrices");
        }();
    }
}
 
template <std::size_t Rows, std::size_t Cols, class T>
[[nodiscard]] constexpr Mat<Cols, Rows, T> transpose(Mat<Rows, Cols, T> const& m) noexcept
{
    Mat<Cols, Rows, T> result{};
    for (std::size_t r = 0; r < Rows; ++r) {
        for (std::size_t c = 0; c < Cols; ++c) {
            result.fields[c][r] = m.fields[r][c];
        }
    }
    return result;
}
 
// Convenient factory functions
template <std::size_t R, std::size_t C, class T = float>
[[nodiscard]] constexpr Mat<R, C, T> zeros() noexcept
{
    return Mat<R, C, T>::zeros();
}
 
template <std::size_t N, class T = float>
[[nodiscard]] constexpr Mat<N, N, T> identity() noexcept
{
    return Mat<N, N, T>::identity();
}
 
template <std::size_t R, std::size_t C, class T = float>
[[nodiscard]] constexpr Mat<R, C, T> ones() noexcept
{
    return Mat<R, C, T>::ones();
}
 
/**************************************************************************************
|                                                                                     |
|                           Symmetric 3×3 eigen decomposition                        |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
[[nodiscard]] constexpr Vec<3, T> eigenValues(Mat<3, 3, T> const& m) noexcept
{
    T const a = m[0][0], d = m[0][1], f = m[0][2];
    T const b = m[1][1], e = m[1][2];
    T const c = m[2][2];
 
    T const x_1 =
        a * a + b * b + c * c - a * b - a * c - b * c + T(3) * (d * d + f * f + e * e);
 
    T const x_2 = -(T(2) * a - b - c) * (T(2) * b - a - c) * (T(2) * c - a - b) +
                  T(9) * ((T(2) * c - a - b) * (d * d) + (T(2) * b - a - c) * (f * f) +
                          (T(2) * a - b - c) * (e * e)) -
                  T(54) * (d * e * f);
 
    T const phi =
        T(0) < x_2
            ? std::atan(std::sqrt(T(4) * x_1 * x_1 * x_1 - x_2 * x_2) / x_2)
            : (T(0) > x_2 ? std::atan(std::sqrt(T(4) * x_1 * x_1 * x_1 - x_2 * x_2) / x_2) +
                                std::numbers::pi_v<T>
                          : std::numbers::pi_v<T> / T(2));
 
    return Vec<3, T>(
        (a + b + c - T(2) * std::sqrt(x_1) * std::cos(phi / T(3))) / T(3),
        (a + b + c +
         T(2) * std::sqrt(x_1) * std::cos((phi + std::numbers::pi_v<T>) / T(3))) /
            T(3),
        (a + b + c +
         T(2) * std::sqrt(x_1) * std::cos((phi - std::numbers::pi_v<T>) / T(3))) /
            T(3));
}
 
template <std::floating_point T>
[[nodiscard]] constexpr std::array<Vec<3, T>, 3> eigenVectors(
    Mat<3, 3, T> const& m) noexcept
{
    return eigenVectors(m, eigenValues(m));
}
 
template <std::floating_point T>
[[nodiscard]] constexpr std::array<Vec<3, T>, 3> eigenVectors(
    Mat<3, 3, T> const& m, Vec<3, T> const& eigen_values) noexcept
{
    assert(eigenValues(m) == eigen_values);
 
    T const d = m[0][1];
    T const f = m[0][2] == T(0) ? std::numeric_limits<T>::epsilon() : m[0][2];
    T const b = m[1][1], e = m[1][2];
    T const c = m[2][2];
 
    T const l_1 = eigen_values[0];
    T const l_2 = eigen_values[1];
    T const l_3 = eigen_values[2];
 
    T const m_1 = (d * (c - l_1) - e * f) / (f * (b - l_1) - d * e);
    T const m_2 = (d * (c - l_2) - e * f) / (f * (b - l_2) - d * e);
    T const m_3 = (d * (c - l_3) - e * f) / (f * (b - l_3) - d * e);
 
    return {normalize(Vec<3, T>((l_1 - c - e * m_1) / f, m_1, T(1))),
            normalize(Vec<3, T>((l_2 - c - e * m_2) / f, m_2, T(1))),
            normalize(Vec<3, T>((l_3 - c - e * m_3) / f, m_3, T(1)))};
}
 
/**************************************************************************************
|                                                                                     |
|                              Projection / view matrices                             |
|                                                                                     |
**************************************************************************************/
 
template <std::floating_point T>
[[nodiscard]] Mat<4, 4, T> orthogonal(T left, T right, T bottom, T top)
{
    Mat<4, 4, T> m{};
    m[0][0] = T(2) / (right - left);
    m[1][1] = T(2) / (top - bottom);
    m[2][2] = -T(1);
    m[0][3] = -(right + left) / (right - left);
    m[1][3] = -(top + bottom) / (top - bottom);
    m[3][3] = T(1);
    return m;
}
 
template <std::floating_point T>
[[nodiscard]] Mat<4, 4, T> orthogonal(T left, T right, T bottom, T top, T zNear, T zFar)
{
    Mat<4, 4, T> m{};
    m[3][3] = T(1);
 
    m[0][0] = T(2) / (right - left);
    m[1][1] = T(2) / (top - bottom);
    m[2][2] = T(1) / (zFar - zNear);
    m[0][3] = -(right + left) / (right - left);
    m[1][3] = -(top + bottom) / (top - bottom);
    m[2][3] = -zNear / (zFar - zNear);
 
    return m;
}
 
template <std::floating_point T>
[[nodiscard]] Mat<4, 4, T> perspective(T fovy, T aspect, T near, T far)
{
    Mat<4, 4, T> m{};
 
    T const tan_half_fovy = std::tan(fovy / T(2));
 
    m[0][0] = T(1) / (aspect * tan_half_fovy);
    m[1][1] = T(1) / tan_half_fovy;
    m[2][2] = far / (far - near);
    m[3][2] = T(1);
    m[2][3] = -(far * near) / (far - near);
 
    return m;
}
 
template <std::floating_point T>
[[nodiscard]] Mat<4, 4, T> perspective(T fx, T fy, T cx, T cy, T width, T height, T near,
                                       T far)
{
    Mat<4, 4, T> m{};
 
    m[0][0] = T(2) * fx / width;
    m[1][1] = T(2) * fy / height;
    m[0][2] = (T(2) * cx / width) - T(1);
    m[1][2] = (T(2) * cy / height) - T(1);
    m[2][2] = far / (far - near);
    m[3][2] = T(1);
    m[2][3] = -(far * near) / (far - near);
 
    return m;
}
 
template <std::floating_point T>
[[nodiscard]] Mat<4, 4, T> lookAt(Vec<3, T> const& eye, Vec<3, T> const& target,
                                  Vec<3, T> const& up)
{
    Mat<4, 4, T> m{};
 
    Vec<3, T> const f(normalize(target - eye));
    Vec<3, T> const s(normalize(cross(f, up)));
    Vec<3, T> const u(cross(s, f));
 
    m[0][0] = s[0];
    m[0][1] = s[1];
    m[0][2] = s[2];
    m[1][0] = -u[0];
    m[1][1] = -u[1];
    m[1][2] = -u[2];
    m[2][0] = f[0];
    m[2][1] = f[1];
    m[2][2] = f[2];
    m[0][3] = -dot(s, eye);
    m[1][3] = dot(u, eye);
    m[2][3] = -dot(f, eye);
    m[3][3] = T(1);
 
    return m;
}
 
template <std::floating_point T>
[[nodiscard]] Mat<4, 4, T> infinitePerspective(T fovy, T aspect, T near)
{
    Mat<4, 4, T> m{};
    T const      tan_half_fovy = std::tan(fovy / T(2));
 
    m[0][0] = T(1) / (aspect * tan_half_fovy);
    m[1][1] = T(1) / tan_half_fovy;
 
    m[2][2] = T(1);
    m[3][2] = T(1);
    m[2][3] = -near;
 
    return m;
}
 
template <std::floating_point T>
[[nodiscard]] Mat<4, 4, T> rotate(Mat<4, 4, T> const& m, T angle, Vec<3, T> const& v)
{
    T const c = std::cos(angle);
    T const s = std::sin(angle);
 
    Vec<3, T> const axis(normalize(v));
    Vec<3, T> const temp((T(1) - c) * axis);
 
    // Rotation matrix R in row-major: rot[row][col] = R_{row,col}
    Mat<3, 3, T> rot{};
    rot[0][0] = c + temp[0] * axis[0];
    rot[0][1] = temp[1] * axis[0] - s * axis[2];
    rot[0][2] = temp[2] * axis[0] + s * axis[1];
    rot[1][0] = temp[0] * axis[1] + s * axis[2];
    rot[1][1] = c + temp[1] * axis[1];
    rot[1][2] = temp[2] * axis[1] - s * axis[0];
    rot[2][0] = temp[0] * axis[2] - s * axis[1];
    rot[2][1] = temp[1] * axis[2] + s * axis[0];
    rot[2][2] = c + temp[2] * axis[2];
 
    // Compute M * R for upper-left 3x3, preserving last row and column
    Mat<4, 4, T> result{};
    for (std::size_t i = 0; i < 3; ++i) {
        for (std::size_t j = 0; j < 3; ++j) {
            for (std::size_t k = 0; k < 3; ++k) {
                result[i][j] += m[i][k] * rot[k][j];
            }
        }
        result[i][3] = m[i][3];
    }
    result[3] = m[3];
    return result;
}
 
/**************************************************************************************
|                                                                                     |
|                                 Structured bindings                                 |
|                                                                                     |
**************************************************************************************/
 
template <std::size_t I, std::size_t Rows, std::size_t Cols, class T>
  requires(I < Rows)
[[nodiscard]] constexpr Vec<Cols, T>& get(Mat<Rows, Cols, T>& m) noexcept
{
    return m.fields[I];
}
 
template <std::size_t I, std::size_t Rows, std::size_t Cols, class T>
  requires(I < Rows)
[[nodiscard]] constexpr Vec<Cols, T> const& get(Mat<Rows, Cols, T> const& m) noexcept
{
    return m.fields[I];
}
 
template <std::size_t I, std::size_t Rows, std::size_t Cols, class T>
  requires(I < Rows)
[[nodiscard]] constexpr Vec<Cols, T>&& get(Mat<Rows, Cols, T>&& m) noexcept
{
    return std::move(m.fields[I]);
}
 
/**************************************************************************************
|                                                                                     |
|                                       Print                                         |
|                                                                                     |
**************************************************************************************/
 
template <std::size_t Rows, std::size_t Cols, class T>
std::ostream& operator<<(std::ostream& out, Mat<Rows, Cols, T> const& m)
{
    for (std::size_t r = 0; r < Rows; ++r) {
        if (r > 0) out << '\n';
        out << m.fields[r];
    }
    return out;
}
 
}  // namespace ufo
 
template <std::size_t Rows, std::size_t Cols, class T>
struct std::tuple_size<ufo::Mat<Rows, Cols, T>>
    : std::integral_constant<std::size_t, Rows> {
};
 
template <std::size_t I, std::size_t Rows, std::size_t Cols, class T>
struct std::tuple_element<I, ufo::Mat<Rows, Cols, T>> {
    using type = ufo::Vec<Cols, T>;
};
 
template <std::size_t Rows, std::size_t Cols, std::formattable<char> T>
struct std::formatter<ufo::Mat<Rows, Cols, T>> {
    constexpr auto parse(std::format_parse_context& ctx) { return ctx.begin(); }
 
    auto format(ufo::Mat<Rows, Cols, T> const& m, std::format_context& ctx) const
    {
        auto out = ctx.out();
        for (std::size_t r = 0; r < Rows; ++r) {
            if (r > 0) out = std::format_to(out, "\n");
            out = std::format_to(out, "{}", m.fields[r]);
        }
        return out;
    }
};
 
#endif  // UFO_NUMERIC_MAT_HPP