qr_givens.h

Go to the documentation of this file.

#ifndef FUSED_ALGORITHMS_QR_GIVENS_H
#define FUSED_ALGORITHMS_QR_GIVENS_H
 
#include "config.h"
#include "fused/fused_matrix.h"
#include "fused/fused_vector.h"
#include "math/math_utils.h"
 
namespace matrix_algorithms
{
 
    template <typename T, my_size_t M, my_size_t N>
    struct GivensQRResult
    {
        FusedMatrix<T, M, M> Q; 
        FusedMatrix<T, M, N> R; 
    };
 
    template <typename T, my_size_t M, my_size_t N>
    GivensQRResult<T, M, N> qr_givens(const FusedMatrix<T, M, N> &A)
    {
        static_assert(is_floating_point_v<T>,
                      "qr_givens requires a floating-point scalar type");
        static_assert(M >= N, "qr_givens requires M >= N");
 
        GivensQRResult<T, M, N> result;
        result.R = A;
        result.Q = FusedMatrix<T, M, M>(T(0));
        result.Q.setIdentity();
 
        for (my_size_t j = 0; j < N; ++j)
        {
            // Zero elements below diagonal, bottom to top
            for (my_size_t i = M - 1; i > j; --i)
            {
                T a = result.R(i - 1, j);
                T b = result.R(i, j);
 
                // Skip if already zero
                if (math::abs(b) <= T(PRECISION_TOLERANCE))
                    continue;
 
                // Compute Givens rotation
                T r = math::sqrt(a * a + b * b);
                T c = a / r;
                T s = b / r;
 
                // Apply rotation to rows i-1 and i of R (columns j..N-1)
                for (my_size_t k = j; k < N; ++k)
                {
                    T r1 = result.R(i - 1, k);
                    T r2 = result.R(i, k);
                    result.R(i - 1, k) = c * r1 + s * r2;
                    result.R(i, k) = -s * r1 + c * r2;
                }
 
                // Accumulate rotation into Q (columns i-1 and i)
                for (my_size_t k = 0; k < M; ++k)
                {
                    T q1 = result.Q(k, i - 1);
                    T q2 = result.Q(k, i);
                    result.Q(k, i - 1) = c * q1 + s * q2;
                    result.Q(k, i) = -s * q1 + c * q2;
                }
            }
        }
 
        return result;
    }
 
} // namespace matrix_algorithms
 
#endif // FUSED_ALGORITHMS_QR_GIVENS_H