qr.h

Go to the documentation of this file.

#ifndef FUSED_ALGORITHMS_QR_H
#define FUSED_ALGORITHMS_QR_H
 
#include "config.h"
#include "fused/fused_matrix.h"
#include "fused/fused_vector.h"
#include "math/math_utils.h" // math::sqrt, math::abs
 
namespace matrix_algorithms
{
 
    // ========================================================================
    // QR Result
    // ========================================================================
 
    template <typename T, my_size_t M, my_size_t N>
    struct QRResult
    {
        FusedMatrix<T, M, N> QR; 
        FusedVector<T, N> tau;   
 
        FusedMatrix<T, M, M> Q() const
        {
            FusedMatrix<T, M, M> result(T(0));
            result.setIdentity();
 
            for (my_size_t jj = N; jj-- > 0;)
            {
                T tj = tau(jj);
 
                if (tj == T(0))
                    continue;
 
                // Apply Hⱼ to result from the left: result = Hⱼ · result
                // v = [0..0, 1, QR(jj+1,jj), ..., QR(M-1,jj)]
                for (my_size_t k = 0; k < M; ++k)
                {
                    // dot = vᵀ · result(:, k)
                    T dot = result(jj, k); // v(jj) = 1 (implicit)
 
                    for (my_size_t i = jj + 1; i < M; ++i)
                    {
                        dot += QR(i, jj) * result(i, k);
                    }
 
                    // result(:, k) -= τ · v · dot
                    result(jj, k) -= tj * dot;
 
                    for (my_size_t i = jj + 1; i < M; ++i)
                    {
                        result(i, k) -= tj * QR(i, jj) * dot;
                    }
                }
            }
 
            return result;
        }
 
        FusedMatrix<T, M, N> R() const
        {
            FusedMatrix<T, M, N> result(T(0));
 
            for (my_size_t i = 0; i < M; ++i)
            {
                for (my_size_t j = i; j < N; ++j)
                {
                    result(i, j) = QR(i, j);
                }
            }
 
            return result;
        }
 
        FusedVector<T, M> apply_Qt(const FusedVector<T, M> &b) const
        {
            FusedVector<T, M> result = b;
 
            for (my_size_t j = 0; j < N; ++j)
            {
                T tj = tau(j);
 
                if (tj == T(0))
                    continue;
 
                // dot = vᵀ · result
                T dot = result(j); // v(j) = 1 (implicit)
 
                for (my_size_t i = j + 1; i < M; ++i)
                {
                    dot += QR(i, j) * result(i);
                }
 
                // result -= τ · v · dot
                result(j) -= tj * dot;
 
                for (my_size_t i = j + 1; i < M; ++i)
                {
                    result(i) -= tj * QR(i, j) * dot;
                }
            }
 
            return result;
        }
    };
 
    // ========================================================================
    // Householder QR Decomposition
    // ========================================================================
 
    template <typename T, my_size_t M, my_size_t N>
    QRResult<T, M, N> qr_householder(const FusedMatrix<T, M, N> &A)
    {
        static_assert(is_floating_point_v<T>,
                      "qr_householder requires a floating-point scalar type");
        static_assert(M >= N, "qr_householder requires M >= N");
 
        QRResult<T, M, N> result;
        result.QR = A;
        result.tau = FusedVector<T, N>(T(0));
 
        for (my_size_t j = 0; j < N; ++j)
        {
            // 1. Compute norm of sub-column QR(j:M-1, j)
            T norm_sq = T(0);
 
            for (my_size_t i = j; i < M; ++i)
            {
                norm_sq += result.QR(i, j) * result.QR(i, j);
            }
 
            T norm = math::sqrt(norm_sq);
 
            if (norm <= T(PRECISION_TOLERANCE))
            {
                result.tau(j) = T(0);
                continue;
            }
 
            // 2. Choose α = -sign(QR(j,j)) · norm
            T alpha = (result.QR(j, j) >= T(0)) ? -norm : norm;
 
            // 3. Form Householder vector v
            // v(j) = QR(j,j) - α,  v(i) = QR(i,j) for i > j
            T v0 = result.QR(j, j) - alpha;
 
            // 4. Compute τ and normalize v
            // vᵀv = v0² + Σ QR(i,j)² for i > j
            T vtv = v0 * v0;
 
            for (my_size_t i = j + 1; i < M; ++i)
            {
                vtv += result.QR(i, j) * result.QR(i, j);
            }
 
            T tj = T(2) * v0 * v0 / vtv;
            result.tau(j) = tj;
 
            // Normalize: store v(i)/v(0) below diagonal
            for (my_size_t i = j + 1; i < M; ++i)
            {
                result.QR(i, j) /= v0;
            }
 
            // 5. Apply reflection to remaining columns k = j+1..N-1
            // col_k -= τ · v_norm · (v_normᵀ · col_k)
            // v_norm(j) = 1, v_norm(i) = QR(i,j) for i > j
            for (my_size_t k = j + 1; k < N; ++k)
            {
                T dot = result.QR(j, k); // v_norm(j) = 1
 
                for (my_size_t i = j + 1; i < M; ++i)
                {
                    dot += result.QR(i, j) * result.QR(i, k);
                }
 
                result.QR(j, k) -= tj * dot;
 
                for (my_size_t i = j + 1; i < M; ++i)
                {
                    result.QR(i, k) -= tj * result.QR(i, j) * dot;
                }
            }
 
            // 6. Store R(j,j) = α
            result.QR(j, j) = alpha;
        }
 
        return result;
    }
 
} // namespace matrix_algorithms
 
#endif // FUSED_ALGORITHMS_QR_H