lu.h

Go to the documentation of this file.

#ifndef FUSED_ALGORITHMS_LU_H
#define FUSED_ALGORITHMS_LU_H
 
#include "config.h"
#include "utilities/expected.h"
#include "matrix_traits.h"
#include "fused/fused_matrix.h"
#include "fused/fused_vector.h"
#include "math/math_utils.h"
 
namespace matrix_algorithms
{
 
    using matrix_traits::MatrixStatus;
 
    // ========================================================================
    // LU Result
    // ========================================================================
 
    template <typename T, my_size_t N>
    struct LUResult
    {
        FusedMatrix<T, N, N> LU;        
        FusedVector<my_size_t, N> perm; 
        int sign;                       
 
        FusedMatrix<T, N, N> L() const
        {
            FusedMatrix<T, N, N> result(T(0));
 
            for (my_size_t i = 0; i < N; ++i)
            {
                result(i, i) = T(1); // unit diagonal
 
                for (my_size_t j = 0; j < i; ++j)
                {
                    result(i, j) = LU(i, j);
                }
            }
 
            return result;
        }
 
        FusedMatrix<T, N, N> U() const
        {
            FusedMatrix<T, N, N> result(T(0));
 
            for (my_size_t i = 0; i < N; ++i)
            {
                for (my_size_t j = i; j < N; ++j)
                {
                    result(i, j) = LU(i, j);
                }
            }
 
            return result;
        }
    };
 
    // ========================================================================
    // LU Decomposition
    // ========================================================================
 
    template <typename T, my_size_t N>
    Expected<LUResult<T, N>, MatrixStatus> lu(
        const FusedMatrix<T, N, N> &A,
        T tol = T(PRECISION_TOLERANCE))
    {
        static_assert(is_floating_point_v<T>,
                      "lu requires a floating-point scalar type");
 
        LUResult<T, N> result;
        result.LU = A; // work on a copy
        result.sign = 1;
 
        // Initialize permutation to identity
        for (my_size_t i = 0; i < N; ++i)
        {
            result.perm(i) = i;
        }
 
        for (my_size_t j = 0; j < N; ++j)
        {
            // 1. Find pivot: row with max |A(p,j)| for p >= j
            my_size_t pivot = j;
            T max_val = math::abs(result.LU(j, j));
 
            for (my_size_t p = j + 1; p < N; ++p)
            {
                T val = math::abs(result.LU(p, j));
 
                if (val > max_val)
                {
                    max_val = val;
                    pivot = p;
                }
            }
 
            // 2. Swap rows j and pivot
            if (pivot != j)
            {
                // Swap entire rows in LU
                for (my_size_t k = 0; k < N; ++k)
                {
                    T tmp = result.LU(j, k);
                    result.LU(j, k) = result.LU(pivot, k);
                    result.LU(pivot, k) = tmp;
                }
 
                // Swap permutation entries
                my_size_t tmp_perm = result.perm(j);
                result.perm(j) = result.perm(pivot);
                result.perm(pivot) = tmp_perm;
 
                result.sign = -result.sign;
            }
 
            // 3. Check for singularity
            T diag = result.LU(j, j);
 
            if (math::abs(diag) <= tol)
            {
                return Unexpected{MatrixStatus::Singular};
            }
 
            // 4. Eliminate below pivot
            for (my_size_t i = j + 1; i < N; ++i)
            {
                T factor = result.LU(i, j) / diag;
                result.LU(i, j) = factor; // store L factor
 
                for (my_size_t k = j + 1; k < N; ++k)
                {
                    result.LU(i, k) -= factor * result.LU(j, k);
                }
            }
        }
 
        return move(result);
    }
 
    template <typename T, my_size_t N>
    LUResult<T, N> lu_or_die(const FusedMatrix<T, N, N> &A)
    {
        auto result = lu(A);
 
        if (!result.has_value())
        {
            MyErrorHandler::error("LU decomposition failed");
        }
 
        return move(result.value());
    }
 
} // namespace matrix_algorithms
 
#endif // FUSED_ALGORITHMS_LU_H