// Copyright (C) 2026 Kiyotsugu Arai // SPDX-License-Identifier: LGPL-3.0-or-later // decomposition.hpp // // Matrix decomposition algorithms // // This file implements various matrix decomposition algorithms. // It includes LU decomposition, Cholesky decomposition, QR decomposition, SVD, and more. // // Main features and complexity (n×n matrix): // - LU decomposition — with partial pivoting, O(n^3) // - Cholesky decomposition — symmetric positive definite matrices only, O(n^3/3) ≈ half of LU // - QR decomposition — Householder transformation, O(2n^3/3), used for least-squares problems // - SVD — Golub-Reinsch method, O(mn^2 + n^3) (when m≥n) // - LDL decomposition — symmetric matrices (need not be positive definite), a variant of Cholesky // // Related: // - eigenvalues.hpp — eigenvalue decomposition (SVD-based or QR iteration) // - solvers.hpp — linear solvers using LU/Cholesky/QR // // References: // Golub, Van Loan "Matrix Computations" 4th ed. Ch.2 (LU), Ch.4 (Cholesky/QR), Ch.6 (SVD) // Anderson et al. "LAPACK Users' Guide" 3rd ed. #ifndef SANGI_DECOMPOSITION_HPP #define SANGI_DECOMPOSITION_HPP #include #include #include #include #include #include #include #include #include #include #ifdef SANGI_SVD_PHASE_TIMING #include #include #endif #include #include #include #include #include #include #include namespace sangi { namespace algorithms { /** * @brief Check whether a matrix is symmetric * @tparam T element type * @param a matrix to check * @param tolerance tolerance * @return true if the matrix is symmetric, false otherwise */ template requires concepts::MatrixOf bool is_symmetric(const M& a, decltype(numeric_traits::epsilon()) tolerance = numeric_traits::epsilon() * 10) { using T = typename M::value_type; if (a.rows() != a.cols()) { return false; } for (std::size_t i = 0; i < a.rows(); ++i) { for (std::size_t j = i + 1; j < a.cols(); ++j) { if constexpr (numeric_traits::is_complex) { // Complex: Hermitian check a(i,j) == conj(a(j,i)) if (std::abs(a(i, j) - sangi::conj(a(j, i))) > tolerance) { return false; } } else { if (std::abs(a(i, j) - a(j, i)) > tolerance) { return false; } } } } return true; } /** * @brief Check whether a matrix is positive definite * @tparam T element type * @param a matrix to check * @return true if the matrix is positive definite (Hermitian positive definite for Complex) */ template requires sangi::BaseMatrixLike bool is_positive_definite(const MA& a) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); if (!is_symmetric(a)) { return false; } // Check that all diagonal elements are positive (necessary condition) for (typename Matrix::size_type i = 0; i < a.rows(); ++i) { if constexpr (numeric_traits::is_complex) { // Diagonal elements of a Hermitian matrix are real and should be positive if (a(i, i).real() <= 0) { return false; } } else { if (a(i, i) <= numeric_traits::zero()) { return false; } } } // Check whether Cholesky decomposition is possible (sufficient condition) try { cholesky_decomposition(a); return true; } catch (const MathError&) { return false; } } // Pivot selection strategy enum class PivotStrategy { None, // No pivot selection (use diagonal elements as-is) Partial, // Partial pivot selection (largest absolute value within column, smallest height for Rational) Full // Full pivot selection (select from the entire remaining submatrix) }; // ── Internal helper for pivot selection ── // Uses numeric_traits::pivotBetter to select with the optimal criterion for the type // (floating point: largest abs, Rational: smallest height) template struct PivotSelector { // Partial pivot: select the best from rows k..n-1 of column k static typename Matrix::size_type selectPartial(const Matrix& lu, typename Matrix::size_type k, typename Matrix::size_type n) { typename Matrix::size_type best = k; for (typename Matrix::size_type i = k + 1; i < n; ++i) { if (lu(i, k) == numeric_traits::zero()) continue; if (lu(best, k) == numeric_traits::zero() || numeric_traits::pivotBetter(lu(i, k), lu(best, k))) best = i; } return best; } // Full pivot: select the best from submatrix (k..n-1, k..n-1) static std::pair::size_type, typename Matrix::size_type> selectFull(const Matrix& lu, typename Matrix::size_type k, typename Matrix::size_type n) { typename Matrix::size_type bestR = k, bestC = k; for (typename Matrix::size_type i = k; i < n; ++i) { for (typename Matrix::size_type j = k; j < n; ++j) { if (lu(i, j) == numeric_traits::zero()) continue; if (lu(bestR, bestC) == numeric_traits::zero() || numeric_traits::pivotBetter(lu(i, j), lu(bestR, bestC))) { bestR = i; bestC = j; } } } return { bestR, bestC }; } }; /** * @brief Compute the LU decomposition * @tparam T element type * @param a matrix to decompose * @param pivot pivot strategy (default: Partial) * @return a pair of the LU matrix and pivot information * The LU matrix stores L (lower triangular part) and U (upper triangular part) in a single matrix * The pivot information is the record of row swaps * For full pivoting, column swaps are also recorded (n rows + n columns) */ template requires concepts::MatrixOf std::pair, std::vector> lu_decomposition(const M& a, PivotStrategy pivot = PivotStrategy::Partial) { using T = typename M::value_type; SANGI_STATIC_ASSERT_SQUARE(M); if (a.rows() != a.cols()) { assert(false && "DimensionError: lu_decomposition: matrix must be square"); throw DimensionError("lu_decomposition: matrix must be square"); } const auto n = a.rows(); #if SANGI_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { Matrix lu(n, n); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) lu(i, j) = a(i, j); std::vector ipiv(n); lapack_int info; if constexpr (std::is_same_v) info = LAPACKE_sgetrf(LAPACK_ROW_MAJOR, (lapack_int)n, (lapack_int)n, lu.data(), (lapack_int)n, ipiv.data()); else info = LAPACKE_dgetrf(LAPACK_ROW_MAJOR, (lapack_int)n, (lapack_int)n, lu.data(), (lapack_int)n, ipiv.data()); if (info > 0) throw MathError("lu_decomposition: singular matrix detected"); // Convert LAPACKE ipiv from 1-based → 0-based std::vector::size_type> pivots(n); for (std::size_t i = 0; i < n; ++i) pivots[i] = static_cast::size_type>(ipiv[i] - 1); return { lu, pivots }; } #endif Matrix lu(n, n); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) lu(i, j) = a(i, j); std::vector pivots(n); // Block LU decomposition (LAPACK DGETRF style) — for float/double with n >= 64 if constexpr (std::is_same_v || std::is_same_v) { constexpr std::size_t NB = 64; if (n >= NB) { T* __restrict data = lu.data(); // L21 sign-flipped buffer (reused across the whole loop) Matrix L21_buf(n, NB); for (std::size_t jb = 0; jb < n; jb += NB) { const std::size_t jend = (std::min)(jb + NB, n); const std::size_t nb = jend - jb; // Phase 1: panel factorization (columns jb..jend-1) for (std::size_t j = jb; j < jend; ++j) { // Partial pivot selection std::size_t pivot_row = j; T pivot_val = std::abs(data[j * n + j]); for (std::size_t i = j + 1; i < n; ++i) { T av = std::abs(data[i * n + j]); if (av > pivot_val) { pivot_val = av; pivot_row = i; } } pivots[j] = pivot_row; // Row swap (all rows) if (pivot_row != j) { T* row_j = data + j * n; T* row_p = data + pivot_row * n; for (std::size_t c = 0; c < n; ++c) std::swap(row_j[c], row_p[c]); } // Singularity check if (std::abs(data[j * n + j]) <= std::numeric_limits::epsilon()) throw MathError("lu_decomposition: singular matrix detected"); // L column scaling T inv_diag = T(1) / data[j * n + j]; for (std::size_t i = j + 1; i < n; ++i) data[i * n + j] *= inv_diag; // In-panel rank-1 update (columns j+1..jend-1 only, SIMD axpy) { const std::size_t panel_rem = jend - j - 1; if (panel_rem > 0) { for (std::size_t i = j + 1; i < n; ++i) { T neg_lij = -data[i * n + j]; computation::simd::axpy_simd( &data[i * n + j + 1], neg_lij, &data[j * n + j + 1], panel_rem); } } } } if (jend >= n) break; const std::size_t remaining = n - jend; // Phase 2: TRSM — solve L11 * U12 = A12 by forward substitution // L11 is the unit lower triangular part of lu(jb..jend-1, jb..jend-1) // A12/U12 is lu(jb..jend-1, jend..n-1) — overwritten in-place for (std::size_t r = 1; r < nb; ++r) { for (std::size_t t = 0; t < r; ++t) { T neg_lrt = -data[(jb + r) * n + (jb + t)]; computation::simd::axpy_simd( &data[(jb + r) * n + jend], neg_lrt, &data[(jb + t) * n + jend], remaining); } } // Phase 3: trailing matrix update A22 -= L21 * U12 // Sign-flip L21 and copy into the buffer; U12 is referenced directly { L21_buf.resize(remaining, nb); for (std::size_t i = 0; i < remaining; ++i) { const T* __restrict src = &data[(jend + i) * n + jb]; T* __restrict dst = &L21_buf(i, 0); for (std::size_t j = 0; j < nb; ++j) dst[j] = -src[j]; } // gemm: A22 += (-L21) * U12 — stride-based direct accumulation DefaultComputePolicy::gemm( L21_buf.data(), &data[jb * n + jend], &data[jend * n + jend], remaining, remaining, nb, nb, n, n); } } return { lu, pivots }; } } // Scalar LU decomposition (small matrices or non-floating-point types) // For full pivoting, column swaps are also recorded: pivots[0..n-1] = rows, pivots[n..2n-1] = columns std::vector::size_type> col_pivots; if (pivot == PivotStrategy::Full) { col_pivots.resize(n); for (typename Matrix::size_type i = 0; i < n; ++i) col_pivots[i] = i; } for (typename Matrix::size_type k = 0; k < n; ++k) { // Pivot selection typename Matrix::size_type pivot_row = k; typename Matrix::size_type pivot_col = k; if (pivot == PivotStrategy::Partial) { pivot_row = PivotSelector::selectPartial(lu, k, n); } else if (pivot == PivotStrategy::Full) { auto [pr, pc] = PivotSelector::selectFull(lu, k, n); pivot_row = pr; pivot_col = pc; } // PivotStrategy::None: pivot_row = k, pivot_col = k (as-is) pivots[k] = pivot_row; // Row swap if (pivot_row != k) { for (typename Matrix::size_type j = 0; j < n; ++j) std::swap(lu(k, j), lu(pivot_row, j)); } // Column swap (full pivoting only) if (pivot == PivotStrategy::Full && pivot_col != k) { for (typename Matrix::size_type i = 0; i < n; ++i) std::swap(lu(i, k), lu(i, pivot_col)); std::swap(col_pivots[k], col_pivots[pivot_col]); } if (numeric_traits::abs(lu(k, k)) <= numeric_traits::epsilon()) { throw MathError("lu_decomposition: singular matrix detected"); } for (typename Matrix::size_type i = k + 1; i < n; ++i) { const T factor = lu(i, k) / lu(k, k); lu(i, k) = factor; for (typename Matrix::size_type j = k + 1; j < n; ++j) { lu(i, j) -= factor * lu(k, j); } } } // Full pivoting: append column swap information to the second half of pivots if (pivot == PivotStrategy::Full) { pivots.resize(2 * n); for (typename Matrix::size_type i = 0; i < n; ++i) pivots[n + i] = col_pivots[i]; } return { lu, pivots }; } /** * @brief Solve a system of linear equations using LU decomposition * @tparam T element type * @param a coefficient matrix * @param b right-hand-side vector * @param pivot pivot strategy (default: Partial) * @return solution vector */ template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> lu_solve(const MA& a, const VB& b, PivotStrategy pivot = PivotStrategy::Partial) { using T = sangi::element_t; // Compile-time dimension checks fire only when both operands // carry static dimensions (StaticMatrix / StaticVector). SANGI_STATIC_ASSERT_SQUARE(MA); SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); if (a.rows() != a.cols()) { assert(false && "DimensionError: lu_solve: matrix must be square"); throw DimensionError("lu_solve: matrix must be square"); } if (a.rows() != b.size()) { assert(false && "DimensionError: lu_solve: matrix and vector dimensions mismatch"); throw DimensionError("lu_solve: matrix and vector dimensions mismatch"); } const auto n = a.rows(); #if SANGI_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { Matrix Acopy = a; Vector x = b; std::vector ipiv(n); lapack_int info; if constexpr (std::is_same_v) info = LAPACKE_sgesv(LAPACK_ROW_MAJOR, (lapack_int)n, 1, Acopy.data(), (lapack_int)n, ipiv.data(), x.data(), 1); else info = LAPACKE_dgesv(LAPACK_ROW_MAJOR, (lapack_int)n, 1, Acopy.data(), (lapack_int)n, ipiv.data(), x.data(), 1); if (info > 0) throw MathError("lu_solve: singular matrix detected"); return x; } #endif // LU decomposition auto [lu, pivots] = lu_decomposition(a, pivot); const bool fullPivot = (pivot == PivotStrategy::Full); // Prepare the right-hand-side vector Vector x = b; // Row pivot permutation for (std::size_t i = 0; i < n; ++i) { if (pivots[i] != i) { std::swap(x[i], x[pivots[i]]); } } // float/double: forward/backward substitution with raw pointers if constexpr (std::is_same_v || std::is_same_v) { const T* __restrict ld = lu.data(); T* __restrict xd = x.data(); // Forward substitution (Ly = b) for (std::size_t i = 1; i < n; ++i) { const T* __restrict row = ld + i * n; T sum = xd[i]; for (std::size_t j = 0; j < i; ++j) sum -= row[j] * xd[j]; xd[i] = sum; } // Backward substitution (Ux = y) for (std::size_t ii = 0; ii < n; ++ii) { std::size_t i = n - 1 - ii; const T* __restrict row = ld + i * n; T sum = xd[i]; for (std::size_t j = i + 1; j < n; ++j) sum -= row[j] * xd[j]; xd[i] = sum / row[i]; } // Full pivoting: inverse permutation of column swaps if (fullPivot) { Vector tmp = x; for (std::size_t i = 0; i < n; ++i) x[pivots[n + i]] = tmp[i]; } return x; } // Generic type: forward substitution (Ly = b) for (std::size_t i = 1; i < n; ++i) { for (std::size_t j = 0; j < i; ++j) { x[i] -= lu(i, j) * x[j]; } } // Backward substitution (Ux = y) for (std::size_t i = n; i-- > 0; ) { for (std::size_t j = i + 1; j < n; ++j) { x[i] -= lu(i, j) * x[j]; } x[i] /= lu(i, i); } // Full pivoting: inverse permutation of column swaps if (fullPivot) { Vector tmp = x; for (std::size_t i = 0; i < n; ++i) x[pivots[n + i]] = tmp[i]; } return x; } /** * @brief Solve a system of linear equations by Gaussian elimination (augmented-matrix based) * * Without going through LU decomposition, directly forward-eliminate + back-substitute the augmented matrix [A|b]. * For Rational, the smallest-height pivot suppresses coefficient growth. * * @tparam T element type (double, Float, Rational, etc.) * @param a coefficient matrix (n×n) * @param b right-hand-side vector (n) * @param pivot pivot strategy (default: Partial) * @return solution vector */ template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> gaussian_elimination(const MA& a, const VB& b, PivotStrategy pivot = PivotStrategy::Partial) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); if (a.rows() != a.cols()) { assert(false && "DimensionError: gaussian_elimination: matrix must be square"); throw DimensionError("gaussian_elimination: matrix must be square"); } if (a.rows() != b.size()) { assert(false && "DimensionError: gaussian_elimination: matrix and vector dimensions mismatch"); throw DimensionError("gaussian_elimination: matrix and vector dimensions mismatch"); } const auto n = a.rows(); // Build the augmented matrix [A|b] (n × n+1) Matrix aug(n, n + 1); for (std::size_t i = 0; i < n; ++i) { for (std::size_t j = 0; j < n; ++j) aug(i, j) = a(i, j); aug(i, n) = b[i]; } // Track column order for full pivoting std::vector colOrder(n); for (std::size_t i = 0; i < n; ++i) colOrder[i] = i; // Forward elimination for (std::size_t k = 0; k < n; ++k) { // Pivot selection std::size_t pivotRow = k, pivotCol = k; if (pivot == PivotStrategy::Partial) { // Partial pivot: select from rows k..n-1 of column k #ifdef SANGI_RATIONAL_HPP if constexpr (std::is_same_v) { std::size_t best = n; Int bestH; for (std::size_t i = k; i < n; ++i) { if (aug(i, k).isZero()) continue; Int h = height(aug(i, k)); if (best == n || h < bestH) { bestH = h; best = i; } } if (best != n) pivotRow = best; } else #endif { T bestVal = numeric_traits::abs(aug(k, k)); for (std::size_t i = k + 1; i < n; ++i) { T v = numeric_traits::abs(aug(i, k)); if (v > bestVal) { bestVal = v; pivotRow = i; } } } } else if (pivot == PivotStrategy::Full) { // Full pivot: select from submatrix (k..n-1, k..n-1) #ifdef SANGI_RATIONAL_HPP if constexpr (std::is_same_v) { std::size_t bestR = n, bestC = k; Int bestH; for (std::size_t i = k; i < n; ++i) { for (std::size_t j = k; j < n; ++j) { if (aug(i, j).isZero()) continue; Int h = height(aug(i, j)); if (bestR == n || h < bestH) { bestH = h; bestR = i; bestC = j; } } } if (bestR != n) { pivotRow = bestR; pivotCol = bestC; } } else #endif { T bestVal = numeric_traits::abs(aug(k, k)); for (std::size_t i = k; i < n; ++i) { for (std::size_t j = k; j < n; ++j) { T v = numeric_traits::abs(aug(i, j)); if (v > bestVal) { bestVal = v; pivotRow = i; pivotCol = j; } } } } } // Row swap if (pivotRow != k) { for (std::size_t j = 0; j <= n; ++j) std::swap(aug(k, j), aug(pivotRow, j)); } // Column swap (full pivoting only; the b column is not swapped) if (pivot == PivotStrategy::Full && pivotCol != k) { for (std::size_t i = 0; i < n; ++i) std::swap(aug(i, k), aug(i, pivotCol)); std::swap(colOrder[k], colOrder[pivotCol]); } // Singularity check if (numeric_traits::abs(aug(k, k)) <= numeric_traits::epsilon()) throw MathError("gaussian_elimination: singular matrix detected"); // Elimination for (std::size_t i = k + 1; i < n; ++i) { T factor = aug(i, k) / aug(k, k); for (std::size_t j = k + 1; j <= n; ++j) aug(i, j) -= factor * aug(k, j); aug(i, k) = T(0); } } // Backward substitution Vector y(n); for (std::size_t i = n; i-- > 0; ) { y[i] = aug(i, n); for (std::size_t j = i + 1; j < n; ++j) y[i] -= aug(i, j) * y[j]; y[i] /= aug(i, i); } // Full pivoting: inverse permutation of column swaps if (pivot == PivotStrategy::Full) { Vector x(n); for (std::size_t i = 0; i < n; ++i) x[colOrder[i]] = y[i]; return x; } return y; } /** * @brief Compute the inverse matrix using LU decomposition * @tparam T element type * @param a original matrix * @param pivot pivot strategy (default: Partial) * @return inverse matrix */ template requires sangi::BaseMatrixLike Matrix> lu_inverse(const MA& a, PivotStrategy pivot = PivotStrategy::Partial) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); if (a.rows() != a.cols()) { assert(false && "DimensionError: lu_inverse: matrix must be square"); throw DimensionError("lu_inverse: matrix must be square"); } const auto n = a.rows(); // Solve the system for each column vector of the identity matrix Matrix inverse(n, n); Vector e(n, T(0)); Vector x(n); for (typename Matrix::size_type j = 0; j < n; ++j) { // Set the column vector of the identity matrix for (typename Matrix::size_type i = 0; i < n; ++i) { e[i] = (i == j) ? T(1) : T(0); } // Solve the system x = lu_solve(a, e, pivot); // Store the result into the inverse matrix for (typename Matrix::size_type i = 0; i < n; ++i) { inverse(i, j) = x[i]; } } return inverse; } /** * @brief Compute the determinant using LU decomposition * @tparam T element type * @param a original matrix * @param pivot pivot strategy (default: Partial) * @return value of the determinant */ template requires sangi::BaseMatrixLike sangi::element_t lu_determinant(const MA& a, PivotStrategy pivot = PivotStrategy::Partial) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); if (a.rows() != a.cols()) { assert(false && "DimensionError: lu_determinant: matrix must be square"); throw DimensionError("lu_determinant: matrix must be square"); } const auto n = a.rows(); // LU decomposition auto [lu, pivots] = lu_decomposition(a, pivot); // Compute the determinant T det = T(1); int sign = 1; // Compute the sign change due to pivot swaps for (typename Matrix::size_type i = 0; i < n; ++i) { if (pivots[i] != i) { sign = -sign; } } // Product of the diagonal elements for (typename Matrix::size_type i = 0; i < n; ++i) { det *= lu(i, i); } return det * T(sign); } /** * @brief Compute the determinant using the Bareiss algorithm (for integer types, division-free) * * Using Sylvester's identity, which guarantees that the divisions in the elimination always divide evenly, * computes the determinant of an integer matrix in O(n³) keeping intermediate results integral. * It can also be used for floating-point types, but lu_determinant is more efficient. * * @tparam T element type (Int, int, long long, etc.) * @param a square matrix * @return determinant det(A) */ template requires sangi::BaseMatrixLike sangi::element_t bareiss_determinant(const MA& a) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); if (a.rows() != a.cols()) { assert(false && "DimensionError: bareiss_determinant: matrix must be square"); throw DimensionError("bareiss_determinant: matrix must be square"); } const auto n = a.rows(); if (n == 0) return T(1); if (n == 1) return a(0, 0); if (n == 2) return a(0, 0) * a(1, 1) - a(0, 1) * a(1, 0); Matrix M = a; int sign = 1; T prev_pivot = T(1); for (typename Matrix::size_type k = 0; k < n; ++k) { // Partial pivot selection typename Matrix::size_type pivot_row = k; for (typename Matrix::size_type i = k + 1; i < n; ++i) { if (M(i, k) != T(0) && (M(pivot_row, k) == T(0) || numeric_traits::pivotBetter(M(i, k), M(pivot_row, k)))) pivot_row = i; } if (pivot_row != k) { for (typename Matrix::size_type j = 0; j < n; ++j) std::swap(M(k, j), M(pivot_row, j)); sign = -sign; } if (M(k, k) == T(0)) return T(0); for (typename Matrix::size_type i = k + 1; i < n; ++i) { for (typename Matrix::size_type j = k + 1; j < n; ++j) { M(i, j) = (M(i, j) * M(k, k) - M(i, k) * M(k, j)) / prev_pivot; } } prev_pivot = M(k, k); } return M(n - 1, n - 1) * T(sign); } /** * @brief Compute the Cholesky decomposition * @tparam T element type * @param a matrix to decompose (symmetric positive definite matrix) * @return the L matrix of the Cholesky decomposition (lower triangular matrix) */ template requires concepts::MatrixOf Matrix cholesky_decomposition(const M& a) { using T = typename M::value_type; SANGI_STATIC_ASSERT_SQUARE(M); if (a.rows() != a.cols()) { assert(false && "DimensionError: cholesky_decomposition: matrix must be square"); throw DimensionError("cholesky_decomposition: matrix must be square"); } const auto n = a.rows(); #if SANGI_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { Matrix L(n, n); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) L(i, j) = a(i, j); lapack_int info; if constexpr (std::is_same_v) info = LAPACKE_spotrf(LAPACK_ROW_MAJOR, 'L', (lapack_int)n, L.data(), (lapack_int)n); else info = LAPACKE_dpotrf(LAPACK_ROW_MAJOR, 'L', (lapack_int)n, L.data(), (lapack_int)n); if (info > 0) throw MathError("cholesky_decomposition: matrix is not positive definite"); // Zero out the upper triangular part (LAPACKE writes only the lower triangle, leaving the upper as-is) for (std::size_t i = 0; i < n; ++i) for (std::size_t j = i + 1; j < n; ++j) L(i, j) = T(0); return L; } #endif // float/double block Cholesky — no is_symmetric check needed // (non-PD is detected via sqrt; symmetry is the caller's responsibility) if constexpr (std::is_same_v || std::is_same_v) { const std::size_t NB = (n < 256) ? 32 : 64; if (n >= 32) { // Copy the whole of A (the upper triangle is not read) Matrix L(n, n); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) L(i, j) = a(i, j); T* __restrict data = L.data(); // Pre-allocate buffers (reused across the whole loop) // trsm_cols: column buffer for TRSM (columns are contiguous) std::vector trsm_cols(n * NB); // L_CB_data: row-major buffer for SYRK (rows are contiguous, lda=nb) std::vector L_CB_data(n * NB); for (std::size_t jb = 0; jb < n; jb += NB) { const std::size_t jend = (std::min)(jb + NB, n); const std::size_t nb = jend - jb; // Phase 1: diagonal-block Cholesky (nb×nb) using PT = PacketTraits; constexpr std::size_t PS = PT::size; for (std::size_t j = jb; j < jend; ++j) { // Diagonal element: L(j,j)² = A(j,j) - Σ_k L(j,k)² T sum = data[j * n + j]; for (std::size_t k = jb; k < j; ++k) sum -= data[j * n + k] * data[j * n + k]; if (sum <= T(0)) throw MathError("cholesky_decomposition: matrix is not positive definite"); data[j * n + j] = std::sqrt(sum); T inv_diag = T(1) / data[j * n + j]; // Off-diagonal elements of column j: L(i,j) = (A(i,j) - Σ_k L(i,k)*L(j,k)) / L(j,j) const std::size_t col_len = jend - j - 1; if (col_len == 0) continue; // Vectorize the dot product over the k loop with SIMD // For each row i = j+1..jend-1, sum[i] -= L(i,k)*L(j,k) // L(j,k) = data[j*n+k], L(i,k) = data[i*n+k] for (std::size_t k = jb; k < j; ++k) { T ljk = data[j * n + k]; for (std::size_t i = j + 1; i < jend; ++i) data[i * n + j] -= data[i * n + k] * ljk; } for (std::size_t i = j + 1; i < jend; ++i) data[i * n + j] *= inv_diag; } if (jend >= n) break; const std::size_t remaining = n - jend; // Phase 2: TRSM — apply axpy_simd via the column buffer { T* tcols = trsm_cols.data(); for (std::size_t j = 0; j < nb; ++j) for (std::size_t i = 0; i < remaining; ++i) tcols[j * remaining + i] = data[(jend + i) * n + (jb + j)]; for (std::size_t j = 0; j < nb; ++j) { T* col_j = tcols + j * remaining; for (std::size_t k = 0; k < j; ++k) { T neg_ljk = -data[(jb + j) * n + (jb + k)]; computation::simd::axpy_simd( col_j, neg_ljk, tcols + k * remaining, remaining); } T inv_diag = T(1) / data[(jb + j) * n + (jb + j)]; for (std::size_t i = 0; i < remaining; ++i) col_j[i] *= inv_diag; } // Write the TRSM result back to L while making a row-major copy for SYRK for (std::size_t i = 0; i < remaining; ++i) { for (std::size_t j = 0; j < nb; ++j) { T val = tcols[j * remaining + i]; data[(jend + i) * n + (jb + j)] = val; L_CB_data[i * nb + j] = val; } } } // Phase 3: SYRK — data[jend..n, jend..n] -= L_CB * L_CB^T // Compute only the lower triangle directly (no transpose buffer, halving FLOPs) { const std::size_t m = remaining; const std::size_t kk = nb; const T* __restrict A = L_CB_data.data(); const std::size_t TB = 64; // The SYRK tile is always 64 (independent of NB) for (std::size_t ib = 0; ib < m; ib += TB) { const std::size_t i_end = (std::min)(ib + TB, m); // Off-diagonal tile (jb2 < ib): 2×4 microkernel for (std::size_t jb2 = 0; jb2 < ib; jb2 += TB) { const std::size_t j_end = (std::min)(jb2 + TB, m); // Process two rows at a time: share the load in the j direction std::size_t i = ib; for (; i + 2 <= i_end; i += 2) { const T* __restrict ai0 = A + (i+0) * kk; const T* __restrict ai1 = A + (i+1) * kk; T* __restrict ci0 = data + (jend + i+0) * n + jend; T* __restrict ci1 = data + (jend + i+1) * n + jend; std::size_t j = jb2; for (; j + 4 <= j_end; j += 4) { const T* __restrict aj0 = A + (j+0)*kk; const T* __restrict aj1 = A + (j+1)*kk; const T* __restrict aj2 = A + (j+2)*kk; const T* __restrict aj3 = A + (j+3)*kk; auto c00 = PT::mul(PT::load(ai0), PT::load(aj0)); auto c01 = PT::mul(PT::load(ai0), PT::load(aj1)); auto c02 = PT::mul(PT::load(ai0), PT::load(aj2)); auto c03 = PT::mul(PT::load(ai0), PT::load(aj3)); auto c10 = PT::mul(PT::load(ai1), PT::load(aj0)); auto c11 = PT::mul(PT::load(ai1), PT::load(aj1)); auto c12 = PT::mul(PT::load(ai1), PT::load(aj2)); auto c13 = PT::mul(PT::load(ai1), PT::load(aj3)); for (std::size_t p = PS; p < kk; p += PS) { auto va0 = PT::load(ai0 + p); auto va1 = PT::load(ai1 + p); auto vb0 = PT::load(aj0 + p); auto vb1 = PT::load(aj1 + p); auto vb2 = PT::load(aj2 + p); auto vb3 = PT::load(aj3 + p); c00 = PT::fmadd(va0, vb0, c00); c01 = PT::fmadd(va0, vb1, c01); c02 = PT::fmadd(va0, vb2, c02); c03 = PT::fmadd(va0, vb3, c03); c10 = PT::fmadd(va1, vb0, c10); c11 = PT::fmadd(va1, vb1, c11); c12 = PT::fmadd(va1, vb2, c12); c13 = PT::fmadd(va1, vb3, c13); } ci0[j+0] -= PT::reduce_add(c00); ci0[j+1] -= PT::reduce_add(c01); ci0[j+2] -= PT::reduce_add(c02); ci0[j+3] -= PT::reduce_add(c03); ci1[j+0] -= PT::reduce_add(c10); ci1[j+1] -= PT::reduce_add(c11); ci1[j+2] -= PT::reduce_add(c12); ci1[j+3] -= PT::reduce_add(c13); } for (; j < j_end; ++j) { auto s0 = PT::mul(PT::load(ai0), PT::load(A + j*kk)); auto s1 = PT::mul(PT::load(ai1), PT::load(A + j*kk)); for (std::size_t p = PS; p < kk; p += PS) { auto vb = PT::load(A + j*kk + p); s0 = PT::fmadd(PT::load(ai0 + p), vb, s0); s1 = PT::fmadd(PT::load(ai1 + p), vb, s1); } ci0[j] -= PT::reduce_add(s0); ci1[j] -= PT::reduce_add(s1); } } // Remainder rows for (; i < i_end; ++i) { const T* __restrict ai = A + i * kk; T* __restrict ci = data + (jend + i) * n + jend; std::size_t j = jb2; for (; j + 4 <= j_end; j += 4) { auto s0 = PT::mul(PT::load(ai), PT::load(A + (j+0)*kk)); auto s1 = PT::mul(PT::load(ai), PT::load(A + (j+1)*kk)); auto s2 = PT::mul(PT::load(ai), PT::load(A + (j+2)*kk)); auto s3 = PT::mul(PT::load(ai), PT::load(A + (j+3)*kk)); for (std::size_t p = PS; p < kk; p += PS) { auto va = PT::load(ai + p); s0 = PT::fmadd(va, PT::load(A + (j+0)*kk + p), s0); s1 = PT::fmadd(va, PT::load(A + (j+1)*kk + p), s1); s2 = PT::fmadd(va, PT::load(A + (j+2)*kk + p), s2); s3 = PT::fmadd(va, PT::load(A + (j+3)*kk + p), s3); } ci[j+0] -= PT::reduce_add(s0); ci[j+1] -= PT::reduce_add(s1); ci[j+2] -= PT::reduce_add(s2); ci[j+3] -= PT::reduce_add(s3); } for (; j < j_end; ++j) { auto s = PT::mul(PT::load(ai), PT::load(A + j*kk)); for (std::size_t p = PS; p < kk; p += PS) s = PT::fmadd(PT::load(ai + p), PT::load(A + j*kk + p), s); ci[j] -= PT::reduce_add(s); } } } // Diagonal tile: lower triangle only for (std::size_t i = ib; i < i_end; ++i) { const T* __restrict ai = A + i * kk; T* __restrict ci = data + (jend + i) * n + jend; std::size_t j = ib; for (; j + 4 <= i + 1; j += 4) { auto s0 = PT::mul(PT::load(ai), PT::load(A + (j+0)*kk)); auto s1 = PT::mul(PT::load(ai), PT::load(A + (j+1)*kk)); auto s2 = PT::mul(PT::load(ai), PT::load(A + (j+2)*kk)); auto s3 = PT::mul(PT::load(ai), PT::load(A + (j+3)*kk)); for (std::size_t p = PS; p < kk; p += PS) { auto va = PT::load(ai + p); s0 = PT::fmadd(va, PT::load(A + (j+0)*kk + p), s0); s1 = PT::fmadd(va, PT::load(A + (j+1)*kk + p), s1); s2 = PT::fmadd(va, PT::load(A + (j+2)*kk + p), s2); s3 = PT::fmadd(va, PT::load(A + (j+3)*kk + p), s3); } ci[j+0] -= PT::reduce_add(s0); ci[j+1] -= PT::reduce_add(s1); ci[j+2] -= PT::reduce_add(s2); ci[j+3] -= PT::reduce_add(s3); } for (; j <= i; ++j) { auto s = PT::mul(PT::load(ai), PT::load(A + j*kk)); for (std::size_t p = PS; p < kk; p += PS) s = PT::fmadd(PT::load(ai + p), PT::load(A + j*kk + p), s); ci[j] -= PT::reduce_add(s); } } } } } return L; } // float/double small matrix (n < 32): scalar Cholesky (in-place) { Matrix L(n, n); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) L(i, j) = a(i, j); T* __restrict ld = L.data(); for (std::size_t i = 0; i < n; ++i) { T sum = ld[i * n + i]; for (std::size_t k = 0; k < i; ++k) sum -= ld[i * n + k] * ld[i * n + k]; if (sum <= T(0)) throw MathError("cholesky_decomposition: matrix is not positive definite"); ld[i * n + i] = std::sqrt(sum); T inv_diag = T(1) / ld[i * n + i]; for (std::size_t j = i + 1; j < n; ++j) { T s = ld[j * n + i]; for (std::size_t k = 0; k < i; ++k) s -= ld[j * n + k] * ld[i * n + k]; ld[j * n + i] = s * inv_diag; } } return L; } } // Generic type: symmetry/Hermitian check + scalar Cholesky { const auto eps10 = numeric_traits::epsilon() * 10; for (std::size_t i = 0; i < n; ++i) { for (std::size_t j = i + 1; j < n; ++j) { if constexpr (numeric_traits::is_complex) { // Hermitian: a(i,j) == conj(a(j,i)) if (std::abs(a(i, j) - sangi::conj(a(j, i))) > eps10) throw MathError("cholesky_decomposition: matrix must be Hermitian"); } else { if (std::abs(a(i, j) - a(j, i)) > eps10) throw MathError("cholesky_decomposition: matrix must be symmetric"); } } } } const T zero = numeric_traits::zero(); const T one = numeric_traits::one(); Matrix L(n, n, zero); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j <= i; ++j) L(i, j) = a(i, j); for (typename Matrix::size_type i = 0; i < n; ++i) { T sum = L(i, i); for (typename Matrix::size_type k = 0; k < i; ++k) { if constexpr (numeric_traits::is_complex) sum -= sangi::conj(L(i, k)) * L(i, k); else sum -= L(i, k) * L(i, k); } if constexpr (numeric_traits::is_complex) { // For Hermitian positive definite, the sum of the diagonal element is real positive auto re = sum.real(); if (re <= 0) throw MathError("cholesky_decomposition: matrix is not positive definite"); L(i, i) = T(std::sqrt(re)); } else { if (sum <= zero) throw MathError("cholesky_decomposition: matrix is not positive definite"); L(i, i) = std::sqrt(sum); } T inv_diag = one / L(i, i); for (typename Matrix::size_type j = i + 1; j < n; ++j) { sum = L(j, i); for (typename Matrix::size_type k = 0; k < i; ++k) { if constexpr (numeric_traits::is_complex) sum -= L(j, k) * sangi::conj(L(i, k)); else sum -= L(j, k) * L(i, k); } L(j, i) = sum * inv_diag; } } return L; } /** * @brief Solve a system of linear equations using the Cholesky decomposition * @tparam T element type * @param a coefficient matrix (symmetric positive definite matrix) * @param b right-hand-side vector * @return solution vector */ template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> cholesky_solve(const MA& a, const VB& b_basevec) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); Vector b(b_basevec); // 1 copy via BaseVector ctor (contiguous buffer for the SIMD path) if (a.rows() != a.cols()) { assert(false && "DimensionError: cholesky_solve: matrix must be square"); throw DimensionError("cholesky_solve: matrix must be square"); } if (a.rows() != b.size()) { assert(false && "DimensionError: cholesky_solve: matrix and vector dimensions mismatch"); throw DimensionError("cholesky_solve: matrix and vector dimensions mismatch"); } const auto n = a.rows(); #if SANGI_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { Matrix Acopy = a; Vector x = b; lapack_int info; if constexpr (std::is_same_v) info = LAPACKE_sposv(LAPACK_ROW_MAJOR, 'L', (lapack_int)n, 1, Acopy.data(), (lapack_int)n, x.data(), 1); else info = LAPACKE_dposv(LAPACK_ROW_MAJOR, 'L', (lapack_int)n, 1, Acopy.data(), (lapack_int)n, x.data(), 1); if (info > 0) throw MathError("cholesky_solve: matrix is not positive definite"); return x; } #endif // Cholesky decomposition Matrix L = cholesky_decomposition(a); // float/double: forward/backward substitution with raw pointers if constexpr (std::is_same_v || std::is_same_v) { const T* __restrict ld = L.data(); const T* __restrict bd = b.data(); // Forward substitution (Ly = b) — row-wise read (contiguous) Vector y(n); T* __restrict yd = y.data(); for (std::size_t i = 0; i < n; ++i) { const T* __restrict row = ld + i * n; T sum = bd[i]; std::size_t j = 0; using PT = PacketTraits; constexpr std::size_t PS = PT::size; if (i >= PS) { auto vsum = PT::mul(PT::load(row), PT::load(yd)); for (j = PS; j + PS <= i; j += PS) vsum = PT::fmadd(PT::load(row + j), PT::load(yd + j), vsum); sum -= PT::reduce_add(vsum); } for (; j < i; ++j) sum -= row[j] * yd[j]; yd[i] = sum / row[i]; } // Backward substitution (L^T x = y) — column-wise axpy (contiguous via row reads) Vector x = y; T* __restrict xd = x.data(); for (std::size_t jj = 0; jj < n; ++jj) { std::size_t j = n - 1 - jj; xd[j] /= ld[j * n + j]; T neg_xj = -xd[j]; const T* __restrict row_j = ld + j * n; computation::simd::axpy_simd(xd, neg_xj, row_j, j); } return x; } // Generic type: forward substitution (Ly = b) Vector y(n); for (std::size_t i = 0; i < n; ++i) { T sum = b[i]; for (std::size_t j = 0; j < i; ++j) sum -= L(i, j) * y[j]; y[i] = sum / L(i, i); } // Backward substitution (L^H x = y) *for real numbers L^H = L^T Vector x(n); for (std::size_t i = n; i-- > 0; ) { T sum = y[i]; for (std::size_t j = i + 1; j < n; ++j) { if constexpr (numeric_traits::is_complex) sum -= sangi::conj(L(j, i)) * x[j]; else sum -= L(j, i) * x[j]; } // L(i,i) is always real positive due to Cholesky, so conj is unnecessary x[i] = sum / L(i, i); } return x; } // ==================================================================== // Householder transformation standalone API // ==================================================================== /** * @brief Compute the Householder vector and coefficient * * For input vector x, returns v, beta, alpha satisfying * (I - beta * v * v^T) * x = alpha * e1. * * @param x input vector (size >= 1) * @return {v, beta, alpha} — v: Householder vector, beta: coefficient, alpha: first component of the result */ template std::tuple, T, T> householder_vector(const BaseVector& x_basevec) { Vector x(x_basevec); // 1 copy via BaseVector ctor const auto n = x.size(); if (n == 0) { assert(false && "DimensionError: householder_vector: empty vector"); throw DimensionError("householder_vector: empty vector"); } const T zero = numeric_traits::zero(); const T one = numeric_traits::one(); const T two = one + one; // Compute the norm of x (Complex: conj(x[i]) * x[i]) T sigma = zero; for (std::size_t i = 1; i < n; ++i) { if constexpr (numeric_traits::is_complex) sigma += sangi::conj(x[i]) * x[i]; else sigma += x[i] * x[i]; } Vector v = x; if constexpr (numeric_traits::is_complex) { auto sigma_abs = std::abs(sigma); if (sigma_abs == 0) { // x is in the e1 direction return {v, zero, x[0]}; } // Compute ||x|| auto x_norm_real = std::sqrt(std::abs(sangi::conj(x[0]) * x[0] + sigma)); T x_norm = T(x_norm_real); // Sign: match the phase auto abs_x0 = std::abs(x[0]); T alpha = (abs_x0 > decltype(abs_x0)(0)) ? T(-x_norm_real) * x[0] / T(abs_x0) : T(x_norm_real); v[0] = x[0] - alpha; // beta = 2 / (v^H v) T v_norm_sq = sangi::conj(v[0]) * v[0] + sigma; T beta = two / v_norm_sq; return {v, beta, alpha}; } else { if (sigma == zero && x[0] >= zero) { // x is already in the positive e1 direction → identity transform return {v, zero, x[0]}; } if (sigma == zero && x[0] < zero) { // x = -alpha * e1 → sign flip v[0] = zero; return {v, two, -x[0]}; } T x_norm = static_cast(std::sqrt( static_cast(x[0] * x[0] + sigma))); // Choose the sign for numerical stability T alpha = (x[0] >= zero) ? -x_norm : x_norm; v[0] = x[0] - alpha; // beta = 2 / (v^T v) = 2 / (v[0]^2 + sigma) T v_norm_sq = v[0] * v[0] + sigma; T beta = two / v_norm_sq; return {v, beta, alpha}; } } /** * @brief Apply a Householder reflection from the left: A ← (I - beta * v * v^H) * A * * Applied to the specified row range [row_start, row_start+v.size()) of matrix A. * @param v Householder vector * @param beta coefficient * @param A target matrix (updated in-place) * @param row_start starting row of application (default 0) * @param col_start starting column of application (default 0) */ template void apply_householder_left( const BaseVector& v_basevec, T beta, Matrix& A, std::size_t row_start = 0, std::size_t col_start = 0) { Vector v(v_basevec); // 1 copy via BaseVector ctor const auto vn = v.size(); const auto cols = A.cols(); const T zero = numeric_traits::zero(); // For each column j, w[j] = v^H * A(row_start:row_start+vn, j) for (std::size_t j = col_start; j < cols; ++j) { T dot = zero; for (std::size_t i = 0; i < vn; ++i) { if constexpr (numeric_traits::is_complex) dot += sangi::conj(v[i]) * A(row_start + i, j); else dot += v[i] * A(row_start + i, j); } T factor = beta * dot; for (std::size_t i = 0; i < vn; ++i) { A(row_start + i, j) -= factor * v[i]; } } } /** * @brief Apply a Householder reflection from the right: A ← A * (I - beta * v * v^H) * * Applied to the specified column range [col_start, col_start+v.size()) of matrix A. */ template void apply_householder_right( Matrix& A, const BaseVector& v_basevec, T beta, std::size_t row_start = 0, std::size_t col_start = 0) { Vector v(v_basevec); // 1 copy via BaseVector ctor const auto vn = v.size(); const auto rows = A.rows(); const T zero = numeric_traits::zero(); // For each row i, w[i] = A(i, col_start:col_start+vn) * v for (std::size_t i = row_start; i < rows; ++i) { T dot = zero; for (std::size_t j = 0; j < vn; ++j) { dot += A(i, col_start + j) * v[j]; } T factor = beta * dot; for (std::size_t j = 0; j < vn; ++j) { if constexpr (numeric_traits::is_complex) A(i, col_start + j) -= factor * sangi::conj(v[j]); else A(i, col_start + j) -= factor * v[j]; } } } /** * @brief Direct solver via Householder QR decomposition: Ax = b * * For an m×n matrix A (m >= n), returns the least-squares solution. * For m == n it is the direct solution of a nonsingular system. * * @param A coefficient matrix (m×n, m >= n) * @param b right-hand-side vector (size m) * @return x solution vector (size n) */ template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> householder_solve(const MA& A, const VB& b) { using T = sangi::element_t; SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); const auto m = A.rows(); const auto n = A.cols(); if (m < n) { assert(false && "DimensionError: householder_solve: requires m >= n (overdetermined or square)"); throw DimensionError("householder_solve: requires m >= n (overdetermined or square)"); } if (b.size() != m) { assert(false && "DimensionError: householder_solve: b size must equal A rows"); throw DimensionError("householder_solve: b size must equal A rows"); } // R = copy of A, d = copy of b (compute Q^T b directly) Matrix R = A; Vector d = b; auto min_mn = std::min(m, n); // Block Householder QR (WY representation) — for float/double with n >= 64 if constexpr (std::is_same_v || std::is_same_v) { constexpr std::size_t NB = 16; if (min_mn >= NB * 2) { T* __restrict rdata = R.data(); T* __restrict ddata = d.data(); // Pre-allocate Householder coefficients tau and buffers (reused across the whole loop) std::vector tau(min_mn); std::vector w_buf(n); std::vector T_fac(NB * NB); std::vector VT_rm(NB * m); std::vector V_rm(m * NB); std::vector W_buf(NB * n); for (std::size_t jb = 0; jb < min_mn; jb += NB) { const std::size_t jend = (std::min)(jb + NB, min_mn); const std::size_t nb = jend - jb; // Phase 1: panel factorization (columns jb..jend-1) // Scalar Householder for each column j, SIMD-applied to the remaining columns in the panel using PT = PacketTraits; constexpr std::size_t PS = PT::size; for (std::size_t j = jb; j < jend; ++j) { const std::size_t len = m - j; // sigma = sum of squares below diagonal T sigma = T(0); for (std::size_t i = 1; i < len; ++i) sigma += rdata[(j + i) * n + j] * rdata[(j + i) * n + j]; if (sigma == T(0) && rdata[j * n + j] >= T(0)) { tau[j] = T(0); continue; } T x0 = rdata[j * n + j]; T x_norm = std::sqrt(x0 * x0 + sigma); T alpha = (x0 >= T(0)) ? -x_norm : x_norm; T v0 = x0 - alpha; T v_norm_sq = v0 * v0 + sigma; T tau_j = T(2) * v0 * v0 / v_norm_sq; tau[j] = tau_j; // Normalize v (v[0] = 1, v[i] /= v0) T inv_v0 = T(1) / v0; for (std::size_t i = 1; i < len; ++i) rdata[(j + i) * n + j] *= inv_v0; rdata[j * n + j] = alpha; // Apply Householder to the remaining columns in the panel (SIMD) // v = [1, rdata[j+1..m-1, j]] // Process dot + axpy for PS columns simultaneously const std::size_t col_start = j + 1; const std::size_t col_end = jend; const std::size_t rem_cols = col_end - col_start; const auto neg_tau = PT::set1(-tau_j); std::size_t cc = 0; for (; cc + PS <= rem_cols; cc += PS) { const std::size_t c = col_start + cc; // Pass 1: dot product (v^T * A[:,c:c+PS-1]) auto dot_v = PT::load(&rdata[j * n + c]); // v[0]=1 for (std::size_t i = 1; i < len; ++i) { auto vi = PT::set1(rdata[(j + i) * n + j]); dot_v = PT::fmadd(vi, PT::load(&rdata[(j + i) * n + c]), dot_v); } // factor = -tau * dot (negated to prepare for fmadd) auto neg_factor = PT::mul(neg_tau, dot_v); // Pass 2: A[:,c:c+PS-1] += neg_factor * v PT::store(&rdata[j * n + c], PT::add(PT::load(&rdata[j * n + c]), neg_factor)); // v[0]=1 for (std::size_t i = 1; i < len; ++i) { auto vi = PT::set1(rdata[(j + i) * n + j]); T* ptr = &rdata[(j + i) * n + c]; PT::store(ptr, PT::fmadd(neg_factor, vi, PT::load(ptr))); } } // Scalar remainder for (; cc < rem_cols; ++cc) { const std::size_t c = col_start + cc; T dot = rdata[j * n + c]; for (std::size_t i = 1; i < len; ++i) dot += rdata[(j + i) * n + j] * rdata[(j + i) * n + c]; T factor = tau_j * dot; rdata[j * n + c] -= factor; for (std::size_t i = 1; i < len; ++i) rdata[(j + i) * n + c] -= factor * rdata[(j + i) * n + j]; } // Apply to d as well { T dot = ddata[j]; for (std::size_t i = 1; i < len; ++i) dot += rdata[(j + i) * n + j] * ddata[j + i]; T factor = tau_j * dot; ddata[j] -= factor; for (std::size_t i = 1; i < len; ++i) ddata[j + i] -= factor * rdata[(j + i) * n + j]; } } if (jend >= n) continue; const std::size_t trailing = n - jend; const std::size_t pr = m - jb; // panel rows // Phase 2: batch-update the trailing columns with the WY block representation // Q_block = I - V * T * V^T, T is nb×nb upper triangular // // (b) Expand V into a row-major buffer (done first since it is used in building the T factor) // VT_rm = V^T in row-major (nb × pr, lda=pr) — for gemm step (c) // V_rm = V in row-major (pr × nb, lda=nb) — for gemm step (e) std::fill_n(VT_rm.data(), nb * pr, T(0)); std::fill_n(V_rm.data(), pr * nb, T(0)); for (std::size_t kk = 0; kk < nb; ++kk) { const std::size_t j = jb + kk; VT_rm[kk * pr + kk] = T(1); V_rm[kk * nb + kk] = T(1); const std::size_t len_j = m - j; for (std::size_t i = 1; i < len_j; ++i) { T val = rdata[(j + i) * n + j]; VT_rm[kk * pr + kk + i] = val; V_rm[(kk + i) * nb + kk] = val; } } // (a) Build the T factor (using the contiguous VT_rm buffer) std::fill_n(T_fac.data(), nb * nb, T(0)); for (std::size_t jj = 0; jj < nb; ++jj) { const std::size_t j = jb + jj; T_fac[jj * nb + jj] = tau[j]; if (tau[j] == T(0)) continue; // T[0:jj, jj] = -tau[j] * T[0:jj, 0:jj] * V[0:jj]^T * v_jj // Use VT_rm: v_kk is VT_rm[kk*pr + kk:], v_jj is VT_rm[jj*pr + jj:] const std::size_t len_j = m - j; for (std::size_t kk = 0; kk < jj; ++kk) { // Dot product over contiguous memory (VT_rm[kk, jj:jj+len_j] and VT_rm[jj, jj:jj+len_j]) const T* __restrict vk_ptr = &VT_rm[kk * pr + jj]; const T* __restrict vj_ptr = &VT_rm[jj * pr + jj]; T dot = T(0); for (std::size_t i = 0; i < len_j; ++i) dot += vk_ptr[i] * vj_ptr[i]; T_fac[kk * nb + jj] = dot; } // T[0:jj, jj] = -tau[j] * T[0:jj, 0:jj] * z for (std::size_t kk = 0; kk < jj; ++kk) { T sum = T(0); for (std::size_t ll = kk; ll < jj; ++ll) sum += T_fac[kk * nb + ll] * T_fac[ll * nb + jj]; T_fac[kk * nb + jj] = -tau[j] * sum; } } // (c) W = V^T * A_trail (nb × trailing) via gemm // VT_rm (nb × pr, lda=pr) * A_trail (pr × trailing, ldb=n) → W (nb × trailing) std::fill_n(W_buf.data(), nb * trailing, T(0)); DefaultComputePolicy::gemm( VT_rm.data(), &rdata[jb * n + jend], W_buf.data(), nb, trailing, pr, pr, n, trailing); // (d) W = T^T * W (lower triangular nb×nb × nb×trailing) // Since Q^T = I - V * T^T * V^T, apply T^T // In-place lower-triangular trmv: compute from bottom to top for (std::size_t kk = nb; kk-- > 0; ) { T t_kk = T_fac[kk * nb + kk]; if (t_kk != T(1)) { T* w_row = &W_buf[kk * trailing]; for (std::size_t c = 0; c < trailing; ++c) w_row[c] *= t_kk; } for (std::size_t ll = 0; ll < kk; ++ll) { T t_lk = T_fac[ll * nb + kk]; if (t_lk == T(0)) continue; computation::simd::axpy_simd( &W_buf[kk * trailing], t_lk, &W_buf[ll * trailing], trailing); } } // (e) A_trail -= V * W via gemm // Sign-flip W so that A_trail += V * (-W) for (std::size_t i = 0; i < nb * trailing; ++i) W_buf[i] = -W_buf[i]; // V_rm (pr × nb, lda=nb) * (-W) (nb × trailing, ldb=trailing) // → accumulate into A_trail (pr × trailing, ldc=n) DefaultComputePolicy::gemm( V_rm.data(), W_buf.data(), &rdata[jb * n + jend], pr, trailing, nb, nb, trailing, n); } // Backward substitution double max_diag = 0.0; for (std::size_t i = 0; i < n; ++i) { double ad = std::abs(static_cast(rdata[i * n + i])); if (ad > max_diag) max_diag = ad; } double sing_tol = max_diag * static_cast(n) * std::numeric_limits::epsilon(); if (sing_tol == 0.0) sing_tol = std::numeric_limits::epsilon(); Vector x(n, T{0}); for (std::size_t ii = 0; ii < n; ++ii) { std::size_t i = n - 1 - ii; T sum = ddata[i]; for (std::size_t j = i + 1; j < n; ++j) sum -= rdata[i * n + j] * x[j]; if (std::abs(static_cast(rdata[i * n + i])) < sing_tol) throw MathError("householder_solve: singular or near-singular matrix"); x[i] = sum / rdata[i * n + i]; } return x; } } // Scalar Householder QR (small matrices or non-floating-point types) for (std::size_t k = 0; k < min_mn; ++k) { // Build the Householder vector from row k onward of column k std::size_t len = m - k; Vector x(len); for (std::size_t i = 0; i < len; ++i) { x[i] = R(k + i, k); } auto [v, beta, alpha] = householder_vector(x); if (beta == T{0}) continue; // Apply to R from the left apply_householder_left(v, beta, R, k, k); // Apply to d as well: d ← (I - beta * v * v^T) * d T dot_d = T{0}; for (std::size_t i = 0; i < len; ++i) { dot_d += v[i] * d[k + i]; } T factor_d = beta * dot_d; for (std::size_t i = 0; i < len; ++i) { d[k + i] -= factor_d * v[i]; } } // For singularity detection: the largest absolute value of R's diagonal double max_diag = 0.0; for (std::size_t i = 0; i < n; ++i) { double ad = std::abs(static_cast(R(i, i))); if (ad > max_diag) max_diag = ad; } double sing_tol = max_diag * static_cast(n) * std::numeric_limits::epsilon(); if (sing_tol == 0.0) sing_tol = std::numeric_limits::epsilon(); // Backward substitution: R(0:n, 0:n) x = d(0:n) Vector x(n, T{0}); for (std::size_t ii = 0; ii < n; ++ii) { std::size_t i = n - 1 - ii; T sum = d[i]; for (std::size_t j = i + 1; j < n; ++j) { sum -= R(i, j) * x[j]; } T rii = R(i, i); if (std::abs(static_cast(rii)) < sing_tol) { throw MathError("householder_solve: singular or near-singular matrix"); } x[i] = sum / rii; } return x; } // ==================================================================== // Bidiagonalization standalone API // ==================================================================== /** * @brief Structure storing the result of bidiagonalization * * A = U * B * V^T * B is an upper bidiagonal matrix (diagonal d, superdiagonal e) */ template struct BidiagonalResult { Matrix U; ///< left orthogonal matrix (m×m) Vector d; ///< diagonal elements (size min(m,n)) Vector e; ///< superdiagonal elements (size min(m,n)-1, e[k] = B(k, k+1)) Matrix V; ///< right orthogonal matrix (n×n) }; /** * @brief Bidiagonalization by Householder transformation * * Transforms an m×n matrix A (m >= n) into an upper bidiagonal matrix B. * Returns U (m×m), B (upper bidiagonal), V (n×n) satisfying A = U * B * V^T. * * For m < n, bidiagonalize A^T and transpose back. * * @param A input matrix * @return BidiagonalResult {U, d, e, V} */ template requires sangi::BaseMatrixLike BidiagonalResult> bidiagonalize(const MA& A) { using T = sangi::element_t; // rectangular OK (for m < n, handle A^T) const size_t m = A.rows(); const size_t n = A.cols(); if (m == 0 || n == 0) { assert(false && "DimensionError: bidiagonalize: empty matrix"); throw DimensionError("bidiagonalize: empty matrix"); } if (m < n) { assert(false && "DimensionError: bidiagonalize: requires m >= n"); throw DimensionError( "bidiagonalize: requires m >= n (use A^T for wide matrices)"); } // below, m >= n const size_t min_mn = n; // Working copy Matrix W(A); // Initialize U (m×m), V (n×n) as identity matrices Matrix U = Matrix::identity(m); Matrix V = Matrix::identity(n); const T zero = numeric_traits::zero(); const T one = numeric_traits::one(); // Diagonal and superdiagonal elements std::vector diag_vals(n, zero); std::vector super_vals(n, zero); // super_vals[k+1] = B(k, k+1) for (size_t k = 0; k < n; ++k) { // --- Left Householder: zero out W(k:m, k) from row k+1 onward --- { auto sigma_sq = decltype(std::abs(zero))(0); for (size_t i = k; i < m; ++i) sigma_sq += std::abs(W(i, k)) * std::abs(W(i, k)); auto alpha_r = std::sqrt(sigma_sq); if (alpha_r > 0) { T alpha; if constexpr (numeric_traits::is_complex) { auto abs_wkk = std::abs(W(k, k)); alpha = (abs_wkk > 0) ? T(-alpha_r) * W(k, k) / T(abs_wkk) : T(alpha_r); } else { alpha = (W(k, k) > zero) ? T(-alpha_r) : T(alpha_r); } W(k, k) -= alpha; T beta = one / (alpha * W(k, k)); // = -2 / (v^H v) // Apply to W(k:m, k+1:n) for (size_t j = k + 1; j < n; ++j) { T dot_val = zero; for (size_t i = k; i < m; ++i) { if constexpr (numeric_traits::is_complex) dot_val += sangi::conj(W(i, k)) * W(i, j); else dot_val += W(i, k) * W(i, j); } dot_val *= beta; for (size_t i = k; i < m; ++i) W(i, j) += W(i, k) * dot_val; } // Accumulate into U (U = U * H_k) for (size_t i = 0; i < m; ++i) { T dot_val = zero; for (size_t j = k; j < m; ++j) { if constexpr (numeric_traits::is_complex) dot_val += U(i, j) * sangi::conj(W(j, k)); else dot_val += U(i, j) * W(j, k); } dot_val *= beta; for (size_t j = k; j < m; ++j) U(i, j) += dot_val * W(j, k); } diag_vals[k] = alpha; } else { diag_vals[k] = W(k, k); } } // --- Right Householder: zero out W(k, k+2:n) --- if (k + 2 <= n - 1) { auto sigma_sq = decltype(std::abs(zero))(0); for (size_t j = k + 1; j < n; ++j) sigma_sq += std::abs(W(k, j)) * std::abs(W(k, j)); auto alpha_r = std::sqrt(sigma_sq); if (alpha_r > 0) { T alpha; if constexpr (numeric_traits::is_complex) { auto abs_wk1 = std::abs(W(k, k + 1)); alpha = (abs_wk1 > 0) ? T(-alpha_r) * W(k, k + 1) / T(abs_wk1) : T(alpha_r); } else { alpha = (W(k, k + 1) > zero) ? T(-alpha_r) : T(alpha_r); } W(k, k + 1) -= alpha; T beta = one / (alpha * W(k, k + 1)); // Apply to W(k+1:m, k+1:n) for (size_t i = k + 1; i < m; ++i) { T dot_val = zero; for (size_t j = k + 1; j < n; ++j) { if constexpr (numeric_traits::is_complex) dot_val += sangi::conj(W(k, j)) * W(i, j); else dot_val += W(k, j) * W(i, j); } dot_val *= beta; for (size_t j = k + 1; j < n; ++j) W(i, j) += W(k, j) * dot_val; } // Accumulate into V for (size_t i = 0; i < n; ++i) { T dot_val = zero; for (size_t j = k + 1; j < n; ++j) { if constexpr (numeric_traits::is_complex) dot_val += sangi::conj(W(k, j)) * V(i, j); else dot_val += W(k, j) * V(i, j); } dot_val *= beta; for (size_t j = k + 1; j < n; ++j) V(i, j) += W(k, j) * dot_val; } super_vals[k + 1] = alpha; } else { super_vals[k + 1] = W(k, k + 1); } } else if (k + 1 < n) { super_vals[k + 1] = W(k, k + 1); } } // Convert the result into Vectors Vector d(min_mn); for (size_t i = 0; i < min_mn; ++i) d[i] = diag_vals[i]; Vector e(min_mn > 1 ? min_mn - 1 : 0); for (size_t i = 0; i < e.size(); ++i) e[i] = super_vals[i + 1]; return { std::move(U), std::move(d), std::move(e), std::move(V) }; } /** * @brief Utility to explicitly construct a bidiagonal matrix * * @param d diagonal elements (size p) * @param e superdiagonal elements (size p-1) * @return Matrix upper bidiagonal matrix (p×p) */ template Matrix build_bidiagonal_matrix(const BaseVector& d_basevec, const BaseVector& e_basevec) { Vector d(d_basevec); // 1 copy via BaseVector ctor Vector e(e_basevec); // 1 copy via BaseVector ctor const size_t p = d.size(); if (p > 1 && e.size() != p - 1) { assert(false && "DimensionError: build_bidiagonal_matrix: e.size() must equal d.size()-1"); throw DimensionError("build_bidiagonal_matrix: e.size() must equal d.size()-1"); } Matrix B(p, p, numeric_traits::zero()); for (size_t i = 0; i < p; ++i) B(i, i) = d[i]; for (size_t i = 0; i + 1 < p; ++i) B(i, i + 1) = e[i]; return B; } // ==================================================================== // QR decomposition // ==================================================================== /** * @brief Compute the QR decomposition (via Householder reflections) * @tparam T element type * @param a matrix to decompose * @return a pair of Q and R */ template requires concepts::MatrixOf std::pair, Matrix> qr_decomposition(const M& a) { using T = typename M::value_type; const auto m = a.rows(); const auto n = a.cols(); #if SANGI_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { const auto k = std::min(m, n); Matrix R(m, n); for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < n; ++j) R(i, j) = a(i, j); std::vector tau(k); lapack_int info; // QR decomposition (Householder form) if constexpr (std::is_same_v) info = LAPACKE_sgeqrf(LAPACK_ROW_MAJOR, (lapack_int)m, (lapack_int)n, R.data(), (lapack_int)n, tau.data()); else info = LAPACKE_dgeqrf(LAPACK_ROW_MAJOR, (lapack_int)m, (lapack_int)n, R.data(), (lapack_int)n, tau.data()); if (info != 0) throw MathError("qr_decomposition: LAPACKE_geqrf failed"); // Generate Q explicitly: copy the Householder vectors into an m×m matrix Matrix Q(m, m, T(0)); for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < n; ++j) Q(i, j) = R(i, j); if constexpr (std::is_same_v) info = LAPACKE_sorgqr(LAPACK_ROW_MAJOR, (lapack_int)m, (lapack_int)m, (lapack_int)k, Q.data(), (lapack_int)m, tau.data()); else info = LAPACKE_dorgqr(LAPACK_ROW_MAJOR, (lapack_int)m, (lapack_int)m, (lapack_int)k, Q.data(), (lapack_int)m, tau.data()); if (info != 0) throw MathError("qr_decomposition: LAPACKE_orgqr failed"); // Zero out the lower triangular part of R for (std::size_t i = 1; i < m; ++i) for (std::size_t j = 0; j < std::min(i, n); ++j) R(i, j) = T(0); return { Q, R }; } #endif Matrix Q(m, m); Matrix R(m, n); for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < n; ++j) R(i, j) = a(i, j); const T zero = numeric_traits::zero(); const T one = numeric_traits::one(); const T two = one + one; // Initialize Q as the identity matrix for (typename Matrix::size_type i = 0; i < m; ++i) { for (typename Matrix::size_type j = 0; j < m; ++j) { Q(i, j) = (i == j) ? one : zero; } } // Computation via Householder reflections for (typename Matrix::size_type k = 0; k < std::min(m, n); ++k) { // Compute the Householder vector Vector x(m - k); for (typename Matrix::size_type i = 0; i < m - k; ++i) { x[i] = R(k + i, k); } // Compute the norm (Complex-aware: dot returns conj(x)*x, so take the real part) auto dot_xx = dot(x, x); auto x_norm_real = std::sqrt(std::abs(dot_xx)); T x_norm = T(x_norm_real); // Zero-vector check if (x_norm_real < numeric_traits::epsilon()) { continue; } // Signed norm (for Complex, match the phase) T alpha; if constexpr (numeric_traits::is_complex) { auto abs_x0 = std::abs(x[0]); alpha = (abs_x0 > decltype(abs_x0)(0)) ? T(-x_norm_real) * x[0] / T(abs_x0) : T(x_norm_real); } else { alpha = (x[0] >= zero) ? -x_norm : x_norm; } // Modify only the first element (x[0] - alpha) x[0] -= alpha; // Squared norm of the Householder vector auto x_norm_squared = dot(x, x); // Check for numerical stability if (std::abs(x_norm_squared) < numeric_traits::epsilon()) { continue; } // Apply the Householder matrix for (typename Matrix::size_type j = k; j < n; ++j) { T dot_product = zero; for (typename Matrix::size_type i = 0; i < m - k; ++i) { if constexpr (numeric_traits::is_complex) dot_product += sangi::conj(x[i]) * R(k + i, j); else dot_product += x[i] * R(k + i, j); } T factor = (two * dot_product) / x_norm_squared; for (typename Matrix::size_type i = 0; i < m - k; ++i) { R(k + i, j) -= factor * x[i]; } } // Likewise apply the Householder matrix to Q for (typename Matrix::size_type j = 0; j < m; ++j) { T dot_product = zero; for (typename Matrix::size_type i = 0; i < m - k; ++i) { if constexpr (numeric_traits::is_complex) dot_product += sangi::conj(x[i]) * Q(k + i, j); else dot_product += x[i] * Q(k + i, j); } T factor = (two * dot_product) / x_norm_squared; for (typename Matrix::size_type i = 0; i < m - k; ++i) { Q(k + i, j) -= factor * x[i]; } } } // Q is composed of column vectors, so a transpose is actually required Matrix Q_transpose(m, m); for (typename Matrix::size_type i = 0; i < m; ++i) { for (typename Matrix::size_type j = 0; j < m; ++j) { Q_transpose(i, j) = Q(j, i); } } return { Q_transpose, R }; } /** * @brief Solve a system of linear equations using QR decomposition * @tparam T element type * @param a coefficient matrix * @param b right-hand-side vector * @return solution vector */ template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> qr_solve(const MA& a, const VB& b) { using T = sangi::element_t; SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); if (a.rows() != b.size()) { assert(false && "DimensionError: qr_solve: matrix and vector dimensions mismatch"); throw DimensionError("qr_solve: matrix and vector dimensions mismatch"); } // float/double: use householder_solve (does not form Q explicitly) if constexpr (std::is_same_v || std::is_same_v) { return householder_solve(a, b); } const auto m = a.rows(); const auto n = a.cols(); // QR decomposition auto [Q, R] = qr_decomposition(a); // Compute Q^T * b Vector Q_transpose_b(m); for (typename Matrix::size_type i = 0; i < m; ++i) { Q_transpose_b[i] = T(0); for (typename Matrix::size_type j = 0; j < m; ++j) { Q_transpose_b[i] += Q(j, i) * b[j]; // Q^T = Q(j,i) } } // Backward substitution (Rx = Q^T * b) Vector x(n, T(0)); // Least-squares solution (for overdetermined systems) const auto min_rank = std::min(m, n); for (typename Matrix::size_type i = min_rank; i-- > 0; ) { T sum = Q_transpose_b[i]; for (typename Matrix::size_type j = i + 1; j < min_rank; ++j) { sum -= R(i, j) * x[j]; } // Check for zero diagonal elements if (std::abs(R(i, i)) < std::numeric_limits::epsilon()) { // In the case of a singular matrix // Consider switching to singular value decomposition (SVD) throw MathError("qr_solve: matrix is singular or nearly singular"); } x[i] = sum / R(i, i); } return x; } /** * @brief QR decomposition with column pivoting (via Householder reflections) * * Decomposes into the form AP = QR. * At each step, select the column with the largest residual norm and perform a column swap. * This makes R's diagonal elements |R(0,0)| >= |R(1,1)| >= ..., enabling * numerical rank determination. * * @tparam T element type * @param a the m×n matrix to decompose * @return a tuple of (Q, R, perm) * - Q: m×m orthogonal matrix * - R: m×n upper triangular matrix * - perm: column permutation vector (perm[j] = original column index) * * Meaning of the permutation: the perm[j]-th column of A = the j-th column of Q*R * That is, A * P = Q * R (where P is a permutation matrix) */ template std::tuple, Matrix, std::vector::size_type>> qr_col_pivoting(const BaseMatrix& a) { using size_type = typename Matrix::size_type; const auto m = a.rows(); const auto n = a.cols(); const auto k = std::min(m, n); Matrix Q(m, m); Matrix R(a); // 1 copy: body modifies R in place // Initialize Q as the identity matrix for (size_type i = 0; i < m; ++i) for (size_type j = 0; j < m; ++j) Q(i, j) = (i == j) ? T(1) : T(0); // Permutation vector (initialized to the identity permutation) std::vector perm(n); for (size_type j = 0; j < n; ++j) perm[j] = j; // Precompute the squared residual norm of each column std::vector col_norms_sq(n, T(0)); for (size_type j = 0; j < n; ++j) { for (size_type i = 0; i < m; ++i) { col_norms_sq[j] += R(i, j) * R(i, j); } } for (size_type step = 0; step < k; ++step) { // Search for the column with the largest norm (from columns step onward) size_type max_col = step; T max_norm = col_norms_sq[step]; for (size_type j = step + 1; j < n; ++j) { if (col_norms_sq[j] > max_norm) { max_norm = col_norms_sq[j]; max_col = j; } } // Column swap if (max_col != step) { // Swap the columns of R for (size_type i = 0; i < m; ++i) { std::swap(R(i, step), R(i, max_col)); } // Swap the norms std::swap(col_norms_sq[step], col_norms_sq[max_col]); // Record the permutation std::swap(perm[step], perm[max_col]); } // Compute the Householder vector Vector x(m - step); for (size_type i = 0; i < m - step; ++i) { x[i] = R(step + i, step); } T x_norm = std::sqrt(dot(x, x)); if (x_norm < std::numeric_limits::epsilon()) { continue; } T alpha = (x[0] >= T(0)) ? -x_norm : x_norm; x[0] -= alpha; T x_norm_sq = dot(x, x); if (x_norm_sq < std::numeric_limits::epsilon()) { continue; } // Apply the Householder transformation to R for (size_type j = step; j < n; ++j) { T dp = T(0); for (size_type i = 0; i < m - step; ++i) { dp += x[i] * R(step + i, j); } T factor = (T(2) * dp) / x_norm_sq; for (size_type i = 0; i < m - step; ++i) { R(step + i, j) -= factor * x[i]; } } // Apply the Householder transformation to Q for (size_type j = 0; j < m; ++j) { T dp = T(0); for (size_type i = 0; i < m - step; ++i) { dp += x[i] * Q(step + i, j); } T factor = (T(2) * dp) / x_norm_sq; for (size_type i = 0; i < m - step; ++i) { Q(step + i, j) -= factor * x[i]; } } // Update the residual norms (columns step+1 onward) for (size_type j = step + 1; j < n; ++j) { col_norms_sq[j] -= R(step, j) * R(step, j); if (col_norms_sq[j] < T(0)) col_norms_sq[j] = T(0); } } // Transpose Q Matrix Qt(m, m); for (size_type i = 0; i < m; ++i) for (size_type j = 0; j < m; ++j) Qt(i, j) = Q(j, i); return { Qt, R, perm }; } /** * @brief Compute the numerical rank of a column-pivoted QR decomposition * * Determine the numerical rank from R's diagonal elements. * When |R(i,i)| > threshold, the i-th column is considered independent. * * @param R the upper triangular matrix obtained from qr_col_pivoting * @param threshold threshold (default: eps * max(m,n) * |R(0,0)|) * @return numerical rank */ template typename Matrix::size_type qr_rank( const Matrix& R, T threshold = T(-1)) { using size_type = typename Matrix::size_type; const auto k = std::min(R.rows(), R.cols()); if (k == 0) return 0; if (threshold < T(0)) { // Default threshold: eps * max(m,n) * |R(0,0)| T max_dim = T(std::max(R.rows(), R.cols())); threshold = std::numeric_limits::epsilon() * max_dim * std::abs(R(0, 0)); } size_type rank = 0; for (size_type i = 0; i < k; ++i) { if (std::abs(R(i, i)) > threshold) { ++rank; } } return rank; } /** * @brief Solve a system of linear equations using QR decomposition with column pivoting * * Solve A*x = b via column-pivoted QR decomposition. * For rank-deficient cases, solve only for the rank-many variables and set the rest to 0. * * @param a coefficient matrix (m×n) * @param b right-hand-side vector (m) * @return solution vector (n) */ template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> qr_col_pivoting_solve(const MA& a, const VB& b) { using T = sangi::element_t; SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); if (a.rows() != b.size()) { assert(false && "DimensionError: qr_col_pivoting_solve: dimensions mismatch"); throw DimensionError( "qr_col_pivoting_solve: matrix and vector dimensions mismatch"); } const auto m = a.rows(); const auto n = a.cols(); auto [Q, R, perm] = qr_col_pivoting(a); // Compute Q^T * b Vector Qtb(m, T(0)); for (typename Matrix::size_type i = 0; i < m; ++i) { for (typename Matrix::size_type j = 0; j < m; ++j) { Qtb[i] += Q(j, i) * b[j]; } } // Determine the numerical rank auto r = qr_rank(R); // Backward substitution (up to rank r) Vector y(n, T(0)); for (typename Matrix::size_type i = r; i-- > 0; ) { T sum = Qtb[i]; for (typename Matrix::size_type j = i + 1; j < r; ++j) { sum -= R(i, j) * y[j]; } y[i] = sum / R(i, i); } // Apply the inverse permutation to restore the original column order Vector x(n, T(0)); for (typename Matrix::size_type j = 0; j < n; ++j) { x[perm[j]] = y[j]; } return x; } // ==================================================================== // Tridiagonalization standalone API // ==================================================================== /** * @brief Structure storing the result of tridiagonalization * * Symmetric matrix A = Q * T * Q^T * T is a tridiagonal matrix (diagonal d, subdiagonal e) */ template struct TridiagonalResult { Matrix Q; ///< orthogonal matrix (n×n) Vector d; ///< diagonal elements (size n) Vector e; ///< subdiagonal elements (size n-1, e[k] = T(k, k+1) = T(k+1, k)) }; /** * @brief Tridiagonalization of a symmetric matrix by Householder transformation * * Transforms an n×n symmetric matrix A into a tridiagonal matrix T. * Returns Q (orthogonal matrix) and T (tridiagonal) satisfying A = Q * T * Q^T. * * Reference: Golub & Van Loan "Matrix Computations" 4th ed. §8.3 * * @param A symmetric matrix (n×n) * @return TridiagonalResult {Q, d, e} */ template requires sangi::BaseMatrixLike TridiagonalResult> tridiagonalize(const MA& A) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); const auto n = A.rows(); if (n != A.cols()) { assert(false && "DimensionError: tridiagonalize: matrix must be square"); throw DimensionError("tridiagonalize: matrix must be square"); } if (n == 0) { assert(false && "DimensionError: tridiagonalize: empty matrix"); throw DimensionError("tridiagonalize: empty matrix"); } // Symmetry check (T-aware, Float-aware) for (std::size_t i = 0; i < n; ++i) for (std::size_t j = i + 1; j < n; ++j) { T aij = A(i, j); T aji = A(j, i); T scale = std::max(std::abs(aij), T(1)); T diff = std::abs(aij - aji); T tol = scale * T(static_cast(n)) * std::numeric_limits::epsilon() + T(1) * std::numeric_limits::epsilon(); // small constant term if (diff > tol) throw MathError("tridiagonalize: matrix must be symmetric"); } // Working copy Matrix W(A); // Save the Householder vectors, coefficients, and the corresponding k struct HouseholderInfo { std::size_t k; Vector v; T beta; }; std::vector hh_list; // Householder tridiagonalization for (std::size_t k = 0; k + 2 <= n; ++k) { const std::size_t len = n - k - 1; // sigma = Σ W(k+2:n, k)² T sigma = T(0); for (std::size_t i = 1; i < len; ++i) sigma += W(k + 1 + i, k) * W(k + 1 + i, k); T x0 = W(k + 1, k); if (sigma == T(0) && x0 >= T(0)) { continue; // already in tridiagonal form } T alpha = std::sqrt(x0 * x0 + sigma); if (x0 > T(0)) alpha = -alpha; T v0 = x0 - alpha; T beta = T(2) / (v0 * v0 + sigma); // Save v Vector v(len); v[0] = v0; for (std::size_t i = 1; i < len; ++i) v[i] = W(k + 1 + i, k); hh_list.push_back({ k, v, beta }); // Symmetric Householder update: W ← (I - β vvᵀ) W (I - β vvᵀ) // Golub & Van Loan's efficient formula: // p = β * W(k+1:n, k+1:n) * v // K = β/2 * vᵀp // q = p - K * v // W(k+1:n, k+1:n) -= v*qᵀ + q*vᵀ Vector p(len, T(0)); for (std::size_t i = 0; i < len; ++i) { T dot = T(0); for (std::size_t j = 0; j < len; ++j) dot += W(k + 1 + i, k + 1 + j) * v[j]; p[i] = beta * dot; } T K = T(0); for (std::size_t i = 0; i < len; ++i) K += v[i] * p[i]; K *= beta / T(2); Vector q(len); for (std::size_t i = 0; i < len; ++i) q[i] = p[i] - K * v[i]; for (std::size_t i = 0; i < len; ++i) for (std::size_t j = 0; j < len; ++j) W(k + 1 + i, k + 1 + j) -= v[i] * q[j] + q[i] * v[j]; // Set the subdiagonal element W(k + 1, k) = alpha; W(k, k + 1) = alpha; } // Extract the diagonal and subdiagonal elements Vector d(n, T(0)); Vector e(n > 1 ? n - 1 : 0, T(0)); for (std::size_t i = 0; i < n; ++i) d[i] = W(i, i); for (std::size_t i = 0; i + 1 < n; ++i) e[i] = W(i, i + 1); // Backward accumulation of Q: Q = H_0 * H_1 * ... * H_{n-3} // Q = I, in reverse order Q ← (I - β v vᵀ) * Q // However, since each H_k acts on rows k+1:n: // Q(k+1:n, :) -= β * v * (vᵀ * Q(k+1:n, :)) Matrix Q = Matrix::identity(n); for (std::size_t ii = 0; ii < hh_list.size(); ++ii) { std::size_t idx = hh_list.size() - 1 - ii; auto& hh = hh_list[idx]; std::size_t k = hh.k; const auto& v = hh.v; T beta = hh.beta; std::size_t len = v.size(); // Q(k+1:k+1+len, :) -= β * v * (vᵀ * Q(k+1:k+1+len, :)) for (std::size_t j = 0; j < n; ++j) { T dot = T(0); for (std::size_t i = 0; i < len; ++i) dot += v[i] * Q(k + 1 + i, j); T factor = beta * dot; for (std::size_t i = 0; i < len; ++i) Q(k + 1 + i, j) -= factor * v[i]; } } return { std::move(Q), std::move(d), std::move(e) }; } /** * @brief Compact result of tridiagonalization (keeps the Householder vectors, does not form Q) * * Instead of forming Q explicitly in the back-transformation phase of eigen_symmetric, * applying the Householder vectors directly to Z saves O(n³). */ template struct TridiagonalCompact { struct Reflector { std::size_t k; Vector v; T beta; }; std::vector refs; Vector d; Vector e; }; /** * @brief Householder tridiagonalization (compact form) * * Does not form Q explicitly; returns the list of Householder vectors and coefficients. * Used to apply the Householder columns directly to Z in eigen_symmetric's back-transformation. * Applies the lower-triangular access optimization to SYMV. * * @param A symmetric matrix (n×n) — symmetry must be verified by the caller * @return TridiagonalCompact {refs, d, e} */ template requires sangi::BaseMatrixLike TridiagonalCompact> tridiagonalize_compact(const MA& A) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); const auto n = A.rows(); if (n == 0) { assert(false && "DimensionError: tridiagonalize_compact: empty matrix"); throw DimensionError("tridiagonalize_compact: empty matrix"); } Matrix W(A); TridiagonalCompact result; result.d = Vector(n, T(0)); result.e = Vector(n > 1 ? n - 1 : 0, T(0)); for (std::size_t k = 0; k + 2 <= n; ++k) { const std::size_t len = n - k - 1; T sigma = T(0); for (std::size_t i = 1; i < len; ++i) sigma += W(k + 1 + i, k) * W(k + 1 + i, k); T x0 = W(k + 1, k); if (sigma == T(0) && x0 >= T(0)) { result.refs.push_back({k, Vector(len, T(0)), T(0)}); continue; } T alpha = std::sqrt(x0 * x0 + sigma); if (x0 > T(0)) alpha = -alpha; T v0 = x0 - alpha; T beta = T(2) / (v0 * v0 + sigma); Vector v(len); v[0] = v0; for (std::size_t i = 1; i < len; ++i) v[i] = W(k + 1 + i, k); result.refs.push_back({k, v, beta}); // p = β * W(k+1:n, k+1:n) * v Vector p(len, T(0)); { T* __restrict wd = W.data(); const std::size_t wn = W.cols(); const T* __restrict vd = v.data(); T* __restrict pd = p.data(); for (std::size_t i = 0; i < len; ++i) { const T* __restrict row = wd + (k + 1 + i) * wn + (k + 1); T dot = T(0); for (std::size_t j = 0; j < len; ++j) dot += row[j] * vd[j]; pd[i] = beta * dot; } } T K = T(0); for (std::size_t i = 0; i < len; ++i) K += v[i] * p[i]; K *= beta / T(2); Vector q(len); for (std::size_t i = 0; i < len; ++i) q[i] = p[i] - K * v[i]; { T* __restrict wd = W.data(); const std::size_t wn = W.cols(); const T* __restrict vd = v.data(); const T* __restrict qd = q.data(); for (std::size_t i = 0; i < len; ++i) { T* __restrict row = wd + (k + 1 + i) * wn + (k + 1); const T vi = vd[i], qi = qd[i]; for (std::size_t j = 0; j < len; ++j) row[j] -= vi * qd[j] + qi * vd[j]; } } W(k + 1, k) = alpha; W(k, k + 1) = alpha; } for (std::size_t i = 0; i < n; ++i) result.d[i] = W(i, i); for (std::size_t i = 0; i + 1 < n; ++i) result.e[i] = W(i, i + 1); return result; } /** * @brief Apply a sequence of Householder reflections to matrix Z from the left * * Z ← Q * Z = H_0 * H_1 * ... * H_{m-1} * Z * H_k = I - β_k * v_k * v_kᵀ (acts on rows k+1:n) * * Apply from the inside: first H_{m-1}, then H_{m-2}, ..., last H_0 * Transforms Z directly without forming Q explicitly. * * Column-major loop order: keeping each column in the L1 cache while * applying all reflectors (effective when Z exceeds L2). */ template void apply_householder_sequence( const std::vector::Reflector>& refs, std::size_t n, T* __restrict z_col_major, std::size_t n_cols) { const std::size_t m = refs.size(); if (m == 0) return; // Pre-extract the reflector data into a flat buffer // (avoid the indirection overhead of Vector) struct FlatRef { std::size_t k; std::size_t len; T beta; const T* vd; }; std::vector flat(m); for (std::size_t ii = 0; ii < m; ++ii) { std::size_t idx = m - 1 - ii; const auto& hh = refs[idx]; flat[ii] = {hh.k, hh.v.size(), hh.beta, hh.v.data()}; } // Column blocking + 4-column unrolling: // Load v once and apply to 4 columns simultaneously (register reuse + ILP) constexpr std::size_t NB_Z = 64; for (std::size_t j_blk = 0; j_blk < n_cols; j_blk += NB_Z) { const std::size_t j_end = std::min(j_blk + NB_Z, n_cols); for (std::size_t ii = 0; ii < m; ++ii) { const auto& fr = flat[ii]; if (fr.beta == T(0)) continue; const T* __restrict vd = fr.vd; const std::size_t k1 = fr.k + 1; const std::size_t len = fr.len; const T bt = fr.beta; std::size_t j = j_blk; for (; j + 4 <= j_end; j += 4) { T* __restrict c0 = z_col_major + j * n; T* __restrict c1 = c0 + n; T* __restrict c2 = c1 + n; T* __restrict c3 = c2 + n; T d0 = T(0), d1 = T(0), d2 = T(0), d3 = T(0); for (std::size_t i = 0; i < len; ++i) { T vi = vd[i]; d0 += vi * c0[k1 + i]; d1 += vi * c1[k1 + i]; d2 += vi * c2[k1 + i]; d3 += vi * c3[k1 + i]; } d0 *= bt; d1 *= bt; d2 *= bt; d3 *= bt; for (std::size_t i = 0; i < len; ++i) { T vi = vd[i]; c0[k1 + i] -= d0 * vi; c1[k1 + i] -= d1 * vi; c2[k1 + i] -= d2 * vi; c3[k1 + i] -= d3 * vi; } } for (; j < j_end; ++j) { T* __restrict col = z_col_major + j * n; T dot = T(0); for (std::size_t i = 0; i < len; ++i) dot += vd[i] * col[k1 + i]; const T factor = bt * dot; for (std::size_t i = 0; i < len; ++i) col[k1 + i] -= factor * vd[i]; } } } } /** * @brief Utility to explicitly construct a tridiagonal matrix * * @param d diagonal elements (size n) * @param e subdiagonal elements (size n-1) * @return Matrix tridiagonal matrix (n×n) */ template Matrix build_tridiagonal_matrix(const BaseVector& d_basevec, const BaseVector& e_basevec) { Vector d(d_basevec); // 1 copy via BaseVector ctor Vector e(e_basevec); // 1 copy via BaseVector ctor const size_t n = d.size(); if (n > 1 && e.size() != n - 1) { assert(false && "DimensionError: build_tridiagonal_matrix: e.size() must equal d.size()-1"); throw DimensionError("build_tridiagonal_matrix: e.size() must equal d.size()-1"); } Matrix T_mat(n, n, T(0)); for (size_t i = 0; i < n; ++i) T_mat(i, i) = d[i]; for (size_t i = 0; i + 1 < n; ++i) { T_mat(i, i + 1) = e[i]; T_mat(i + 1, i) = e[i]; } return T_mat; } // Function computing the eigenvalue decomposition of a symmetric matrix (cyclic Jacobi method) // Computes all eigenvalues and eigenvectors via orthogonal rotations // Input: A (n×n symmetric matrix) // Output: { eigenvalues (ascending), eigenvectors (column vectors) } template requires concepts::MatrixOf std::tuple, Matrix> symmetric_eigen(const M& A) { using T = typename M::value_type; const size_t n = A.rows(); if (n == 0) return { Vector(0), Matrix(0, 0) }; if (n == 1) return { Vector{A(0, 0)}, Matrix::identity(1) }; const T eps = std::numeric_limits::epsilon(); const int max_sweep = 50; Matrix V(n, n, T(0)); for (size_t i = 0; i < n; ++i) V(i, i) = T(1); Matrix S(n, n); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) S(i, j) = A(i, j); for (int sweep = 0; sweep < max_sweep; ++sweep) { // Frobenius norm of the off-diagonal elements T off_norm = T(0); for (size_t i = 0; i < n; ++i) for (size_t j = i + 1; j < n; ++j) off_norm += S(i, j) * S(i, j); if (off_norm <= eps * eps) break; // Threshold (Rutishauser): skip small elements in the first 4 sweeps const T threshold = (sweep < 4) ? T(0.2) * off_norm / T(n * n) : T(0); // Cyclic Jacobi: scan all (p, q) pairs in row order for (size_t p = 0; p < n - 1; ++p) { for (size_t q = p + 1; q < n; ++q) { T apq = S(p, q); T g = std::abs(apq); // Skip if below the threshold if (g * g <= threshold) continue; // Also skip if already sufficiently small if (g <= eps * (std::abs(S(p, p)) + std::abs(S(q, q)))) { S(p, q) = S(q, p) = T(0); continue; } // Stable rotation-parameter computation (avoids atan2/sin/cos) T h = S(q, q) - S(p, p); T t; // t = s/c = tan(θ) if (std::abs(h) <= eps * g) { t = (apq >= T(0)) ? T(1) : T(-1); } else { T tau = h / (T(2) * apq); t = (tau >= T(0)) ? T(1) / (tau + std::sqrt(T(1) + tau * tau)) : T(-1) / (-tau + std::sqrt(T(1) + tau * tau)); } T c = T(1) / std::sqrt(T(1) + t * t); T s = t * c; T tau_gs = s / (T(1) + c); // Givens parameter // Update S: S = J^T S J (diagonal and matrix elements) T dp = S(p, p) - t * apq; T dq = S(q, q) + t * apq; S(p, p) = dp; S(q, q) = dq; S(p, q) = S(q, p) = T(0); for (size_t r = 0; r < n; ++r) { if (r == p || r == q) continue; T srp = S(r, p); T srq = S(r, q); S(r, p) = S(p, r) = srp - s * (srq + tau_gs * srp); S(r, q) = S(q, r) = srq + s * (srp - tau_gs * srq); } // Update the eigenvectors for (size_t r = 0; r < n; ++r) { T vrp = V(r, p); T vrq = V(r, q); V(r, p) = vrp - s * (vrq + tau_gs * vrp); V(r, q) = vrq + s * (vrp - tau_gs * vrq); } } } } // Extract the eigenvalues and sort in ascending order Vector eigenvalues(n); for (size_t i = 0; i < n; ++i) eigenvalues[i] = S(i, i); // Selection sort (eigenvectors are reordered simultaneously) for (size_t i = 0; i < n - 1; ++i) { size_t mi = i; for (size_t j = i + 1; j < n; ++j) if (eigenvalues[j] < eigenvalues[mi]) mi = j; if (mi != i) { std::swap(eigenvalues[i], eigenvalues[mi]); for (size_t r = 0; r < n; ++r) std::swap(V(r, i), V(r, mi)); } } return { eigenvalues, V }; } /** * @brief Compute the singular value decomposition (SVD) * @tparam T element type * @param a matrix to decompose * @param transV {true: return V^T, false: return V} * @return a tuple of {U, Σ, V^T} */ // --------------------------------------------------------------- // Golub-Kahan SVD (bidiagonalization + implicit-shift QR iteration) // // The old implementation used Jacobi on A^T*A, squaring the condition number, // degrading singular-value accuracy to sqrt(eps) ≈ 1e-8 even for double. // This implementation bidiagonalizes A directly via Householder transformation and // obtains the singular values via Golub-Kahan QR iteration. // Reference: Golub & Van Loan "Matrix Computations" 4th ed. §8.6 // --------------------------------------------------------------- namespace svd_detail { // Givens rotation: [c s; -s c]^T [a; b] = [r; 0] template void givens(T a, T b, T& c, T& s) { const T z = numeric_traits::zero(); const T o = numeric_traits::one(); if (b == z) { c = o; s = z; } else if (std::abs(b) > std::abs(a)) { T tau = -a / b; s = o / std::sqrt(o + tau * tau); c = s * tau; } else { T tau = -b / a; c = o / std::sqrt(o + tau * tau); s = c * tau; } } // ----- Symmetric tridiagonal QL iteration (base case of D&C) ----- // d[0..n-1] diagonal, e[0..n-1] subdiagonal (e[n-1]=0 sentinel) // Q[n×n] row-major: initialized to the identity by the caller // Output: d = eigenvalues (ascending), columns of Q = eigenvectors template void tridiag_ql_eig(T* __restrict d, T* __restrict e, size_t n, T* __restrict Q) { const T eps = std::numeric_limits::epsilon(); const int max_iter = 30 * static_cast(n); int total_iter = 0; for (size_t l = 0; l < n; ) { size_t mi = l; while (mi + 1 < n) { if (std::abs(e[mi]) <= eps * (std::abs(d[mi]) + std::abs(d[mi + 1]))) break; ++mi; } if (mi == l) { ++l; continue; } if (++total_iter > max_iter) break; T g = (d[l + 1] - d[l]) / (T(2) * e[l]); T r = std::sqrt(g * g + T(1)); g = d[mi] - d[l] + e[l] / (g + ((g >= T(0)) ? r : -r)); T s = T(1), c = T(1), p = T(0); bool lucky = false; for (size_t ii = 0; ii < mi - l; ++ii) { size_t i = mi - 1 - ii; T f = s * e[i]; T b = c * e[i]; if (std::abs(f) >= std::abs(g)) { c = g / f; r = std::sqrt(c * c + T(1)); e[i + 1] = f * r; s = T(1) / r; c *= s; } else { s = f / g; r = std::sqrt(s * s + T(1)); e[i + 1] = g * r; c = T(1) / r; s *= c; } if (e[i + 1] == T(0)) { d[i + 1] -= p; e[mi] = T(0); lucky = true; break; } g = d[i + 1] - p; r = (d[i] - g) * s + T(2) * c * b; p = s * r; d[i + 1] = g + p; g = c * r - b; for (size_t k = 0; k < n; ++k) { T qi = Q[k * n + i], qi1 = Q[k * n + i + 1]; Q[k * n + i + 1] = s * qi + c * qi1; Q[k * n + i] = c * qi - s * qi1; } } if (!lucky) { d[l] -= p; e[l] = g; e[mi] = T(0); } } // Sort ascending + reorder Q's columns for (size_t i = 0; i + 1 < n; ++i) { size_t mi = i; for (size_t j = i + 1; j < n; ++j) if (d[j] < d[mi]) mi = j; if (mi != i) { std::swap(d[i], d[mi]); for (size_t k = 0; k < n; ++k) std::swap(Q[k * n + i], Q[k * n + mi]); } } } // ----- Secular equation solver ----- // Solve f(λ) = 1 + ρ * Σ z2[i]/(d[i]-λ) = 0 within (lo, hi) // z2[i] = z[i]² (precomputed) template T secular_solve(const T* d, const T* z2, T rho, size_t n, T lo, T hi) { const T eps = std::numeric_limits::epsilon(); T lam = (lo + hi) / T(2); for (int iter = 0; iter < 100; ++iter) { if (hi - lo <= T(4) * eps * (std::abs(hi) + std::abs(lo) + T(1))) break; T f = T(1), fp = T(0); for (size_t i = 0; i < n; ++i) { T diff = d[i] - lam; if (std::abs(diff) < eps * eps) continue; T t = z2[i] / diff; f += rho * t; fp += rho * t / diff; } // Bracket update if (f * rho < T(0)) lo = lam; else hi = lam; if (std::abs(f) <= eps * T(n)) break; // Newton step (with bracket guard) if (std::abs(fp) > T(0)) { T next = lam - f / fp; lam = (next > lo && next < hi) ? next : (lo + hi) / T(2); } else { lam = (lo + hi) / T(2); } } return lam; } // ----- Symmetric tridiagonal Divide-and-Conquer eigenvalue decomposition ----- // d[0..n-1] diagonal (output: eigenvalues, ascending) // e[0..n-1] subdiagonal (e[n-1]=0 sentinel, destroyed) // Q[n×n] row-major: output is the eigenvector columns template void tridiag_dc_eig(T* d, T* e, size_t n, T* Q) { constexpr size_t DC_BASE = 25; if (n <= DC_BASE) { std::fill(Q, Q + n * n, T(0)); for (size_t i = 0; i < n; ++i) Q[i * n + i] = T(1); tridiag_ql_eig(d, e, n, Q); return; } const size_t k = n / 2, n2 = n - k; const T rho = e[k - 1]; d[k - 1] -= rho; d[k] -= rho; e[k - 1] = T(0); // Recursion: partial eigenvector matrices std::vector Q1(k * k), Q2(n2 * n2); tridiag_dc_eig(d, e, k, Q1.data()); tridiag_dc_eig(d + k, e + k, n2, Q2.data()); // z vector: last row of Q1 + first row of Q2 std::vector z(n); for (size_t j = 0; j < k; ++j) z[j] = Q1[(k - 1) * k + j]; for (size_t j = 0; j < n2; ++j) z[k + j] = Q2[j]; // Merge-sort d[0..k-1] and d[k..n-1] std::vector ds(n), zs(n); std::vector perm(n); { size_t a = 0, b = k, p = 0; while (a < k && b < n) { if (d[a] <= d[b]) { ds[p] = d[a]; zs[p] = z[a]; perm[p] = a; ++a; } else { ds[p] = d[b]; zs[p] = z[b]; perm[p] = b; ++b; } ++p; } while (a < k) { ds[p] = d[a]; zs[p] = z[a]; perm[p] = a; ++a; ++p; } while (b < n) { ds[p] = d[b]; zs[p] = z[b]; perm[p] = b; ++b; ++p; } } const T eps = std::numeric_limits::epsilon(); // Compute ||z|| and the maximum |d| T z_norm = T(0), d_max = T(0); for (size_t i = 0; i < n; ++i) { z_norm += zs[i] * zs[i]; d_max = std::max(d_max, std::abs(ds[i])); } z_norm = std::sqrt(z_norm); std::vector lam(n); std::vector W(n * n, T(0)); if (std::abs(rho) > eps && z_norm > eps) { // === Deflation (equivalent to LAPACK dlaed2) === const T tol = T(8) * eps * std::max(d_max, std::abs(rho) * z_norm); // deflated[i]: if true, the eigenvalue is ds[i] and the eigenvector is e_i std::vector deflated(n, false); size_t n_defl = 0; // Type 1: |z[i]| is small → deflation for (size_t i = 0; i < n; ++i) { if (std::abs(zs[i]) <= tol) { deflated[i] = true; ++n_defl; } } // Type 2: adjacent d[i] ≈ d[i+1] → eliminate z[i] via Givens for (size_t i = 0; i + 1 < n; ++i) { if (deflated[i]) continue; if (std::abs(ds[i + 1] - ds[i]) <= tol) { // Via Givens, z[i] → 0, z[i+1] → sqrt(z[i]²+z[i+1]²) T r = std::sqrt(zs[i] * zs[i] + zs[i + 1] * zs[i + 1]); if (r > T(0)) { zs[i] = T(0); zs[i + 1] = r; } deflated[i] = true; ++n_defl; } } // Indices of the non-deflated components const size_t n_active = n - n_defl; std::vector active_idx(n_active); { size_t p = 0; for (size_t i = 0; i < n; ++i) if (!deflated[i]) active_idx[p++] = i; } // Set the deflated eigenvalues and eigenvectors for (size_t i = 0; i < n; ++i) { if (deflated[i]) { lam[i] = ds[i]; W[i * n + i] = T(1); } } if (n_active > 0) { // Extract d, z of the non-deflated part std::vector da(n_active), za2(n_active), za(n_active); for (size_t j = 0; j < n_active; ++j) { da[j] = ds[active_idx[j]]; za[j] = zs[active_idx[j]]; za2[j] = za[j] * za[j]; } // Solve the secular equation (n_active eigenvalues) T za_norm2 = T(0); for (size_t j = 0; j < n_active; ++j) za_norm2 += za2[j]; std::vector la(n_active); for (size_t jj = 0; jj < n_active; ++jj) { T lo, hi; if (rho > T(0)) { lo = da[jj]; hi = (jj + 1 < n_active) ? da[jj + 1] : da[n_active - 1] + rho * za_norm2; } else { hi = da[jj]; lo = (jj > 0) ? da[jj - 1] : da[0] + rho * za_norm2; } la[jj] = secular_solve(da.data(), za2.data(), rho, n_active, lo, hi); lam[active_idx[jj]] = la[jj]; } // === Gu-Eisenstat z recomputation (equivalent to LAPACK dlaed3) === // z̃[i]² = Π_j (λ[j]-d[i]) / Π_{k≠i} (d[k]-d[i]) // Compute in log space to avoid overflow std::vector z_new(n_active); for (size_t i = 0; i < n_active; ++i) { T log_abs = T(0); int sign_prod = 1; for (size_t j = 0; j < n_active; ++j) { T v = la[j] - da[i]; if (v < T(0)) { sign_prod = -sign_prod; v = -v; } log_abs += std::log(std::max(v, std::numeric_limits::min())); } for (size_t j = 0; j < n_active; ++j) { if (j == i) continue; T v = da[j] - da[i]; if (v < T(0)) { sign_prod = -sign_prod; v = -v; } log_abs -= std::log(std::max(v, std::numeric_limits::min())); } // ratio = sign_prod * exp(log_abs), should be positive T abs_val = std::exp(log_abs / T(2)); z_new[i] = std::copysign(abs_val, za[i]); } // Eigenvectors: w[j][i] = z_new[i] / (d[i] - λ[j]) for (size_t jj = 0; jj < n_active; ++jj) { const size_t out_idx = active_idx[jj]; T norm2 = T(0); for (size_t ii = 0; ii < n_active; ++ii) { T diff = da[ii] - la[jj]; T wi = (std::abs(diff) > eps * eps) ? z_new[ii] / diff : T(0); W[active_idx[ii] * n + out_idx] = wi; norm2 += wi * wi; } if (norm2 > T(0)) { T inv = T(1) / std::sqrt(norm2); for (size_t ii = 0; ii < n_active; ++ii) W[active_idx[ii] * n + out_idx] *= inv; } } } // === Sort ascending (lam[]) === // The eigenvalue order may be disturbed after the secular equation/deflation // → sort and also swap W's columns std::vector sort_idx(n); std::iota(sort_idx.begin(), sort_idx.end(), 0); std::sort(sort_idx.begin(), sort_idx.end(), [&](size_t a, size_t b) { return lam[a] < lam[b]; }); std::vector lam_sorted(n); std::vector W_sorted(n * n, T(0)); for (size_t j = 0; j < n; ++j) { lam_sorted[j] = lam[sort_idx[j]]; for (size_t i = 0; i < n; ++i) W_sorted[i * n + j] = W[i * n + sort_idx[j]]; } lam = std::move(lam_sorted); W = std::move(W_sorted); } else { // ρ ≈ 0: merge only for (size_t i = 0; i < n; ++i) { lam[i] = ds[i]; W[i * n + i] = T(1); } } // Restore W's rows to the original order: WU[perm[i], :] = W[i, :] std::vector WU(n * n, T(0)); for (size_t i = 0; i < n; ++i) { const size_t oi = perm[i]; for (size_t j = 0; j < n; ++j) WU[oi * n + j] = W[i * n + j]; } // Output the eigenvalues for (size_t i = 0; i < n; ++i) d[i] = lam[i]; // Merge: Q = [Q1 0; 0 Q2] * WU (gemm) std::fill(Q, Q + n * n, T(0)); DefaultComputePolicy::gemm( Q1.data(), WU.data(), Q, k, n, k, k, n, n); DefaultComputePolicy::gemm( Q2.data(), WU.data() + k * n, Q + k * n, n2, n, n2, n2, n, n); } // ----- Golub-Kahan QR iteration (bidiagonal SVD) ----- // d[0..n-1] diagonal elements (in/out: singular values) // e[0..n-1] superdiagonal elements (e[0]=0, destroyed) // Uc[n*n], Vc[n*n] column-major: assumed initialized to the identity template void bidiag_qr_svd(T* d, T* e, size_t n, T* Uc, T* Vc) { const T eps = std::numeric_limits::epsilon(); const int max_iter = 100 * static_cast(n); // Correct negative diagonal elements to positive for (size_t i = 0; i < n; ++i) { if (d[i] < T(0)) { d[i] = -d[i]; if (i + 1 < n) e[i + 1] = -e[i + 1]; T* col = &Uc[i * n]; for (size_t j = 0; j < n; ++j) col[j] = -col[j]; } } // Column-major Givens helper (contiguous memory access) auto givens_cols = [&](T* __restrict mat, size_t dim, size_t c1, size_t c2, T c, T s) { T* __restrict col1 = mat + c1 * dim; T* __restrict col2 = mat + c2 * dim; size_t i = 0; #ifdef __AVX2__ if constexpr (std::is_same_v) { __m256d vc = _mm256_set1_pd(c); __m256d vs = _mm256_set1_pd(s); for (; i + 4 <= dim; i += 4) { __m256d a = _mm256_loadu_pd(col1 + i); __m256d b = _mm256_loadu_pd(col2 + i); _mm256_storeu_pd(col1 + i, _mm256_add_pd(_mm256_mul_pd(vc, a), _mm256_mul_pd(vs, b))); _mm256_storeu_pd(col2 + i, _mm256_sub_pd(_mm256_mul_pd(vc, b), _mm256_mul_pd(vs, a))); } } #endif for (; i < dim; ++i) { T a = col1[i], b = col2[i]; col1[i] = c * a + s * b; col2[i] = c * b - s * a; } }; for (int iter = 0; iter < max_iter; ++iter) { for (size_t i = 1; i < n; ++i) { if (std::abs(e[i]) <= eps * (std::abs(d[i - 1]) + std::abs(d[i]))) e[i] = T(0); } size_t q = n; while (q > 1 && e[q - 1] == T(0)) --q; if (q <= 1) break; size_t p = q - 1; while (p > 0 && e[p] != T(0)) --p; // Deflation of zero diagonal elements bool handled_zero = false; for (size_t i = p; i < q; ++i) { T ref = T(0); if (i + 1 < n && e[i + 1] != T(0)) ref = std::max(ref, std::abs(e[i + 1])); if (i > 0 && e[i] != T(0)) ref = std::max(ref, std::abs(e[i])); if (ref == T(0)) continue; if (std::abs(d[i]) > eps * ref) continue; d[i] = T(0); handled_zero = true; if (i + 1 < q && e[i + 1] != T(0)) { T bulge = e[i + 1]; e[i + 1] = T(0); for (size_t j = i + 1; j < q; ++j) { T c, s; givens(d[j], bulge, c, s); d[j] = c * d[j] - s * bulge; if (j + 1 < q) { bulge = s * e[j + 1]; e[j + 1] = c * e[j + 1]; } givens_cols(Uc, n, j, i, c, -s); if (j + 1 >= q || std::abs(bulge) <= eps * std::abs(d[j])) break; } } else if (i > p && e[i] != T(0)) { T bulge = e[i]; e[i] = T(0); for (size_t jj = 0; jj < i - p; ++jj) { size_t j = i - 1 - jj; T c, s; givens(d[j], bulge, c, s); d[j] = c * d[j] - s * bulge; if (j > p) { bulge = s * e[j]; e[j] = c * e[j]; } givens_cols(Vc, n, j, i, c, -s); if (j <= p || std::abs(bulge) <= eps * std::abs(d[j])) break; } } break; } if (handled_zero) continue; // Wilkinson shift T d_qm1 = d[q - 1], d_qm2 = d[q - 2]; T e_qm1 = e[q - 1]; T e_qm2 = (q >= 3 && q - 2 > p) ? e[q - 2] : T(0); T t11 = d_qm2 * d_qm2 + e_qm2 * e_qm2; T t12 = d_qm2 * e_qm1; T t22 = d_qm1 * d_qm1 + e_qm1 * e_qm1; T delta = (t11 - t22) / T(2); T mu = t22 - t12 * t12 / (delta + (delta >= T(0) ? T(1) : T(-1)) * std::sqrt(delta * delta + t12 * t12)); T x = d[p] * d[p] - mu; T z = d[p] * e[p + 1]; for (size_t k = p; k < q - 1; ++k) { T c, s; givens(x, z, c, s); if (k > p) e[k] = c * e[k] - s * z; T dk = d[k], dk1 = d[k + 1], ek1 = e[k + 1]; d[k] = c * dk - s * ek1; e[k + 1] = s * dk + c * ek1; T bulge = -s * dk1; d[k + 1] = c * dk1; givens_cols(Vc, n, k, k + 1, c, -s); x = d[k]; z = bulge; givens(x, z, c, s); d[k] = c * x - s * z; T ek1_new = e[k + 1]; e[k + 1] = c * ek1_new - s * d[k + 1]; d[k + 1] = s * ek1_new + c * d[k + 1]; if (k + 2 < q) { z = -s * e[k + 2]; e[k + 2] = c * e[k + 2]; } givens_cols(Uc, n, k, k + 1, c, -s); x = e[k + 1]; } } } } // namespace svd_detail template requires concepts::MatrixOf std::tuple, Vector, Matrix> svd_decomposition(const M& A, bool transV = true) { using T = typename M::value_type; // If called with something other than Matrix (e.g. StaticMatrix), materialize and // delegate to the Matrix path (avoiding dependencies such as .transpose()). if constexpr (!std::is_same_v>) { Matrix A_dyn(A.rows(), A.cols()); for (std::size_t i = 0; i < A.rows(); ++i) for (std::size_t j = 0; j < A.cols(); ++j) A_dyn(i, j) = A(i, j); return svd_decomposition(A_dyn, transV); } else { const size_t m = A.rows(); const size_t n = A.cols(); const size_t min_mn = std::min(m, n); if (min_mn == 0) { return { Matrix(m, min_mn), Vector(0), Matrix(n, n) }; } #if SANGI_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { // thin U + full V^T: // U: m × min(m,n) (thin, expected shape of test BDCSVD.Tall350x250) // Vt: n × n (full, needed for padeHermite's null-space extraction) // Even for wide (m < n), LAPACK can handle it directly, so transpose recursion is unnecessary. // The old m Acopy = A; Vector s(min_mn); Matrix U(m, min_mn, T(0)); Matrix Vt(n, n, T(0)); lapack_int info; // For large matrices, divide-and-conquer SVD (gesdd) is faster than Golub-Reinsch (gesvd). // gesdd has no jobu/jobvt and is controlled by a single jobz, so to obtain the desired // "thin U + full Vt", split into cases as follows: // m >= n: jobz='S' → U is m×min(=m×n, thin), Vt is min×n(=n×n, full) // m < n: jobz='A' → U is m×m(min=m so coincides with thin), Vt is n×n(full) // In both cases U=m×min_mn(ldu=min_mn), Vt=n×n(ldvt=n), returning the same shape // as the gesvd path. constexpr size_t GESDD_CUTOFF = 1000; const bool use_dc = (min_mn >= GESDD_CUTOFF); if (use_dc) { const char jobz = (m >= n) ? 'S' : 'A'; if constexpr (std::is_same_v) info = LAPACKE_sgesdd(LAPACK_ROW_MAJOR, jobz, (lapack_int)m, (lapack_int)n, Acopy.data(), (lapack_int)n, s.data(), U.data(), (lapack_int)min_mn, Vt.data(), (lapack_int)n); else info = LAPACKE_dgesdd(LAPACK_ROW_MAJOR, jobz, (lapack_int)m, (lapack_int)n, Acopy.data(), (lapack_int)n, s.data(), U.data(), (lapack_int)min_mn, Vt.data(), (lapack_int)n); if (info > 0) throw MathError("svd_decomposition: LAPACKE_gesdd did not converge"); } else { // jobu='S', jobvt='A' std::vector superb(min_mn > 0 ? min_mn - 1 : 0); if constexpr (std::is_same_v) info = LAPACKE_sgesvd(LAPACK_ROW_MAJOR, 'S', 'A', (lapack_int)m, (lapack_int)n, Acopy.data(), (lapack_int)n, s.data(), U.data(), (lapack_int)min_mn, Vt.data(), (lapack_int)n, superb.data()); else info = LAPACKE_dgesvd(LAPACK_ROW_MAJOR, 'S', 'A', (lapack_int)m, (lapack_int)n, Acopy.data(), (lapack_int)n, s.data(), U.data(), (lapack_int)min_mn, Vt.data(), (lapack_int)n, superb.data()); if (info > 0) throw MathError("svd_decomposition: LAPACKE_gesvd did not converge"); } if (transV) return { U, s, Vt }; else return { U, s, Vt.transpose() }; } #endif // No MKL: old m < n recursion path (without LAPACK there is no direct way to compute // thin U + full V^T, so compute via transpose) if (m < n) { auto [V, s, Ut] = svd_decomposition(A.transpose(), true); if (transV) return { Ut.transpose(), s, V.transpose() }; else return { Ut.transpose(), s, V }; } // =============================================================== // float/double fast path: lazy accumulation + column-major QR + WY back-transformation // =============================================================== if constexpr (std::is_same_v || std::is_same_v) { Matrix W(A); std::vector d(n, T(0)); std::vector e(n, T(0)); std::vector tau_left(n, T(0)); std::vector tau_right(n, T(0)); // Phase 1: Householder bidiagonalization // Left reflector: normalized vector in W(k+1:m, k), tau_left[k] // Right reflector: normalized vector in W(k, k+2:n), tau_right[k] #ifdef SANGI_SVD_PHASE_TIMING auto _svd_t0 = std::chrono::high_resolution_clock::now(); #endif T* __restrict wdata = W.data(); std::vector _hh_work(n); for (size_t k = 0; k < n; ++k) { // --- Left Householder --- { T sigma = T(0); for (size_t i = k + 1; i < m; ++i) sigma += wdata[i * n + k] * wdata[i * n + k]; if (sigma == T(0) && wdata[k * n + k] >= T(0)) { tau_left[k] = T(0); d[k] = wdata[k * n + k]; } else { T x0 = wdata[k * n + k]; T x_norm = std::sqrt(x0 * x0 + sigma); T alpha = (x0 >= T(0)) ? -x_norm : x_norm; T v0 = x0 - alpha; tau_left[k] = T(2) * v0 * v0 / (v0 * v0 + sigma); T inv_v0 = T(1) / v0; for (size_t i = k + 1; i < m; ++i) wdata[i * n + k] *= inv_v0; // Apply to W(k:m, k+1:n): H = I - tau * v * v^T T tau_k = tau_left[k]; const size_t ncols = n - k - 1; { T* __restrict w = _hh_work.data(); for (size_t c = 0; c < ncols; ++c) w[c] = wdata[k * n + (k + 1 + c)]; for (size_t i = k + 1; i < m; ++i) { T vi = wdata[i * n + k]; computation::simd::axpy_simd(w, vi, &wdata[i * n + k + 1], ncols); } for (size_t c = 0; c < ncols; ++c) w[c] *= tau_k; for (size_t c = 0; c < ncols; ++c) wdata[k * n + (k + 1 + c)] -= w[c]; for (size_t i = k + 1; i < m; ++i) { T vi = wdata[i * n + k]; computation::simd::axpy_simd(&wdata[i * n + k + 1], -vi, w, ncols); } } d[k] = alpha; wdata[k * n + k] = alpha; } } // --- Right Householder (SIMD dot product) --- if (k + 2 <= n - 1) { T sigma = T(0); for (size_t jj = k + 2; jj < n; ++jj) sigma += wdata[k * n + jj] * wdata[k * n + jj]; if (sigma == T(0) && wdata[k * n + k + 1] >= T(0)) { tau_right[k] = T(0); e[k + 1] = wdata[k * n + k + 1]; } else { T x0 = wdata[k * n + k + 1]; T x_norm = std::sqrt(x0 * x0 + sigma); T alpha = (x0 >= T(0)) ? -x_norm : x_norm; T v0 = x0 - alpha; tau_right[k] = T(2) * v0 * v0 / (v0 * v0 + sigma); T inv_v0 = T(1) / v0; for (size_t jj = k + 2; jj < n; ++jj) wdata[k * n + jj] *= inv_v0; // W(k+1:m, k+1:n) -= tau * (A*v) * v^T (SIMD dot + axpy) T tau_k = tau_right[k]; const size_t rcols = n - k - 2; const T* __restrict vrow = &wdata[k * n + k + 2]; using PT = PacketTraits; constexpr size_t PS = PT::size; for (size_t i = k + 1; i < m; ++i) { T* __restrict row_i = &wdata[i * n + k + 1]; // SIMD dot product: row_i[0] + sum(vrow[c]*row_i[1+c]) T dot = row_i[0]; // v[0]=1 if constexpr (PS > 1) { auto acc = PT::set1(T(0)); size_t c = 0; for (; c + PS <= rcols; c += PS) acc = PT::fmadd(PT::load(vrow + c), PT::load(row_i + 1 + c), acc); dot += PT::reduce_add(acc); for (; c < rcols; ++c) dot += vrow[c] * row_i[1 + c]; } else { for (size_t c = 0; c < rcols; ++c) dot += vrow[c] * row_i[1 + c]; } T factor = tau_k * dot; row_i[0] -= factor; if (rcols > 0) computation::simd::axpy_simd(row_i + 1, -factor, vrow, rcols); } e[k + 1] = alpha; } } else if (k + 1 < n) { e[k + 1] = W(k, k + 1); } } #ifdef SANGI_SVD_PHASE_TIMING auto _svd_t1 = std::chrono::high_resolution_clock::now(); #endif // Phase 2: bidiagonal → singular values and singular vectors const T eps = std::numeric_limits::epsilon(); // Column-major n×n matrices (used in Phase 3) std::vector Uc(n * n, T(0)); // U_bidiag column-major std::vector Vc(n * n, T(0)); // V_bidiag column-major for (size_t i = 0; i < n; ++i) { Uc[i * n + i] = T(1); Vc[i * n + i] = T(1); } constexpr size_t DC_SVD_CUTOFF = 200; if (n > DC_SVD_CUTOFF) { // ===== BDCSVD: B^T*B → symmetric tridiagonal D&C ===== // Save the original bidiagonal elements std::vector bd(d.begin(), d.begin() + n); std::vector be(e.begin(), e.begin() + n); // T = B^T*B: symmetric tridiagonal std::vector td(n), te(n, T(0)); for (size_t i = 0; i < n; ++i) td[i] = bd[i] * bd[i] + be[i] * be[i]; // be[0]=0 for (size_t i = 0; i + 1 < n; ++i) te[i] = bd[i] * be[i + 1]; // D&C eigenvalue decomposition: td → eigenvalues (ascending), Q_eig → eigenvectors std::vector Q_eig(n * n); svd_detail::tridiag_dc_eig(td.data(), te.data(), n, Q_eig.data()); // Singular values: σ = sqrt(λ) for (size_t i = 0; i < n; ++i) d[i] = std::sqrt(std::max(td[i], T(0))); // V = Q_eig → Vc (column-major), U = B*V*Σ^{-1} → Uc (column-major) for (size_t j = 0; j < n; ++j) { T sigma = d[j]; // V → Vc for (size_t i = 0; i < n; ++i) Vc[j * n + i] = Q_eig[i * n + j]; // U = B*V*Σ^{-1}: (B*v)_i = bd[i]*v_{i,j} + be[i+1]*v_{i+1,j} if (sigma > eps) { T inv_sigma = T(1) / sigma; for (size_t i = 0; i < n; ++i) { T bv = bd[i] * Q_eig[i * n + j]; if (i + 1 < n) bv += be[i + 1] * Q_eig[(i + 1) * n + j]; Uc[j * n + i] = bv * inv_sigma; } } // For σ ≈ 0, column j of Uc remains 0 (initialized from identity) } } else { // ===== Golub-Kahan QR iteration ===== const int max_iter = 100 * static_cast(n); // Correct negative diagonal elements to positive for (size_t i = 0; i < n; ++i) { if (d[i] < T(0)) { d[i] = -d[i]; if (i + 1 < n) e[i + 1] = -e[i + 1]; // Negate column i of Uc T* col = &Uc[i * n]; for (size_t j = 0; j < n; ++j) col[j] = -col[j]; } } // Column-major Givens helper (contiguous memory access) auto givens_cols = [&](T* __restrict mat, size_t dim, size_t c1, size_t c2, T c, T s) { T* __restrict col1 = mat + c1 * dim; T* __restrict col2 = mat + c2 * dim; size_t i = 0; #ifdef __AVX2__ if constexpr (std::is_same_v) { __m256d vc = _mm256_set1_pd(c); __m256d vs = _mm256_set1_pd(s); for (; i + 4 <= dim; i += 4) { __m256d a = _mm256_loadu_pd(col1 + i); __m256d b = _mm256_loadu_pd(col2 + i); _mm256_storeu_pd(col1 + i, _mm256_add_pd(_mm256_mul_pd(vc, a), _mm256_mul_pd(vs, b))); _mm256_storeu_pd(col2 + i, _mm256_sub_pd(_mm256_mul_pd(vc, b), _mm256_mul_pd(vs, a))); } } #endif for (; i < dim; ++i) { T a = col1[i], b = col2[i]; col1[i] = c * a + s * b; col2[i] = c * b - s * a; } }; for (int iter = 0; iter < max_iter; ++iter) { // Convergence test for (size_t i = 1; i < n; ++i) { if (std::abs(e[i]) <= eps * (std::abs(d[i - 1]) + std::abs(d[i]))) e[i] = T(0); } size_t q = n; while (q > 1 && e[q - 1] == T(0)) --q; if (q <= 1) break; size_t p = q - 1; while (p > 0 && e[p] != T(0)) --p; // Deflation of zero diagonal elements bool handled_zero = false; for (size_t i = p; i < q; ++i) { T ref = T(0); if (i + 1 < n && e[i + 1] != T(0)) ref = std::max(ref, std::abs(e[i + 1])); if (i > 0 && e[i] != T(0)) ref = std::max(ref, std::abs(e[i])); if (ref == T(0)) continue; if (std::abs(d[i]) > eps * ref) continue; d[i] = T(0); handled_zero = true; if (i + 1 < q && e[i + 1] != T(0)) { T bulge = e[i + 1]; e[i + 1] = T(0); for (size_t j = i + 1; j < q; ++j) { T c, s; svd_detail::givens(d[j], bulge, c, s); d[j] = c * d[j] - s * bulge; if (j + 1 < q) { bulge = s * e[j + 1]; e[j + 1] = c * e[j + 1]; } givens_cols(Uc.data(), n, j, i, c, -s); if (j + 1 >= q || std::abs(bulge) <= eps * std::abs(d[j])) break; } } else if (i > p && e[i] != T(0)) { T bulge = e[i]; e[i] = T(0); for (size_t jj = 0; jj < i - p; ++jj) { size_t j = i - 1 - jj; T c, s; svd_detail::givens(d[j], bulge, c, s); d[j] = c * d[j] - s * bulge; if (j > p) { bulge = s * e[j]; e[j] = c * e[j]; } givens_cols(Vc.data(), n, j, i, c, -s); if (j <= p || std::abs(bulge) <= eps * std::abs(d[j])) break; } } break; } if (handled_zero) continue; // Wilkinson shift T d_qm1 = d[q - 1], d_qm2 = d[q - 2]; T e_qm1 = e[q - 1]; T e_qm2 = (q >= 3 && q - 2 > p) ? e[q - 2] : T(0); T t11 = d_qm2 * d_qm2 + e_qm2 * e_qm2; T t12 = d_qm2 * e_qm1; T t22 = d_qm1 * d_qm1 + e_qm1 * e_qm1; T delta = (t11 - t22) / T(2); T mu = t22 - t12 * t12 / (delta + (delta >= T(0) ? T(1) : T(-1)) * std::sqrt(delta * delta + t12 * t12)); T x = d[p] * d[p] - mu; T z = d[p] * e[p + 1]; for (size_t k = p; k < q - 1; ++k) { // Right Givens (columns k, k+1) T c, s; svd_detail::givens(x, z, c, s); if (k > p) e[k] = c * e[k] - s * z; T dk = d[k], dk1 = d[k + 1], ek1 = e[k + 1]; d[k] = c * dk - s * ek1; e[k + 1] = s * dk + c * ek1; T bulge = -s * dk1; d[k + 1] = c * dk1; givens_cols(Vc.data(), n, k, k + 1, c, -s); // Left Givens (rows k, k+1) x = d[k]; z = bulge; svd_detail::givens(x, z, c, s); d[k] = c * x - s * z; T ek1_new = e[k + 1]; e[k + 1] = c * ek1_new - s * d[k + 1]; d[k + 1] = s * ek1_new + c * d[k + 1]; if (k + 2 < q) { z = -s * e[k + 2]; e[k + 2] = c * e[k + 2]; } givens_cols(Uc.data(), n, k, k + 1, c, -s); x = e[k + 1]; } } } // end else (QR iteration) #ifdef SANGI_SVD_PHASE_TIMING auto _svd_t2 = std::chrono::high_resolution_clock::now(); #endif // Phase 3: back-transformation // Negative singular values to positive for (size_t i = 0; i < n; ++i) { if (d[i] < T(0)) { d[i] = -d[i]; T* col = &Uc[i * n]; for (size_t j = 0; j < n; ++j) col[j] = -col[j]; } } // Sort descending (column-major, so swap columns) for (size_t i = 0; i < n - 1; ++i) { size_t max_idx = i; for (size_t j = i + 1; j < n; ++j) if (d[j] > d[max_idx]) max_idx = j; if (max_idx != i) { std::swap(d[i], d[max_idx]); // Swap columns of Uc, Vc T* u1 = &Uc[i * n], *u2 = &Uc[max_idx * n]; T* v1 = &Vc[i * n], *v2 = &Vc[max_idx * n]; for (size_t j = 0; j < n; ++j) { std::swap(u1[j], u2[j]); std::swap(v1[j], v2[j]); } } } // Convert Uc, Vc to row-major → n×n U_bidiag, V_bidiag Matrix Ub(n, n); Matrix Vb(n, n); for (size_t i = 0; i < n; ++i) for (size_t j = 0; j < n; ++j) { Ub(i, j) = Uc[j * n + i]; // column-major→row-major Vb(i, j) = Vc[j * n + i]; } // U = Q_left * Ub: apply the left Householder reflectors to Ub in reverse order to expand to m×n // result (m×n): first n rows = Ub, remaining m-n rows = 0 Matrix U_out(m, n, T(0)); for (size_t i = 0; i < n; ++i) for (size_t j = 0; j < n; ++j) U_out(i, j) = Ub(i, j); // Apply the left reflectors via WY blocks in reverse order: H_{n-1}, ..., H_0 // Q = H_0*H_1*...*H_{n-1}, applied from the right by block { T* __restrict udata = U_out.data(); constexpr size_t NB_BT = 32; size_t k_end = n; while (k_end > 0) { const size_t nb = std::min(NB_BT, k_end); const size_t k_start = k_end - nb; const size_t pr = m - k_start; const size_t cols = n; // T factor (nb × nb upper triangular) std::vector T_fac(nb * nb, T(0)); for (size_t jj = 0; jj < nb; ++jj) { const size_t kj = k_start + jj; T_fac[jj * nb + jj] = tau_left[kj]; if (tau_left[kj] == T(0)) continue; for (size_t kk = 0; kk < jj; ++kk) { const size_t kc = k_start + kk; T dot = W(kj, kc); for (size_t i = jj + 1; i < pr; ++i) dot += W(k_start + i, kc) * W(k_start + i, kj); T_fac[kk * nb + jj] = dot; } for (size_t kk = 0; kk < jj; ++kk) { T sum = T(0); for (size_t ll = kk; ll < jj; ++ll) sum += T_fac[kk * nb + ll] * T_fac[ll * nb + jj]; T_fac[kk * nb + jj] = -tau_left[kj] * sum; } } // V^T (nb × pr) and V (pr × nb), row-major std::vector VT_rm(nb * pr, T(0)); std::vector V_rm(pr * nb, T(0)); for (size_t jj = 0; jj < nb; ++jj) { VT_rm[jj * pr + jj] = T(1); V_rm[jj * nb + jj] = T(1); for (size_t i = jj + 1; i < pr; ++i) { T val = W(k_start + i, k_start + jj); VT_rm[jj * pr + i] = val; V_rm[i * nb + jj] = val; } } // (1) Wbuf = V^T * U_sub (nb × cols) std::vector Wbuf(nb * cols, T(0)); DefaultComputePolicy::gemm( VT_rm.data(), udata + k_start * cols, Wbuf.data(), nb, cols, pr, pr, cols, cols); // (2) Wbuf = T * Wbuf (upper triangular, top to bottom) for (size_t j = 0; j < nb; ++j) { T t_jj = T_fac[j * nb + j]; T* w_row = &Wbuf[j * cols]; if (t_jj != T(1)) { for (size_t c = 0; c < cols; ++c) w_row[c] *= t_jj; } for (size_t l = j + 1; l < nb; ++l) { T t_jl = T_fac[j * nb + l]; if (t_jl == T(0)) continue; computation::simd::axpy_simd( w_row, t_jl, &Wbuf[l * cols], cols); } } // (3) U_sub -= V * Wbuf for (size_t i = 0; i < nb * cols; ++i) Wbuf[i] = -Wbuf[i]; DefaultComputePolicy::gemm( V_rm.data(), Wbuf.data(), udata + k_start * cols, pr, cols, nb, nb, cols, cols); k_end = k_start; } } // Apply the right reflectors via WY blocks in reverse order: G_{n-3}, ..., G_0 { T* __restrict vdata = Vb.data(); constexpr size_t NB_BT = 32; if (n >= 3) { const size_t num_ref = n - 2; size_t k_end = num_ref; while (k_end > 0) { const size_t nb = std::min(NB_BT, k_end); const size_t k_start = k_end - nb; const size_t start_row = k_start + 1; const size_t pr = n - start_row; const size_t cols = n; // T factor (nb × nb upper triangular) std::vector T_fac(nb * nb, T(0)); for (size_t jj = 0; jj < nb; ++jj) { const size_t kj = k_start + jj; T_fac[jj * nb + jj] = tau_right[kj]; if (tau_right[kj] == T(0)) continue; for (size_t kk = 0; kk < jj; ++kk) { const size_t kc = k_start + kk; T dot = W(kc, start_row + jj); for (size_t i = jj + 1; i < pr; ++i) dot += W(kc, start_row + i) * W(kj, start_row + i); T_fac[kk * nb + jj] = dot; } for (size_t kk = 0; kk < jj; ++kk) { T sum = T(0); for (size_t ll = kk; ll < jj; ++ll) sum += T_fac[kk * nb + ll] * T_fac[ll * nb + jj]; T_fac[kk * nb + jj] = -tau_right[kj] * sum; } } // V^T (nb × pr) and V (pr × nb) std::vector VT_rm(nb * pr, T(0)); std::vector V_rm(pr * nb, T(0)); for (size_t jj = 0; jj < nb; ++jj) { const size_t kj = k_start + jj; VT_rm[jj * pr + jj] = T(1); V_rm[jj * nb + jj] = T(1); for (size_t i = jj + 1; i < pr; ++i) { T val = W(kj, start_row + i); VT_rm[jj * pr + i] = val; V_rm[i * nb + jj] = val; } } // (1) Wbuf = V^T * Vb_sub (nb × cols) std::vector Wbuf(nb * cols, T(0)); DefaultComputePolicy::gemm( VT_rm.data(), vdata + start_row * cols, Wbuf.data(), nb, cols, pr, pr, cols, cols); // (2) Wbuf = T * Wbuf (upper triangular, top to bottom) for (size_t j = 0; j < nb; ++j) { T t_jj = T_fac[j * nb + j]; T* w_row = &Wbuf[j * cols]; if (t_jj != T(1)) { for (size_t c = 0; c < cols; ++c) w_row[c] *= t_jj; } for (size_t l = j + 1; l < nb; ++l) { T t_jl = T_fac[j * nb + l]; if (t_jl == T(0)) continue; computation::simd::axpy_simd( w_row, t_jl, &Wbuf[l * cols], cols); } } // (3) Vb_sub -= V * Wbuf for (size_t i = 0; i < nb * cols; ++i) Wbuf[i] = -Wbuf[i]; DefaultComputePolicy::gemm( V_rm.data(), Wbuf.data(), vdata + start_row * cols, pr, cols, nb, nb, cols, cols); k_end = k_start; } } } #ifdef SANGI_SVD_PHASE_TIMING auto _svd_t3 = std::chrono::high_resolution_clock::now(); std::fprintf(stderr, "SVD n=%zu: Phase1=%.1fms Phase2=%.1fms Phase3=%.1fms\n", n, std::chrono::duration(_svd_t1 - _svd_t0).count(), std::chrono::duration(_svd_t2 - _svd_t1).count(), std::chrono::duration(_svd_t3 - _svd_t2).count()); #endif // Build the result Vector sigma(min_mn); for (size_t i = 0; i < min_mn; ++i) sigma[i] = d[i]; if (transV) { Matrix Vt(min_mn, n); for (size_t i = 0; i < min_mn; ++i) for (size_t j = 0; j < n; ++j) Vt(i, j) = Vb(j, i); return { U_out, sigma, Vt }; } else { Matrix V_out(n, min_mn); for (size_t i = 0; i < n; ++i) for (size_t j = 0; j < min_mn; ++j) V_out(i, j) = Vb(i, j); return { U_out, sigma, V_out }; } } // end float/double fast path // =============================================================== // Generic path (non float/double) — Complex-aware // =============================================================== // Real type: the underlying real type for Complex, otherwise T using real_t = decltype(std::abs(T())); const T zero = numeric_traits::zero(); const real_t rzero = real_t(0); const real_t rone = real_t(1); const real_t rtwo = real_t(2); Matrix W(A); Matrix U = Matrix::identity(m); Matrix V = Matrix::identity(n); // d[], e[] are the bidiagonal elements — type T during bidiagonalize, made real later std::vector d_cx(n, zero); std::vector e_cx(n, zero); for (size_t k = 0; k < n; ++k) { // --- Left Householder --- { real_t sigma_sq = rzero; for (size_t i = k; i < m; ++i) sigma_sq += std::abs(W(i, k)) * std::abs(W(i, k)); real_t alpha_r = std::sqrt(sigma_sq); if (alpha_r > rzero) { // Sign selection (Complex: match the phase) T alpha; if constexpr (numeric_traits::is_complex) { auto abs_wkk = std::abs(W(k, k)); alpha = (abs_wkk > rzero) ? T(-alpha_r) * W(k, k) / T(abs_wkk) : T(alpha_r); } else { alpha = (W(k, k) > zero) ? T(-alpha_r) : T(alpha_r); } W(k, k) -= alpha; // tau = 2 / (v^H v), v = W(k:m, k) real_t vhv = rzero; for (size_t i = k; i < m; ++i) vhv += std::abs(W(i, k)) * std::abs(W(i, k)); T tau = T(rtwo / vhv); // W(:, j) -= tau * v * (v^H * W(:, j)) for (size_t j = k + 1; j < n; ++j) { T dot_val = zero; for (size_t i = k; i < m; ++i) { if constexpr (numeric_traits::is_complex) dot_val += sangi::conj(W(i, k)) * W(i, j); else dot_val += W(i, k) * W(i, j); } dot_val = tau * dot_val; for (size_t i = k; i < m; ++i) W(i, j) -= W(i, k) * dot_val; } // U(:, j) -= tau * U(:, j) * (v^H) → U = U * (I - tau v v^H) for (size_t i = 0; i < m; ++i) { T dot_val = zero; for (size_t j = k; j < m; ++j) { if constexpr (numeric_traits::is_complex) dot_val += U(i, j) * W(j, k); else dot_val += U(i, j) * W(j, k); } dot_val = tau * dot_val; for (size_t j = k; j < m; ++j) { if constexpr (numeric_traits::is_complex) U(i, j) -= dot_val * sangi::conj(W(j, k)); else U(i, j) -= dot_val * W(j, k); } } d_cx[k] = alpha; } else { d_cx[k] = W(k, k); } } // --- Right Householder --- if (k + 2 <= n - 1) { real_t sigma_sq = rzero; for (size_t j = k + 1; j < n; ++j) sigma_sq += std::abs(W(k, j)) * std::abs(W(k, j)); real_t alpha_r = std::sqrt(sigma_sq); if (alpha_r > rzero) { T alpha; if constexpr (numeric_traits::is_complex) { auto abs_wk1 = std::abs(W(k, k + 1)); alpha = (abs_wk1 > rzero) ? T(-alpha_r) * W(k, k + 1) / T(abs_wk1) : T(alpha_r); } else { alpha = (W(k, k + 1) > zero) ? T(-alpha_r) : T(alpha_r); } W(k, k + 1) -= alpha; // tau = 2 / (v^H v), v = W(k, k+1:n) real_t vhv = rzero; for (size_t j = k + 1; j < n; ++j) vhv += std::abs(W(k, j)) * std::abs(W(k, j)); T tau = T(rtwo / vhv); // W(i, :) -= tau * (W(i,:) * v) * v^H for (size_t i = k + 1; i < m; ++i) { T dot_val = zero; for (size_t j = k + 1; j < n; ++j) { if constexpr (numeric_traits::is_complex) dot_val += W(i, j) * sangi::conj(W(k, j)); else dot_val += W(i, j) * W(k, j); } dot_val = tau * dot_val; for (size_t j = k + 1; j < n; ++j) W(i, j) -= dot_val * W(k, j); } // V = V * (I - tau v v^H) for (size_t i = 0; i < n; ++i) { T dot_val = zero; for (size_t j = k + 1; j < n; ++j) { if constexpr (numeric_traits::is_complex) dot_val += V(i, j) * sangi::conj(W(k, j)); else dot_val += V(i, j) * W(k, j); } dot_val = tau * dot_val; for (size_t j = k + 1; j < n; ++j) V(i, j) -= dot_val * W(k, j); } e_cx[k + 1] = alpha; } else { e_cx[k + 1] = W(k, k + 1); } } else if (k + 1 < n) { e_cx[k + 1] = W(k, k + 1); } } // Make d_cx[], e_cx[] real and absorb the phases into U, V std::vector d(n, rzero); std::vector e(n, rzero); if constexpr (numeric_traits::is_complex) { // Phase cleanup: U^H A V = B (complex bidiagonal) // Convert B to real positive bidiagonal: B = D_L * B_real * D_R^H // Alternately make d[k] and e[k+1] real positive, absorbing the phases into U, V for (size_t k = 0; k < n; ++k) { // Make d_cx[k] real positive real_t abs_dk = std::abs(d_cx[k]); d[k] = abs_dk; if (abs_dk > rzero) { T phase = d_cx[k] / T(abs_dk); T pconj = sangi::conj(phase); // Absorb the phase into column k of U: U[:,k] *= phase for (size_t i = 0; i < m; ++i) U(i, k) *= phase; // Propagate to e_cx[k+1]: e_cx[k+1] *= pconj if (k + 1 < n) e_cx[k + 1] = pconj * e_cx[k + 1]; } // Make e_cx[k+1] real positive // V[:,k+1] *= conj(phase) applies conj(phase) to column k+1 of B if (k + 1 < n) { real_t abs_ek1 = std::abs(e_cx[k + 1]); e[k + 1] = abs_ek1; if (abs_ek1 > rzero) { T phase = e_cx[k + 1] / T(abs_ek1); T pconj = sangi::conj(phase); // V[:,k+1] *= pconj → applies pconj to column k+1 of B // e[k+1] *= pconj = |e|*phase * conj(phase)/|phase|^2 → real positive for (size_t i = 0; i < n; ++i) V(i, k + 1) *= pconj; // pconj also propagates to d_cx[k+1] d_cx[k + 1] = pconj * d_cx[k + 1]; } } } } else { // Real type: d_cx, e_cx can be used directly as real_t for (size_t i = 0; i < n; ++i) { d[i] = real_t(d_cx[i]); e[i] = real_t(e_cx[i]); } // Correct negative diagonal elements to positive for (size_t i = 0; i < n; ++i) { if (d[i] < rzero) { d[i] = -d[i]; if (i + 1 < n) e[i + 1] = -e[i + 1]; for (size_t j = 0; j < m; ++j) U(j, i) = -U(j, i); } } } const real_t eps = numeric_traits::epsilon(); const int max_iter = 100 * static_cast(n); // Golub-Kahan QR iteration (d[], e[] are real, Givens rotations are also real) for (int iter = 0; iter < max_iter; ++iter) { for (size_t i = 1; i < n; ++i) if (std::abs(e[i]) <= eps * (std::abs(d[i - 1]) + std::abs(d[i]))) e[i] = rzero; size_t q = n; while (q > 1 && e[q - 1] == rzero) --q; if (q <= 1) break; size_t p = q - 1; while (p > 0 && e[p] != rzero) --p; bool handled_zero = false; for (size_t i = p; i < q; ++i) { real_t ref = rzero; if (i + 1 < n && e[i + 1] != rzero) ref = std::max(ref, std::abs(e[i + 1])); if (i > 0 && e[i] != rzero) ref = std::max(ref, std::abs(e[i])); if (ref == rzero) continue; if (std::abs(d[i]) > eps * ref) continue; d[i] = rzero; handled_zero = true; if (i + 1 < q && e[i + 1] != rzero) { real_t bulge = e[i + 1]; e[i + 1] = rzero; for (size_t j = i + 1; j < q; ++j) { real_t c, s; svd_detail::givens(d[j], bulge, c, s); d[j] = c * d[j] - s * bulge; if (j + 1 < q) { bulge = s * e[j + 1]; e[j + 1] = c * e[j + 1]; } for (size_t l = 0; l < m; ++l) { T ui=U(l,i), uj=U(l,j); U(l,i)=T(c)*ui+T(s)*uj; U(l,j)=T(-s)*ui+T(c)*uj; } if (j + 1 >= q || std::abs(bulge) <= eps * std::abs(d[j])) break; } } else if (i > p && e[i] != rzero) { real_t bulge = e[i]; e[i] = rzero; for (size_t jj = 0; jj < i - p; ++jj) { size_t j = i - 1 - jj; real_t c, s; svd_detail::givens(d[j], bulge, c, s); d[j] = c * d[j] - s * bulge; if (j > p) { bulge = s * e[j]; e[j] = c * e[j]; } for (size_t l = 0; l < n; ++l) { T vi=V(l,i), vj=V(l,j); V(l,i)=T(c)*vi+T(s)*vj; V(l,j)=T(-s)*vi+T(c)*vj; } if (j <= p || std::abs(bulge) <= eps * std::abs(d[j])) break; } } break; } if (handled_zero) continue; real_t d_qm1=d[q-1], d_qm2=d[q-2], e_qm1=e[q-1]; real_t e_qm2 = (q>=3 && q-2>p) ? e[q-2] : rzero; real_t t11=d_qm2*d_qm2+e_qm2*e_qm2, t12=d_qm2*e_qm1, t22=d_qm1*d_qm1+e_qm1*e_qm1; real_t delta=(t11-t22)/rtwo; real_t mu=t22-t12*t12/(delta+(delta>=rzero?rone:real_t(-1))*std::sqrt(delta*delta+t12*t12)); real_t x=d[p]*d[p]-mu, z=d[p]*e[p+1]; for (size_t k = p; k < q - 1; ++k) { real_t c, s; svd_detail::givens(x, z, c, s); if (k > p) e[k] = c*e[k]-s*z; real_t dk=d[k], dk1=d[k+1], ek1=e[k+1]; d[k]=c*dk-s*ek1; e[k+1]=s*dk+c*ek1; real_t bulge=-s*dk1; d[k+1]=c*dk1; for (size_t i=0;i d[max_idx]) max_idx = j; if (max_idx != i) { std::swap(d[i], d[max_idx]); for (size_t j = 0; j < m; ++j) std::swap(U(j, i), U(j, max_idx)); for (size_t j = 0; j < n; ++j) std::swap(V(j, i), V(j, max_idx)); } } Vector sigma(min_mn); for (size_t i = 0; i < min_mn; ++i) sigma[i] = T(d[i]); Matrix U_out(m, min_mn); for (size_t i = 0; i < m; ++i) for (size_t j = 0; j < min_mn; ++j) U_out(i, j) = U(i, j); if (transV) { Matrix Vt(min_mn, n); for (size_t i = 0; i < min_mn; ++i) for (size_t j = 0; j < n; ++j) { if constexpr (numeric_traits::is_complex) Vt(i, j) = sangi::conj(V(j, i)); // V^H else Vt(i, j) = V(j, i); // V^T } return { U_out, sigma, Vt }; } else { Matrix V_out(n, min_mn); for (size_t i = 0; i < n; ++i) for (size_t j = 0; j < min_mn; ++j) V_out(i, j) = V(i, j); return { U_out, sigma, V_out }; } } // end of `if constexpr Matrix` else branch } /** * @brief Reconstruct the original matrix from the SVD result * @tparam T element type * @param U left singular vector matrix (m x k) * @param sigma singular value vector (k) * @param V right singular vector matrix (n x k) * @return the reconstructed matrix A (m x n) */ template Matrix reconstruct_matrix_from_svd( const BaseMatrix& U, // U const BaseVector& s, // diagonal elements of Σ const BaseMatrix& Vt) // V^T { const auto m = U.rows(); const auto n = Vt.cols(); // number of columns of V^T const auto r = s.size(); // usually min(m, n) Matrix result(m, n, numeric_traits::zero()); // Reconstruction A = U * Σ * Vt for (std::size_t i = 0; i < m; ++i) { for (std::size_t j = 0; j < n; ++j) { for (std::size_t k = 0; k < r; ++k) { result(i, j) += U(i, k) * s[k] * Vt(k, j); } } } return result; } /** * @brief Solve a system of linear equations using the improved SVD * @tparam T element type * @param a coefficient matrix * @param b right-hand-side vector * @param rcond threshold for the reciprocal condition number (default is 100 times machine epsilon) * @return solution vector */ template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> svd_solve(const MA& a, const VB& b, sangi::element_t rcond = sangi::element_t(numeric_traits>::epsilon() * 100)) { using T = sangi::element_t; SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); if (a.rows() != b.size()) { assert(false && "DimensionError: svd_solve: matrix and vector dimensions mismatch"); throw DimensionError("svd_solve: matrix and vector dimensions mismatch"); } const T zero = numeric_traits::zero(); const auto eps = numeric_traits::epsilon(); const auto m = a.rows(); const auto n = a.cols(); const auto min_mn = std::min(m, n); // 1. SVD decomposition auto [U, sigma, Vt] = svd_decomposition(a); // 2. Find the largest singular value (sigma is always real-valued) auto max_sigma_abs = decltype(std::abs(zero))(0); for (typename Matrix::size_type i = 0; i < min_mn; ++i) { auto si = std::abs(sigma[i]); if (si > max_sigma_abs) max_sigma_abs = si; } if (max_sigma_abs < eps) { return Vector(n, zero); } // 3. Compute the threshold auto threshold_abs = std::abs(rcond) * max_sigma_abs; auto sigma_min_abs = max_sigma_abs; for (typename Matrix::size_type i = 0; i < min_mn; ++i) { auto si = std::abs(sigma[i]); if (si > 0) { if (si < sigma_min_abs) sigma_min_abs = si; } } threshold_abs = std::max(threshold_abs, eps * sigma_min_abs * min_mn); // 4. Compute U^H * b (Complex: conjugate transpose) Vector Utb(min_mn, zero); for (typename Matrix::size_type i = 0; i < min_mn; ++i) { for (typename Matrix::size_type j = 0; j < m; ++j) { if constexpr (numeric_traits::is_complex) Utb[i] += sangi::conj(U(j, i)) * b[j]; else Utb[i] += U(j, i) * b[j]; } } // 5. Compute the solution Vector x(n, zero); for (typename Matrix::size_type j = 0; j < min_mn; ++j) { auto sj = std::abs(sigma[j]); if (sj > threshold_abs) { T factor = Utb[j] / sigma[j]; for (typename Matrix::size_type i = 0; i < n; ++i) { x[i] += Vt(j,i) * factor; } } else if (sj > 0) { // For singular values below the threshold, continuously reduce the weight T weight = T(sj / threshold_abs); T factor = Utb[j] * weight / sigma[j]; for (typename Matrix::size_type i = 0; i < n; ++i) { x[i] += Vt(j, i) * factor; } } } return x; } /** * @brief Compute the LDL decomposition * @tparam T element type * @param a matrix to decompose (symmetric matrix) * @return a pair of L and D (returns L and D separately) */ template requires concepts::MatrixOf std::pair, Vector> ldl_decomposition(const M& a) { using T = typename M::value_type; SANGI_STATIC_ASSERT_SQUARE(M); if (a.rows() != a.cols()) { assert(false && "DimensionError: ldl_decomposition: matrix must be square"); throw DimensionError("ldl_decomposition: matrix must be square"); } if (!is_symmetric(a)) { throw MathError("ldl_decomposition: matrix must be symmetric"); } const auto n = a.rows(); Matrix L(n, n); Vector D(n); // Initialize L as the identity matrix for (typename Matrix::size_type i = 0; i < n; ++i) { for (typename Matrix::size_type j = 0; j < n; ++j) { L(i, j) = (i == j) ? T(1) : T(0); } } // Compute the LDL decomposition for (typename Matrix::size_type j = 0; j < n; ++j) { // Compute D[j] T sum = a(j, j); for (typename Matrix::size_type k = 0; k < j; ++k) { sum -= L(j, k) * L(j, k) * D[k]; } D[j] = sum; // Compute the remaining elements of L for (typename Matrix::size_type i = j + 1; i < n; ++i) { sum = a(i, j); for (typename Matrix::size_type k = 0; k < j; ++k) { sum -= L(i, k) * L(j, k) * D[k]; } // Check for zero diagonal elements if (std::abs(D[j]) < std::numeric_limits::epsilon()) { throw MathError("ldl_decomposition: division by zero"); } L(i, j) = sum / D[j]; } } return { L, D }; } /** * @brief Solve a system of linear equations using the LDL decomposition * @tparam T element type * @param a coefficient matrix (symmetric matrix) * @param b right-hand-side vector * @return solution vector */ template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> ldl_solve(const MA& a, const VB& b) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); if (a.rows() != a.cols()) { assert(false && "DimensionError: ldl_solve: matrix must be square"); throw DimensionError("ldl_solve: matrix must be square"); } if (a.rows() != b.size()) { assert(false && "DimensionError: ldl_solve: matrix and vector dimensions mismatch"); throw DimensionError("ldl_solve: matrix and vector dimensions mismatch"); } const auto n = a.rows(); // LDL decomposition auto [L, D] = ldl_decomposition(a); // Forward substitution (Ly = b) Vector y(n); for (typename Matrix::size_type i = 0; i < n; ++i) { T sum = b[i]; for (typename Matrix::size_type j = 0; j < i; ++j) { sum -= L(i, j) * y[j]; } y[i] = sum; } // Diagonal scaling (Dz = y) Vector z(n); for (typename Matrix::size_type i = 0; i < n; ++i) { if (std::abs(D[i]) < std::numeric_limits::epsilon()) { throw MathError("ldl_solve: division by zero"); } z[i] = y[i] / D[i]; } // Backward substitution (L^T x = z) Vector x(n); for (typename Matrix::size_type i = n; i-- > 0; ) { T sum = z[i]; for (typename Matrix::size_type j = i + 1; j < n; ++j) { sum -= L(j, i) * x[j]; } x[i] = sum; } return x; } /** * @brief Compute the inner product of vectors * @tparam T element type * @tparam V vector type * @param a first vector * @param b second vector * @return value of the inner product */ template requires concepts::VectorOf T dot(const V& a, const V& b) { if (a.size() != b.size()) { assert(false && "DimensionError: dot: vector dimensions mismatch"); throw DimensionError("dot: vector dimensions mismatch"); } T result = T(0); for (typename V::size_type i = 0; i < a.size(); ++i) { result += a[i] * b[i]; } return result; } /** * @brief Compute the norm of a vector * @tparam T element type * @tparam V vector type * @param v vector * @return value of the norm */ template requires concepts::VectorOf T norm(const V& v) { return std::sqrt(dot(v, v)); } /** * @brief Compute the rank of a matrix * Count the singular values of the SVD that are at or above the threshold. * @param a input matrix * @param tol threshold (auto-set if negative: max(m,n) * eps * sigma_max) * @return rank */ template requires sangi::BaseMatrixLike size_t rank(const MA& a, sangi::element_t tol = sangi::element_t(-1)) { using T = sangi::element_t; // No square requirement: rank is defined for any m x n matrix. auto [U, sigma, Vt] = svd_decomposition(a); const size_t k = sigma.size(); if (k == 0) return 0; // Auto-set the threshold T threshold = tol; if (threshold < T(0)) { T sigma_max = sigma[0]; for (size_t i = 1; i < k; ++i) { if (sigma[i] > sigma_max) sigma_max = sigma[i]; } size_t max_dim = std::max(a.rows(), a.cols()); threshold = sigma_max * std::numeric_limits::epsilon() * T(max_dim); } size_t r = 0; for (size_t i = 0; i < k; ++i) { if (sigma[i] > threshold) ++r; } return r; } /** * @brief Compute the condition number of a matrix * 2-norm condition number: sigma_max / sigma_min * @param a input matrix (square matrix) * @return condition number (+infinity for singular matrices) */ template T conditionNumber(const BaseMatrix& a) { auto [U, sigma, Vt] = svd_decomposition(a); const size_t k = sigma.size(); if (k == 0) { return std::numeric_limits::infinity(); } T sigma_max = sigma[0]; T sigma_min = sigma[0]; for (size_t i = 1; i < k; ++i) { if (sigma[i] > sigma_max) sigma_max = sigma[i]; if (sigma[i] < sigma_min) sigma_min = sigma[i]; } if (sigma_min <= T(0)) { return std::numeric_limits::infinity(); } return sigma_max / sigma_min; } // ==================================================================== // Condition Number Estimation // ==================================================================== namespace condest_detail { // Solve Ax = b with an LU-decomposed matrix (forward/backward substitution) template Vector lu_solve_factored( const BaseMatrix& lu, const std::vector::size_type>& pivots, const BaseVector& b) { const auto n = lu.rows(); Vector x(b); // 1 copy via BaseVector ctor (mutated below) // Pivot permutation + forward substitution (Ly = Pb) for (std::size_t k = 0; k < n; ++k) { if (pivots[k] != k) std::swap(x[k], x[pivots[k]]); for (std::size_t i = k + 1; i < n; ++i) x[i] -= lu(i, k) * x[k]; } // Backward substitution (Ux = y) for (std::size_t ii = 0; ii < n; ++ii) { std::size_t i = n - 1 - ii; for (std::size_t j = i + 1; j < n; ++j) x[i] -= lu(i, j) * x[j]; x[i] /= lu(i, i); } return x; } // Solve A^T x = b with an LU-decomposed matrix template Vector lu_solve_transpose_factored( const BaseMatrix& lu, const std::vector::size_type>& pivots, const BaseVector& b) { const auto n = lu.rows(); Vector x(b); // 1 copy via BaseVector ctor (mutated below) // Forward substitution (U^T y = b) for (std::size_t i = 0; i < n; ++i) { x[i] /= lu(i, i); for (std::size_t j = i + 1; j < n; ++j) x[j] -= lu(i, j) * x[i]; } // Backward substitution (L^T z = y) for (std::size_t ii = 0; ii < n; ++ii) { std::size_t i = n - 1 - ii; for (std::size_t j = i + 1; j < n; ++j) x[i] -= lu(j, i) * x[j]; // L's diagonal is 1 } // Inverse pivot permutation for (std::size_t ii = 0; ii < n; ++ii) { std::size_t k = n - 1 - ii; if (pivots[k] != k) std::swap(x[k], x[pivots[k]]); } return x; } } // namespace condest_detail /** * @brief Estimate ‖A⁻¹‖₁ via the Hager-Higham algorithm * * Takes an LU-decomposed matrix and, without explicitly building A⁻¹, * estimates ‖A⁻¹‖₁ in O(n²). * Reference: Higham (1988) "FORTRAN codes for estimating the one-norm of * a real or complex matrix, with applications to condition estimation" * * @param lu LU-decomposed matrix (output of lu_decomposition) * @param pivots pivot information * @return estimate of ‖A⁻¹‖₁ (a lower bound) */ template T estimate_inv_norm1( const BaseMatrix& lu, const std::vector::size_type>& pivots) { const auto n = lu.rows(); if (n == 0) return T(0); // Hager-Higham: at most 5 iterations const int max_iter = 5; // x = (1/n, ..., 1/n) Vector x(n, T(1) / static_cast(n)); T gamma = T(0); for (int iter = 0; iter < max_iter; ++iter) { // w = A⁻¹ x auto w = condest_detail::lu_solve_factored(lu, pivots, x); // gamma_new = ‖w‖₁ T gamma_new = T(0); for (std::size_t i = 0; i < n; ++i) gamma_new += std::abs(static_cast(w[i])); if (iter > 0 && gamma_new <= gamma) { break; } gamma = gamma_new; // y = sign(w) Vector y(n); for (std::size_t i = 0; i < n; ++i) y[i] = (w[i] >= T(0)) ? T(1) : T(-1); // z = A^{-T} y auto z = condest_detail::lu_solve_transpose_factored(lu, pivots, y); // ‖z‖∞ T z_inf = T(0); std::size_t j_max = 0; for (std::size_t i = 0; i < n; ++i) { T az = static_cast(std::abs(static_cast(z[i]))); if (az > z_inf) { z_inf = az; j_max = i; } } // z^T w = Σ y_i * w_i (= ‖w‖₁ since y=sign(w)) // Convergence test: ‖z‖∞ ≤ z^T * w ⇔ ‖z‖∞ ≤ gamma if (z_inf <= gamma) { break; } // x = e_{j_max} for (std::size_t i = 0; i < n; ++i) x[i] = T(0); x[j_max] = T(1); } return static_cast(gamma); } /** * @brief Estimate of the 1-norm condition number: κ₁(A) = ‖A‖₁ · ‖A⁻¹‖₁ * * O(n³ + n²) ≈ O(n³) via LU decomposition + Hager-Higham estimation, but * much faster than the SVD-based conditionNumber(). * For a singular matrix, the LU decomposition throws an exception. * * @param A square matrix * @return estimate of κ₁(A) */ template T condition_number_1norm(const BaseMatrix& A) { if (A.rows() != A.cols()) { assert(false && "DimensionError: condition_number_1norm: matrix must be square"); throw DimensionError("condition_number_1norm: matrix must be square"); } const auto n = A.rows(); if (n == 0) return T(0); // ‖A‖₁ = max column sum T norm1_A = T(0); for (std::size_t j = 0; j < n; ++j) { T col_sum = T(0); for (std::size_t i = 0; i < n; ++i) col_sum += static_cast(std::abs(static_cast(A(i, j)))); if (col_sum > norm1_A) norm1_A = col_sum; } auto [lu, pivots] = lu_decomposition(A); T norm1_inv = estimate_inv_norm1(lu, pivots); return norm1_A * norm1_inv; } /** * @brief Estimate of the ∞-norm condition number: κ∞(A) = ‖A‖∞ · ‖A⁻¹‖∞ = κ₁(A^T) * * @param A square matrix * @return estimate of κ∞(A) */ template T condition_number_inf(const BaseMatrix& A) { return condition_number_1norm(A.transposed()); // 0-copy view } /** * @brief Estimate of the reciprocal condition number: rcond₁(A) = 1 / κ₁(A) * * Corresponds to LAPACK's dgecon. The closer to 0, the closer to singular. * * @param A square matrix * @return estimate of 1/κ₁(A) (LU throws for a singular matrix) */ template T rcond_1norm(const BaseMatrix& A) { T cond = condition_number_1norm(A); if (cond == T(0)) return T(0); return T(1) / cond; } //===================================================================== // Hessenberg decomposition //===================================================================== /** * @brief Hessenberg decomposition: A = Q H Qᵀ * * Transforms a square matrix A into an upper Hessenberg matrix H via Householder reflections. * H(i,j) = 0 for i > j+1. Q is an orthogonal matrix. * * @tparam T element type (double, float, etc.) * @param A input matrix (n×n square matrix) * @return {H, Q} — H: upper Hessenberg matrix, Q: orthogonal matrix (A = Q*H*Qᵀ) */ template requires sangi::BaseMatrixLike std::pair>, Matrix>> hessenberg_decomposition(const MA& A) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); const auto n = A.rows(); if (n != A.cols()) { assert(false && "DimensionError: hessenberg_decomposition: matrix must be square"); throw DimensionError("hessenberg_decomposition: matrix must be square"); } Matrix H = A; Matrix Q(n, n, T(0)); for (std::size_t i = 0; i < n; ++i) Q(i, i) = T(1); if (n <= 2) { return {H, Q}; } // Householder reflection for columns k = 0, 1, ..., n-3 for (std::size_t k = 0; k < n - 2; ++k) { // Build the Householder vector v (targeting H(k+1:n, k)) const std::size_t m = n - k - 1; // vector length std::vector v(m); for (std::size_t i = 0; i < m; ++i) { v[i] = H(k + 1 + i, k); } // ||v||₂ T norm_v = T(0); for (std::size_t i = 0; i < m; ++i) { norm_v += v[i] * v[i]; } norm_v = std::sqrt(norm_v); if (norm_v <= std::numeric_limits::epsilon() * std::abs(H(k, k)) * T(10)) { continue; // already zero → skip } // Add sign(v[0]) * ||v|| to v[0] T alpha = (v[0] >= T(0)) ? -norm_v : norm_v; v[0] -= alpha; // Normalize v T norm_v2 = T(0); for (std::size_t i = 0; i < m; ++i) { norm_v2 += v[i] * v[i]; } if (norm_v2 <= T(0)) continue; T inv_norm = T(1) / std::sqrt(norm_v2); for (std::size_t i = 0; i < m; ++i) { v[i] *= inv_norm; } // H ← (I - 2vvᵀ) H (from the left): H[k+1:n, :] -= 2v(vᵀ H[k+1:n, :]) for (std::size_t j = 0; j < n; ++j) { T dot = T(0); for (std::size_t i = 0; i < m; ++i) { dot += v[i] * H(k + 1 + i, j); } dot *= T(2); for (std::size_t i = 0; i < m; ++i) { H(k + 1 + i, j) -= v[i] * dot; } } // H ← H (I - 2vvᵀ) (from the right): H[:, k+1:n] -= 2(H[:, k+1:n] v)vᵀ for (std::size_t i = 0; i < n; ++i) { T dot = T(0); for (std::size_t j = 0; j < m; ++j) { dot += H(i, k + 1 + j) * v[j]; } dot *= T(2); for (std::size_t j = 0; j < m; ++j) { H(i, k + 1 + j) -= dot * v[j]; } } // Q ← Q (I - 2vvᵀ) (accumulate from the right) for (std::size_t i = 0; i < n; ++i) { T dot = T(0); for (std::size_t j = 0; j < m; ++j) { dot += Q(i, k + 1 + j) * v[j]; } dot *= T(2); for (std::size_t j = 0; j < m; ++j) { Q(i, k + 1 + j) -= dot * v[j]; } } } // Clean up tiny values below the subdiagonal for (std::size_t j = 0; j < n; ++j) { for (std::size_t i = j + 2; i < n; ++i) { H(i, j) = T(0); } } return {H, Q}; } //===================================================================== // Schur decomposition (real Schur) //===================================================================== /** * @brief Real Schur decomposition: A = Q T Qᵀ * * T is a quasi-upper-triangular matrix (real Schur form): * - real eigenvalues are 1×1 diagonal blocks * - complex conjugate pairs are 2×2 diagonal blocks * Q is an orthogonal matrix. * * Algorithm: Hessenberg decomposition → Francis double-shift QR iteration * * @tparam T element type * @param A input matrix (n×n square matrix) * @return {T, Q} — T: quasi-upper-triangular matrix, Q: orthogonal matrix (A = Q*T*Qᵀ) */ template requires sangi::BaseMatrixLike std::pair>, Matrix>> schur_decomposition(const MA& A) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); const auto n = A.rows(); if (n != A.cols()) { assert(false && "DimensionError: schur_decomposition: matrix must be square"); throw DimensionError("schur_decomposition: matrix must be square"); } if (n <= 1) { return {A, Matrix(1, 1, T(1))}; } #if SANGI_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { Matrix Sch = A; Matrix Q(n, n); lapack_int sdim = 0; std::vector wr(n), wi(n); lapack_int info; if constexpr (std::is_same_v) info = LAPACKE_sgees(LAPACK_ROW_MAJOR, 'V', 'N', nullptr, (lapack_int)n, Sch.data(), (lapack_int)n, &sdim, wr.data(), wi.data(), Q.data(), (lapack_int)n); else info = LAPACKE_dgees(LAPACK_ROW_MAJOR, 'V', 'N', nullptr, (lapack_int)n, Sch.data(), (lapack_int)n, &sdim, wr.data(), wi.data(), Q.data(), (lapack_int)n); if (info > 0) throw MathError("schur_decomposition: LAPACKE_gees did not converge"); return { Sch, Q }; } #endif // Step 1: Hessenberg decomposition auto [Sch, Q] = hessenberg_decomposition(A); const int max_iter = 300 * static_cast(n); const T eps = std::numeric_limits::epsilon(); // Step 2: Francis QR iteration // The working range is [ilo, ihi) (0-indexed) int ihi = static_cast(n); int total_iter = 0; int iter_since_deflation = 0; // for exceptional-shift decision while (ihi > 1 && total_iter < max_iter) { // Deflation: find a location where the subdiagonal element is sufficiently small int ilo = ihi - 1; while (ilo > 0) { T threshold = eps * (std::abs(Sch(ilo - 1, ilo - 1)) + std::abs(Sch(ilo, ilo))); if (threshold == T(0)) threshold = eps; if (std::abs(Sch(ilo, ilo - 1)) <= threshold) { Sch(ilo, ilo - 1) = T(0); break; } --ilo; } if (ilo == ihi - 1) { // 1×1 block (real eigenvalue): deflate --ihi; iter_since_deflation = 0; continue; } if (ilo == ihi - 2) { // 2×2 block: compute the eigenvalues and decide T a11 = Sch(ilo, ilo), a12 = Sch(ilo, ilo + 1); T a21 = Sch(ilo + 1, ilo), a22 = Sch(ilo + 1, ilo + 1); T disc = (a11 - a22) * (a11 - a22) + T(4) * a12 * a21; if (disc >= T(0)) { // Real eigenvalues → diagonalize via QR with Wilkinson shift // Shift: the eigenvalue closer to a22 T delta = (a11 - a22) / T(2); T sign_d = (delta >= T(0)) ? T(1) : T(-1); T sigma = a22 - sign_d * a21 * a21 / (std::abs(delta) + std::sqrt(delta * delta + a21 * a21)); // Givens rotation on the shifted matrix T x2 = Sch(ilo, ilo) - sigma; T y2 = Sch(ilo + 1, ilo); T r = std::sqrt(x2 * x2 + y2 * y2); if (r > T(0)) { T c = x2 / r; T s = y2 / r; // Rotate from the left for (int j = ilo; j < static_cast(n); ++j) { T t1 = Sch(ilo, j); T t2 = Sch(ilo + 1, j); Sch(ilo, j) = c * t1 + s * t2; Sch(ilo + 1, j) = -s * t1 + c * t2; } // Rotate from the right for (int i = 0; i <= ilo + 1; ++i) { T t1 = Sch(i, ilo); T t2 = Sch(i, ilo + 1); Sch(i, ilo) = c * t1 + s * t2; Sch(i, ilo + 1) = -s * t1 + c * t2; } // Accumulate Q for (std::size_t i = 0; i < n; ++i) { T t1 = Q(i, ilo); T t2 = Q(i, ilo + 1); Q(i, ilo) = c * t1 + s * t2; Q(i, ilo + 1) = -s * t1 + c * t2; } } } // 2×2 block (complex conjugate pair or already diagonalized): deflate ihi -= 2; iter_since_deflation = 0; continue; } // Francis double-shift QR step auto compute_exceptional_shift = [&]() -> std::pair { // Exceptional shift (LAPACK style): // When the usual shift does not converge, break the pattern // with an ad-hoc shift T sub = std::abs(Sch(ihi - 1, ihi - 2)); T sub2 = (ihi >= 3) ? std::abs(Sch(ihi - 2, ihi - 3)) : T(0); T dat1 = T(0.75); T ss = dat1 * (sub + sub2) + Sch(ihi - 1, ihi - 1) + Sch(ihi - 2, ihi - 2); T tt = Sch(ihi - 1, ihi - 1) * Sch(ihi - 2, ihi - 2) - dat1 * dat1 * sub * sub; return {ss, tt}; }; // Wilkinson shift: eigenvalues of the bottom-right 2×2 block T a11 = Sch(ihi - 2, ihi - 2), a12 = Sch(ihi - 2, ihi - 1); T a21 = Sch(ihi - 1, ihi - 2), a22 = Sch(ihi - 1, ihi - 1); T s_shift = a11 + a22; // trace T t_shift = a11 * a22 - a12 * a21; // determinant // Implicit QR step (bulge chasing) // Initial vector: the first 3 elements of (H - σ₂I)(H - σ₁I)e₁ T h11 = Sch(ilo, ilo), h12 = Sch(ilo, ilo + 1); T h21 = Sch(ilo + 1, ilo), h22 = Sch(ilo + 1, ilo + 1); T x = h11 * h11 + h12 * h21 - s_shift * h11 + t_shift; T y = h21 * (h11 + h22 - s_shift); T z = h21 * Sch(ilo + 2, ilo + 1); // Initial-vector degeneracy detection: // If x≈0, y≈0, bulge chasing is essentially just a row/column // swap and does not converge → avoid with an exceptional shift T xy_mag = std::abs(x) + std::abs(y); T z_mag = std::abs(z); bool need_exceptional = (iter_since_deflation > 0 && iter_since_deflation % 10 == 0) || (z_mag > T(0) && xy_mag < eps * z_mag * T(1000)); if (need_exceptional) { auto [ss, tt] = compute_exceptional_shift(); s_shift = ss; t_shift = tt; x = h11 * h11 + h12 * h21 - s_shift * h11 + t_shift; y = h21 * (h11 + h22 - s_shift); z = h21 * Sch(ilo + 2, ilo + 1); } for (int k = ilo; k < ihi - 1; ++k) { // Chase the bulge with a 3×1 (or 2×1 at end) Householder reflection int p = std::min(3, ihi - k); // Householder vector T norm_xyz; if (p == 3) { norm_xyz = std::sqrt(x * x + y * y + z * z); } else { norm_xyz = std::sqrt(x * x + y * y); } if (norm_xyz == T(0)) { if (k + p < ihi) { x = Sch(k + 1, k); y = (k + 2 < ihi) ? Sch(k + 2, k) : T(0); z = (k + 3 < ihi) ? Sch(k + 3, k + 1) : T(0); } continue; } T beta = (x >= T(0)) ? -norm_xyz : norm_xyz; T v1 = x - beta; T inv_v1 = T(1) / v1; T v2 = y * inv_v1; T v3 = (p == 3) ? z * inv_v1 : T(0); T tau = -v1 / beta; // equivalent to 2 / (1 + v2² + v3²) // Reflect from the left: H[k:k+p, :] -= tau * v * (vᵀ * H[k:k+p, :]) int jstart = (k > ilo) ? k - 1 : k; for (int j = jstart; j < static_cast(n); ++j) { T w = Sch(k, j) + v2 * Sch(k + 1, j); if (p == 3) w += v3 * Sch(k + 2, j); w *= tau; Sch(k, j) -= w; Sch(k + 1, j) -= w * v2; if (p == 3) Sch(k + 2, j) -= w * v3; } // Reflect from the right: H[:, k:k+p] -= tau * (H[:, k:k+p] * v) * vᵀ int iend = std::min(ihi, k + 4); for (int i = 0; i < iend; ++i) { T w = Sch(i, k) + v2 * Sch(i, k + 1); if (p == 3) w += v3 * Sch(i, k + 2); w *= tau; Sch(i, k) -= w; Sch(i, k + 1) -= w * v2; if (p == 3) Sch(i, k + 2) -= w * v3; } // Accumulate Q: Q[:, k:k+p] -= tau * (Q[:, k:k+p] * v) * vᵀ for (std::size_t i = 0; i < n; ++i) { T w = Q(i, k) + v2 * Q(i, k + 1); if (p == 3) w += v3 * Q(i, k + 2); w *= tau; Q(i, k) -= w; Q(i, k + 1) -= w * v2; if (p == 3) Q(i, k + 2) -= w * v3; } // Update x, y, z for the next iteration if (k + 1 < ihi - 1) { x = Sch(k + 1, k); y = Sch(k + 2, k); z = (k + 3 < ihi) ? Sch(k + 3, k) : T(0); } } ++total_iter; ++iter_since_deflation; } // Clean up tiny values on the subdiagonal for (std::size_t i = 1; i < n; ++i) { T threshold = eps * (std::abs(Sch(i - 1, i - 1)) + std::abs(Sch(i, i))); if (threshold == T(0)) threshold = eps; if (std::abs(Sch(i, i - 1)) <= threshold) { Sch(i, i - 1) = T(0); } } // Finalize the real Schur form: decompose 2×2 blocks with disc > 0 (real eigenvalue pairs) // into two 1×1 blocks via Givens rotation. Neglecting this causes // the selector of schur_decomposition_reordered to see only one eigenvalue of the block, // breaking the stable/unstable separation of Hamiltonian matrices (LQR/Laub). // Since a single Givens rotation perturbs the subdiagonal of other blocks, a single // pass may leave some missed (the subdiagonal of an adjacent block is temporarily // perturbed → remains naturally small). Iterate for at most n passes // (no new cleanup loop is created; it stops based on the disc>0 test alone). { for (std::size_t outer = 0; outer < n; ++outer) { bool changed = false; for (std::size_t k = 0; k + 1 < n; ++k) { if (Sch(k + 1, k) == T(0)) continue; T a = Sch(k, k), b = Sch(k, k + 1); T c = Sch(k + 1, k), d = Sch(k + 1, k + 1); // Treat a microscopic subdiagonal as "effectively already 1×1" // (a borderline case deflated by the original Francis QR; touching it degrades accuracy) T subdiag_thr = eps * (std::abs(a) + std::abs(d)); if (subdiag_thr == T(0)) subdiag_thr = eps; if (std::abs(c) <= subdiag_thr) { Sch(k + 1, k) = T(0); continue; } T disc = (a - d) * (a - d) + T(4) * b * c; // Decompose only when numerically "clearly disc > 0" (when the boundary // with the complex conjugate pair is close, do not touch it). tolerance = ‖block‖² * eps² * 100 // (eps²-based: the numerical error for a true 0 disc is ~‖block‖²·eps²). T block_norm_sq = a*a + b*b + c*c + d*d; T tol_sq = block_norm_sq * eps * eps * T(100); if (disc <= tol_sq) continue; // complex conjugate pair or boundary T sq = std::sqrt(disc); T lam1 = ((a + d) + sq) / T(2); // Build the eigenvector v = (v0, v1) for lam1 in a numerically stable way T v0, v1; if (std::abs(b) > std::abs(c)) { v0 = b; v1 = lam1 - a; } else { v0 = lam1 - d; v1 = c; } T r = std::hypot(v0, v1); if (r == T(0)) continue; T cs = v0 / r; T sn = v1 / r; // Update columns k, k+1 of Sch (all rows) for (std::size_t i = 0; i < n; ++i) { T t1 = Sch(i, k), t2 = Sch(i, k + 1); Sch(i, k) = cs * t1 + sn * t2; Sch(i, k + 1) = -sn * t1 + cs * t2; } // Update rows k, k+1 of Sch (all columns) for (std::size_t j = 0; j < n; ++j) { T t1 = Sch(k, j), t2 = Sch(k + 1, j); Sch(k, j) = cs * t1 + sn * t2; Sch(k + 1, j) = -sn * t1 + cs * t2; } // Post-multiply Q (Q · G) for (std::size_t i = 0; i < n; ++i) { T q1 = Q(i, k), q2 = Q(i, k + 1); Q(i, k) = cs * q1 + sn * q2; Q(i, k + 1) = -sn * q1 + cs * q2; } Sch(k + 1, k) = T(0); changed = true; } if (!changed) break; } } return {Sch, Q}; } //===================================================================== // Reordered real Schur decomposition (Bai-Demmel 1993) //===================================================================== /** * @brief Return the size of the k-th diagonal block of the real Schur form (1 or 2) * * If Tm(k+1, k) != 0 it is a 2×2 block (complex conjugate pair), otherwise a 1×1 block. */ template std::size_t schur_block_size(const BaseMatrix& Tm, std::size_t k) { const std::size_t n = Tm.rows(); if (k + 1 >= n) return 1; return (Tm(k + 1, k) != T(0)) ? 2 : 1; } /** * @brief Return the eigenvalue pair of the k-th diagonal block * * For a 1×1 block, fill both with the same real eigenvalue. * For a 2×2 block, return the complex conjugate pair. */ template std::pair, Complex> schur_block_eigenvalues(const BaseMatrix& Tm, std::size_t k) { if (schur_block_size(Tm, k) == 1) { Complex e(Tm(k, k), T(0)); return {e, e}; } const T a = Tm(k, k); const T b = Tm(k, k + 1); const T c = Tm(k + 1, k); const T d = Tm(k + 1, k + 1); const T tr_half = (a + d) / T(2); const T disc = (a - d) * (a - d) + T(4) * b * c; if (disc >= T(0)) { const T sq = std::sqrt(disc); return {Complex(tr_half + sq / T(2), T(0)), Complex(tr_half - sq / T(2), T(0))}; } const T im = std::sqrt(-disc) / T(2); return {Complex(tr_half, im), Complex(tr_half, -im)}; } namespace detail_schur { /** * Solve the small Sylvester equation A11 X - X A22 = C (p, q ∈ {1,2}) * * Kronecker-product form: (I_q ⊗ A11 - A22^T ⊗ I_p) vec(X) = vec(C) * (vec is the column stack) * * Return value: the minimum pivot of Gaussian elimination (near zero means near-degenerate, swap impossible) */ template T solve_sylvester_small(const T* A11, std::size_t p, const T* A22, std::size_t q, const T* C, T* X_out) { const std::size_t n = p * q; // ≤ 4 T M[16] = {T(0)}; T b[4] = {T(0)}; auto Aij = [](const T* A_, std::size_t dim, std::size_t i, std::size_t j) -> T { return A_[i * dim + j]; }; // Column-stack vec convention: vec(X)[l*p + i] = X(i, l) for (std::size_t l = 0; l < q; ++l) { for (std::size_t i = 0; i < p; ++i) { const std::size_t row = l * p + i; for (std::size_t k = 0; k < p; ++k) M[row * n + (l * p + k)] += Aij(A11, p, i, k); for (std::size_t m = 0; m < q; ++m) M[row * n + (m * p + i)] -= Aij(A22, q, m, l); b[row] = C[i * q + l]; } } std::size_t piv[4] = {0, 1, 2, 3}; T sep_min = std::numeric_limits::max(); for (std::size_t k = 0; k < n; ++k) { std::size_t pmax = k; T amax = std::abs(M[piv[k] * n + k]); for (std::size_t i = k + 1; i < n; ++i) { T aval = std::abs(M[piv[i] * n + k]); if (aval > amax) { amax = aval; pmax = i; } } if (amax < sep_min) sep_min = amax; if (amax == T(0)) return T(0); if (pmax != k) std::swap(piv[k], piv[pmax]); for (std::size_t i = k + 1; i < n; ++i) { T factor = M[piv[i] * n + k] / M[piv[k] * n + k]; for (std::size_t j = k; j < n; ++j) M[piv[i] * n + j] -= factor * M[piv[k] * n + j]; b[piv[i]] -= factor * b[piv[k]]; } } T x[4] = {T(0)}; for (std::size_t ii = n; ii-- > 0;) { T s = b[piv[ii]]; for (std::size_t j = ii + 1; j < n; ++j) s -= M[piv[ii] * n + j] * x[j]; x[ii] = s / M[piv[ii] * n + ii]; } for (std::size_t l = 0; l < q; ++l) for (std::size_t i = 0; i < p; ++i) X_out[i * q + l] = x[l * p + i]; return sep_min; } /** * Adjacent block swap of Bai-Demmel 1993 * * Swap Tm[k:k+p, k:k+p] (A11) and Tm[k+p:k+p+q, k+p:k+p+q] (A22). * Accumulate the action into the orthogonal Z as well. p, q ∈ {1, 2}. * Returns false if it fails due to near-degeneracy. */ template bool swap_adjacent_blocks(Matrix& Tm, Matrix& Z, std::size_t k, std::size_t p, std::size_t q, T tolerance) { const std::size_t n = Tm.rows(); const std::size_t s = p + q; if (k + s > n || p == 0 || q == 0 || p > 2 || q > 2) return false; T A11[4] = {T(0)}, A12[4] = {T(0)}, A22[4] = {T(0)}, X[4] = {T(0)}; for (std::size_t i = 0; i < p; ++i) for (std::size_t j = 0; j < p; ++j) A11[i * p + j] = Tm(k + i, k + j); for (std::size_t i = 0; i < p; ++i) for (std::size_t j = 0; j < q; ++j) A12[i * q + j] = Tm(k + i, k + p + j); for (std::size_t i = 0; i < q; ++i) for (std::size_t j = 0; j < q; ++j) A22[i * q + j] = Tm(k + p + i, k + p + j); T sep = solve_sylvester_small(A11, p, A22, q, A12, X); if (sep < tolerance) return false; // V = [-X; I_q] (s×q, row-major) T V[16] = {T(0)}; for (std::size_t i = 0; i < p; ++i) for (std::size_t j = 0; j < q; ++j) V[i * q + j] = -X[i * q + j]; for (std::size_t i = 0; i < q; ++i) V[(p + i) * q + i] = T(1); // Householder QR (column by column) T uvec[2][4] = {{T(0)}}; T tau[2] = {T(0), T(0)}; std::size_t ulen[2] = {0, 0}; for (std::size_t j = 0; j < q; ++j) { const std::size_t len = s - j; ulen[j] = len; T xn2 = T(0); for (std::size_t i = 0; i < len; ++i) { T v = V[(j + i) * q + j]; xn2 += v * v; } T xn = std::sqrt(xn2); if (xn == T(0)) { tau[j] = T(0); continue; } T sign = (V[j * q + j] >= T(0)) ? T(1) : T(-1); T alpha = -sign * xn; T v0 = V[j * q + j] - alpha; if (std::abs(v0) < std::numeric_limits::epsilon() * (xn + T(1))) { tau[j] = T(0); continue; } uvec[j][0] = T(1); for (std::size_t i = 1; i < len; ++i) uvec[j][i] = V[(j + i) * q + j] / v0; T uu = T(1); for (std::size_t i = 1; i < len; ++i) uu += uvec[j][i] * uvec[j][i]; tau[j] = T(2) / uu; for (std::size_t col = j; col < q; ++col) { T w = T(0); for (std::size_t i = 0; i < len; ++i) w += uvec[j][i] * V[(j + i) * q + col]; w *= tau[j]; for (std::size_t i = 0; i < len; ++i) V[(j + i) * q + col] -= uvec[j][i] * w; } } // Apply to rows [k..k+s) of Tm from the left for (std::size_t j = 0; j < q; ++j) { if (tau[j] == T(0)) continue; const std::size_t len = ulen[j]; const std::size_t row_start = k + j; for (std::size_t col = k; col < n; ++col) { T w = T(0); for (std::size_t i = 0; i < len; ++i) w += uvec[j][i] * Tm(row_start + i, col); w *= tau[j]; for (std::size_t i = 0; i < len; ++i) Tm(row_start + i, col) -= uvec[j][i] * w; } } // Apply to columns [k..k+s) of Tm and columns [k..k+s) of Z from the right // Since Q = H_0 H_1 ... H_{q-1}, T Q = T H_0 H_1 ... — apply in forward order for (std::size_t jj = 0; jj < q; ++jj) { if (tau[jj] == T(0)) continue; const std::size_t len = ulen[jj]; const std::size_t col_start = k + jj; for (std::size_t row = 0; row < k + s; ++row) { T w = T(0); for (std::size_t i = 0; i < len; ++i) w += Tm(row, col_start + i) * uvec[jj][i]; w *= tau[jj]; for (std::size_t i = 0; i < len; ++i) Tm(row, col_start + i) -= w * uvec[jj][i]; } for (std::size_t row = 0; row < n; ++row) { T w = T(0); for (std::size_t i = 0; i < len; ++i) w += Z(row, col_start + i) * uvec[jj][i]; w *= tau[jj]; for (std::size_t i = 0; i < len; ++i) Z(row, col_start + i) -= w * uvec[jj][i]; } } // Cleanup: exactly zero out subdiagonal elements that violate the Schur form for (std::size_t i = q; i < s; ++i) for (std::size_t j = 0; j < q; ++j) Tm(k + i, k + j) = T(0); return true; } } // namespace detail_schur /** * @brief Result of the reordered real Schur decomposition */ template struct ReorderedSchur { Matrix T_mat; ///< quasi-upper-triangular (selected eigenvalues gathered top-left) Matrix Z; ///< orthogonal matrix (A = Z * T_mat * Z^T) std::size_t selected_count; ///< total number of selected eigenvalues gathered top-left }; /** * @brief Continuous-time stability: Re(λ) < 0 */ template inline bool is_stable_continuous(const Complex& lambda) { return lambda.re < T(0); } /** * @brief Discrete-time stability: |λ| < 1 */ template inline bool is_stable_discrete(const Complex& lambda) { return std::sqrt(lambda.re * lambda.re + lambda.im * lambda.im) < T(1); } /** * @brief Reordered real Schur decomposition A = Z T Z^T (gather the selected eigenvalues top-left) * * Selection sort via the adjacent block swap of Bai-Demmel 1993. * * @tparam T element type (float, double) * @tparam Selector a function object satisfying bool(const Complex&) * @param A n×n square matrix * @param select selection predicate: gather eigenvalues for which it returns true to the top-left * @return ReorderedSchur{T_mat, Z, selected_count} * * @throws MathError if the adjacent block swap fails due to near-degeneracy * * @note A building block for the Schur-method CARE/DARE solver of Laub (1979). */ template ReorderedSchur schur_decomposition_reordered( const Matrix& A, Selector select) { auto [Tm, Z] = schur_decomposition(A); const std::size_t n = Tm.rows(); const T tol = std::numeric_limits::epsilon() * T(1e3); std::size_t k = 0; while (k < n) { const std::size_t pk = schur_block_size(Tm, k); auto eigs_k = schur_block_eigenvalues(Tm, k); if (select(eigs_k.first)) { k += pk; continue; } std::size_t j = k + pk; bool found = false; while (j < n) { const std::size_t pj = schur_block_size(Tm, j); auto eigs_j = schur_block_eigenvalues(Tm, j); if (select(eigs_j.first)) { found = true; break; } j += pj; } if (!found) break; std::size_t cur = j; while (cur > k) { const std::size_t pj_now = schur_block_size(Tm, cur); std::size_t prev_start, prev_size; if (cur >= 2 && Tm(cur - 1, cur - 2) != T(0)) { prev_start = cur - 2; prev_size = 2; } else { prev_start = cur - 1; prev_size = 1; } bool ok = detail_schur::swap_adjacent_blocks( Tm, Z, prev_start, prev_size, pj_now, tol); if (!ok) { throw MathError( "schur_decomposition_reordered: adjacent block swap failed (eigenvalues too close)"); } cur = prev_start; } k += schur_block_size(Tm, k); } std::size_t cnt = 0; std::size_t pos = 0; while (pos < n) { const std::size_t bs = schur_block_size(Tm, pos); auto eigs = schur_block_eigenvalues(Tm, pos); if (!select(eigs.first)) break; cnt += bs; pos += bs; } return ReorderedSchur{std::move(Tm), std::move(Z), cnt}; } //===================================================================== // Polar decomposition //===================================================================== /** * @brief Polar decomposition: A = U_polar * H_polar * * U_polar: orthogonal matrix (nearest orthogonal matrix) * H_polar: symmetric positive semidefinite matrix * * SVD-based: A = UΣVᵀ → U_polar = UVᵀ, H_polar = VΣVᵀ * * @tparam T element type * @param A input matrix (m×n, m >= n) * @return {U_polar, H_polar} */ template requires sangi::BaseMatrixLike std::pair>, Matrix>> polar_decomposition(const MA& A) { using T = sangi::element_t; // rectangular OK (m >= n) const auto m = A.rows(); const auto n = A.cols(); if (m < n) { assert(false && "DimensionError: polar_decomposition: requires rows >= cols"); throw DimensionError("polar_decomposition: requires rows >= cols"); } // SVD: A = U * diag(sigma) * Vᵀ auto [U, sigma, Vt] = svd_decomposition(A); // U_polar = U * Vᵀ (m×n * n×n → m×n ... n×n if square) // Only for square matrices if (m != n) { assert(false && "DimensionError: polar_decomposition: currently supports square matrices only"); throw DimensionError("polar_decomposition: currently supports square matrices only"); } // U_polar = U * Vt (n×n) Matrix U_polar(n, n, T(0)); for (std::size_t i = 0; i < n; ++i) { for (std::size_t j = 0; j < n; ++j) { T sum = T(0); for (std::size_t k = 0; k < n; ++k) { sum += U(i, k) * Vt(k, j); } U_polar(i, j) = sum; } } // H_polar = Vᵀᵀ * diag(sigma) * Vᵀ = V * diag(sigma) * Vᵀ // V = Vtᵀ Matrix H_polar(n, n, T(0)); for (std::size_t i = 0; i < n; ++i) { for (std::size_t j = 0; j < n; ++j) { T sum = T(0); for (std::size_t k = 0; k < n; ++k) { sum += Vt(k, i) * sigma[k] * Vt(k, j); } H_polar(i, j) = sum; } } return {U_polar, H_polar}; } //===================================================================== // Takagi Factorization //===================================================================== /** * @brief Structure storing the result of the Takagi factorization * * Complex symmetric matrix A = U * D * U^T (note: U^T, NOT U^H) * U: unitary matrix, D: real nonnegative diagonal matrix */ template struct TakagiResult { Matrix> U; ///< unitary matrix (n×n) Vector d; ///< real nonnegative diagonal elements (= singular values) }; /** * @brief Takagi factorization: A = U * diag(d) * U^T * * Factorize a complex symmetric matrix A (A^T = A, note: A^H ≠ A in general). * d are the singular values of A (real nonnegative), U is a unitary matrix. * * Algorithm: with A = X + iY, use the SVD of a real 2n×2n matrix. * M = [[X, -Y], [Y, X]] * The singular values of M are duplicate pairs of the singular values of A. * Reference: Horn & Johnson "Matrix Analysis" 2nd ed., §4.4 * * @param A complex symmetric matrix (n×n, A^T = A) * @return TakagiResult {U, d} */ template TakagiResult takagi_factorization(const BaseMatrix>& A) { const auto n = A.rows(); if (n != A.cols()) { assert(false && "DimensionError: takagi_factorization: matrix must be square"); throw DimensionError("takagi_factorization: matrix must be square"); } if (n == 0) { assert(false && "DimensionError: takagi_factorization: empty matrix"); throw DimensionError("takagi_factorization: empty matrix"); } // Symmetry check (A^T = A, not A^H = A) for (std::size_t i = 0; i < n; ++i) for (std::size_t j = i + 1; j < n; ++j) { auto diff = A(i, j) - A(j, i); double d_abs = std::sqrt(static_cast(diff.re * diff.re + diff.im * diff.im)); double scale = std::sqrt(static_cast(A(i, j).re * A(i, j).re + A(i, j).im * A(i, j).im)); if (d_abs > std::max(scale, 1.0) * static_cast(n) * static_cast(std::numeric_limits::epsilon()) + 1e-14) throw MathError("takagi_factorization: matrix must be symmetric (A^T = A)"); } // Extract the real part X and imaginary part Y Matrix X(n, n, T(0)); Matrix Y(n, n, T(0)); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) { X(i, j) = A(i, j).re; Y(i, j) = A(i, j).im; } // Build the real 2n×2n matrix M = [[X, -Y], [Y, X]] Matrix M(2 * n, 2 * n, T(0)); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) { M(i, j) = X(i, j); M(i, n + j) = -Y(i, j); M(n + i, j) = Y(i, j); M(n + i, n + j) = X(i, j); } // SVD(M) = U_real * Sigma * V_real^T auto [U_real, sigma_real, Vt_real] = svd_decomposition(M); // Singular values appear in duplicate pairs: σ₁, σ₁, σ₂, σ₂, ... // Take the even-indexed singular values Vector d(n); for (std::size_t i = 0; i < n; ++i) d[i] = sigma_real[2 * i]; // Build the unitary matrix U from the upper and lower halves of U_real // Use column 2k of U_real: z_k = U_real(0:n, 2k) + i * U_real(n:2n, 2k) Matrix> U(n, n, Complex(T(0))); for (std::size_t k = 0; k < n; ++k) { for (std::size_t i = 0; i < n; ++i) { U(i, k) = Complex(U_real(i, 2 * k), U_real(n + i, 2 * k)); } } // Phase correction: use the Takagi relation A * conj(u_k) = σ_k * u_k // If z_k differs from the correct u_k by a phase φ (z_k = u_k * e^{iφ}) then: // A * conj(z_k) = A * conj(u_k) * e^{-iφ} = σ_k * u_k * e^{-iφ} // = σ_k * z_k * e^{-2iφ} // Hence w_k = A * conj(z_k) / (σ_k * z_k) = e^{-2iφ} // → φ = -arg(w_k) / 2, correction: z_k ← z_k * e^{-iφ} for (std::size_t k = 0; k < n; ++k) { if (d[k] < std::numeric_limits::epsilon() * T(10)) continue; // w = A * conj(z_k) Vector> w(n, Complex(T(0))); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) w[i] = w[i] + A(i, j) * conj(U(j, k)); // Estimate the phase from the largest component of z_k std::size_t max_idx = 0; T max_abs = T(0); for (std::size_t i = 0; i < n; ++i) { T a = sangi::abs(U(i, k)); if (a > max_abs) { max_abs = a; max_idx = i; } } if (max_abs < std::numeric_limits::epsilon()) continue; // ratio = w[max_idx] / (σ_k * z_k[max_idx]) = e^{-2iφ} Complex ratio = w[max_idx] / (Complex(d[k]) * U(max_idx, k)); T phase_minus2phi = static_cast(std::atan2( static_cast(ratio.im), static_cast(ratio.re))); T phi = -phase_minus2phi / T(2); // z_k ← z_k * e^{-iφ} Complex rot(static_cast(std::cos(static_cast(-phi))), static_cast(std::sin(static_cast(-phi)))); for (std::size_t i = 0; i < n; ++i) U(i, k) = U(i, k) * rot; } return { std::move(U), std::move(d) }; } //===================================================================== // CS decomposition (Cosine-Sine Decomposition) //===================================================================== /** * @brief Result of the CS decomposition * * Decompose an orthogonal matrix Q as follows: * Q = [[U1, 0], [0, U2]] * Σ * [[V1, 0], [0, V2]]^T * * When p ≤ q, the central matrix Σ (m×m) is: * Σ = [[C, -S, 0 ], * [S, C, 0 ], * [0, 0, I_{q-p}]] * where C = diag(cos θ_i), S = diag(sin θ_i), C² + S² = I */ template struct CSDecompositionResult { Matrix U1; ///< p×p orthogonal matrix Matrix U2; ///< q×q orthogonal matrix (q = m - p) Matrix V1; ///< p×p orthogonal matrix Matrix V2; ///< q×q orthogonal matrix Vector theta; ///< r = min(p,q) angles θ_i ∈ [0, π/2] }; namespace detail_cs { // Orthogonal completion: find the remaining columns of M (n×n) orthogonal to the known n_known columns template void orthogonal_completion(Matrix& M, std::size_t n_known) { std::size_t n = M.rows(); std::size_t filled = n_known; for (std::size_t trial = 0; trial < n && filled < n; ++trial) { // Candidate vector e_{trial} Vector v(n, T(0)); v[trial] = T(1); // Remove the projection onto the known columns for (std::size_t j = 0; j < filled; ++j) { T dot = T(0); for (std::size_t i = 0; i < n; ++i) dot += M(i, j) * v[i]; for (std::size_t i = 0; i < n; ++i) v[i] -= dot * M(i, j); } // Compute the norm T norm_sq = T(0); for (std::size_t i = 0; i < n; ++i) norm_sq += v[i] * v[i]; T norm = std::sqrt(norm_sq); if (norm > T(1e-10)) { for (std::size_t i = 0; i < n; ++i) M(i, filled) = v[i] / norm; ++filled; } } } } // namespace detail_cs /** * @brief CS decomposition (Cosine-Sine Decomposition) * * Decompose an orthogonal matrix Q (m×m) with partition size p. * Based on the SVD of Q's top-left block Q11 (p×p), * decompose into unitary block-diagonal factors and cosine/sine diagonal matrices. * * @param Q orthogonal matrix (m×m) * @param p partition size (m/2 if 0). 0 < p < m * @return CSDecompositionResult * @throws DimensionError non-square matrix, empty matrix, invalid partition size */ namespace detail_cs { // Internal implementation for p ≤ q: // Obtain U1, V1, theta from SVD(Q11) and build U2, V2 from Q21, Q12, Q22 template void solve_smaller_block( const BaseMatrix& Q11_basemat, const BaseMatrix& Q12_basemat, const BaseMatrix& Q21_basemat, const BaseMatrix& Q22_basemat, std::size_t p, std::size_t q, Matrix& U1, Matrix& U2, Matrix& V1, Matrix& V2, Vector& theta) { Matrix Q11(Q11_basemat); // 1 copy via BaseMatrix ctor Matrix Q12(Q12_basemat); // 1 copy via BaseMatrix ctor Matrix Q21(Q21_basemat); // 1 copy via BaseMatrix ctor Matrix Q22(Q22_basemat); // 1 copy via BaseMatrix ctor std::size_t r = p; // min(p, q) = p // SVD(Q11) = U1 * diag(σ) * V1t Vector sigma; Matrix V1t; std::tie(U1, sigma, V1t) = svd_decomposition(Q11); V1 = V1t.transpose(); // θ_i = arccos(σ_i) theta = Vector(r); Vector cos_th(r), sin_th(r); for (std::size_t i = 0; i < r; ++i) { T c = std::clamp(sigma[i], T(0), T(1)); cos_th[i] = c; sin_th[i] = std::sqrt(T(1) - c * c); theta[i] = static_cast(std::acos(static_cast(c))); } // W = Q21 * V1 (q×p): column i ≈ sin(θ_i) * U2(:,i) Matrix W(q, p, T(0)); for (std::size_t i = 0; i < q; ++i) for (std::size_t j = 0; j < p; ++j) for (std::size_t k = 0; k < p; ++k) W(i, j) += Q21(i, k) * V1(k, j); // N = Q12^T * U1 (q×p): column i ≈ -sin(θ_i) * V2(:,i) Matrix N(q, p, T(0)); for (std::size_t i = 0; i < q; ++i) for (std::size_t j = 0; j < p; ++j) for (std::size_t k = 0; k < p; ++k) N(i, j) += Q12(k, i) * U1(k, j); const T eps = std::numeric_limits::epsilon() * T(100); U2 = Matrix(q, q, T(0)); V2 = Matrix(q, q, T(0)); std::size_t n_good = 0; std::vector col_filled(q, false); for (std::size_t i = 0; i < r; ++i) { if (sin_th[i] > eps) { for (std::size_t row = 0; row < q; ++row) { U2(row, i) = W(row, i) / sin_th[i]; V2(row, i) = -N(row, i) / sin_th[i]; } col_filled[i] = true; ++n_good; } } // Determine the remaining columns from the residual of Q22 if (n_good < q) { Matrix R(q, q, T(0)); for (std::size_t a = 0; a < q; ++a) for (std::size_t b = 0; b < q; ++b) R(a, b) = Q22(a, b); for (std::size_t i = 0; i < r; ++i) { if (col_filled[i]) { T ci = cos_th[i]; for (std::size_t a = 0; a < q; ++a) for (std::size_t b = 0; b < q; ++b) R(a, b) -= ci * U2(a, i) * V2(b, i); } } auto [Ur, sr, Vrt] = svd_decomposition(R); std::size_t svd_idx = 0; for (std::size_t i = 0; i < q && svd_idx < q; ++i) { if (!col_filled[i]) { for (std::size_t row = 0; row < q; ++row) { U2(row, i) = Ur(row, svd_idx); V2(row, i) = Vrt(svd_idx, row); } ++svd_idx; } } } } } // namespace detail_cs (additional part) template CSDecompositionResult cs_decomposition(const BaseMatrix& Q, std::size_t p = 0) { std::size_t m = Q.rows(); if (m < 2 || Q.cols() != m) { assert(false && "DimensionError: cs_decomposition: requires a square matrix of size >= 2"); throw DimensionError("cs_decomposition: requires a square matrix of size >= 2"); } if (p == 0) p = m / 2; if (p == 0 || p >= m) { assert(false && "DimensionError: cs_decomposition: partition size p must satisfy 0 < p < m"); throw DimensionError("cs_decomposition: partition size p must satisfy 0 < p < m"); } std::size_t q = m - p; // Block extraction Matrix Q11(p, p, T(0)), Q12(p, q, T(0)); Matrix Q21(q, p, T(0)), Q22(q, q, T(0)); for (std::size_t i = 0; i < p; ++i) { for (std::size_t j = 0; j < p; ++j) Q11(i, j) = Q(i, j); for (std::size_t j = 0; j < q; ++j) Q12(i, j) = Q(i, p + j); } for (std::size_t i = 0; i < q; ++i) { for (std::size_t j = 0; j < p; ++j) Q21(i, j) = Q(p + i, j); for (std::size_t j = 0; j < q; ++j) Q22(i, j) = Q(p + i, p + j); } Matrix U1, U2, V1, V2; Vector theta; if (p <= q) { // p ≤ q: build U2, V2 starting from SVD(Q11) (p×p) detail_cs::solve_smaller_block(Q11, Q12, Q21, Q22, p, q, U1, U2, V1, V2, theta); } else { // p > q: build U1, V1 starting from SVD(Q22) (q×q) // Q22 = U2 * C * V2^T (C = diag(cos θ), q×q) std::size_t r = q; Vector sigma; Matrix V2t; std::tie(U2, sigma, V2t) = svd_decomposition(Q22); V2 = V2t.transpose(); theta = Vector(r); Vector cos_th(r), sin_th(r); for (std::size_t i = 0; i < r; ++i) { T c = std::clamp(sigma[i], T(0), T(1)); cos_th[i] = c; sin_th[i] = std::sqrt(T(1) - c * c); theta[i] = static_cast(std::acos(static_cast(c))); } // W = Q12 * V2 (p×q): column i = -sin(θ_i) * U1(:,i) Matrix W(p, q, T(0)); for (std::size_t i = 0; i < p; ++i) for (std::size_t j = 0; j < q; ++j) for (std::size_t k = 0; k < q; ++k) W(i, j) += Q12(i, k) * V2(k, j); // N = Q21^T * U2 (p×q): column i = +sin(θ_i) * V1(:,i) Matrix N(p, q, T(0)); for (std::size_t i = 0; i < p; ++i) for (std::size_t j = 0; j < q; ++j) for (std::size_t k = 0; k < q; ++k) N(i, j) += Q21(k, i) * U2(k, j); const T eps = std::numeric_limits::epsilon() * T(100); U1 = Matrix(p, p, T(0)); V1 = Matrix(p, p, T(0)); std::size_t n_good = 0; std::vector col_filled(p, false); for (std::size_t i = 0; i < r; ++i) { if (sin_th[i] > eps) { for (std::size_t row = 0; row < p; ++row) { U1(row, i) = -W(row, i) / sin_th[i]; // -(-sin * u1) / sin = u1 V1(row, i) = N(row, i) / sin_th[i]; // (+sin * v1) / sin = v1 } col_filled[i] = true; ++n_good; } } // Determine the remaining columns from the residual of Q11 // Q11 = U1 * [[C,0],[0,I_{p-q}]] * V1^T if (n_good < p) { Matrix R(p, p, T(0)); for (std::size_t a = 0; a < p; ++a) for (std::size_t b = 0; b < p; ++b) R(a, b) = Q11(a, b); for (std::size_t i = 0; i < r; ++i) { if (col_filled[i]) { T ci = cos_th[i]; for (std::size_t a = 0; a < p; ++a) for (std::size_t b = 0; b < p; ++b) R(a, b) -= ci * U1(a, i) * V1(b, i); } } auto [Ur, sr, Vrt] = svd_decomposition(R); std::size_t svd_idx = 0; for (std::size_t i = 0; i < p && svd_idx < p; ++i) { if (!col_filled[i]) { for (std::size_t row = 0; row < p; ++row) { U1(row, i) = Ur(row, svd_idx); V1(row, i) = Vrt(svd_idx, row); } ++svd_idx; } } } } return { std::move(U1), std::move(U2), std::move(V1), std::move(V2), std::move(theta) }; } //===================================================================== // Rank Decomposition //===================================================================== /** * @brief Result of the rank decomposition * * A = B * C (B: m×r, C: r×n, r = rank(A)) * SVD-based: B = U_r * diag(σ_r), C = V_r^T * (U_r: the first r columns, σ_r: the nonzero singular values, V_r^T: the first r rows) */ template struct RankDecompositionResult { Matrix B; ///< m×r matrix (column-space basis × singular values) Matrix C; ///< r×n matrix (row-space basis) std::size_t rank; ///< numerical rank }; /** * @brief Rank Decomposition * * Decompose a matrix A (m×n) into A = B * C (B: m×r, C: r×n). * Uses SVD and determines the numerical rank r by a threshold. * * @param A input matrix (m×n) * @param rank specified rank (auto-detected if 0) * @param tol rank-determination threshold (max(m,n) * σ_max * ε if 0) * @return RankDecompositionResult * @throws DimensionError empty matrix * @throws MathError if the specified rank is larger than the actual rank */ template RankDecompositionResult rank_decomposition( const BaseMatrix& A, std::size_t rank = 0, T tol = T(0)) { std::size_t m = A.rows(); std::size_t n = A.cols(); if (m == 0 || n == 0) { assert(false && "DimensionError: rank_decomposition: empty matrix"); throw DimensionError("rank_decomposition: empty matrix"); } auto [U, sigma, Vt] = svd_decomposition(A); // Rank determination std::size_t k = sigma.size(); // min(m, n) if (tol <= T(0)) { // Default threshold: max(m,n) * σ_max * machine_epsilon T sigma_max = (k > 0) ? sigma[0] : T(0); tol = static_cast(std::max(m, n)) * sigma_max * std::numeric_limits::epsilon(); } std::size_t r = 0; for (std::size_t i = 0; i < k; ++i) { if (sigma[i] > tol) ++r; else break; // σ is in descending order } if (rank > 0) { if (rank > r) throw MathError("rank_decomposition: specified rank exceeds numerical rank"); r = rank; } // Case r == 0 (zero matrix) if (r == 0) { return { Matrix(m, 0), Matrix(0, n), 0 }; } // B = U(:, 0:r) * diag(σ(0:r)) (m×r) Matrix B(m, r, T(0)); for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < r; ++j) B(i, j) = U(i, j) * sigma[j]; // C = Vt(0:r, :) (r×n) Matrix C(r, n, T(0)); for (std::size_t i = 0; i < r; ++i) for (std::size_t j = 0; j < n; ++j) C(i, j) = Vt(i, j); return { std::move(B), std::move(C), r }; } //===================================================================== // CUR Decomposition //===================================================================== /** * @brief Result of the CUR decomposition * * A ≈ C * U * R * C: m×c matrix (the selected c columns of A) * U: c×r linking matrix * R: r×n matrix (the selected r rows of A) * col_indices: indices of the selected columns * row_indices: indices of the selected rows */ template struct CURDecompositionResult { Matrix C; ///< m×c matrix (selected columns) Matrix U; ///< c×r linking matrix Matrix R; ///< r×n matrix (selected rows) std::vector col_indices; ///< indices of the selected columns std::vector row_indices; ///< indices of the selected rows }; /** * @brief CUR Decomposition * * Decompose a matrix A (m×n) into A ≈ C * U * R. * Deterministic column/row selection based on SVD leverage scores. * C is a column subset of A, R is a row subset of A, * U = C⁺ * A * R⁺ (linking via pseudo-inverses). * * @param A input matrix (m×n) * @param k number of columns/rows to select (uses the numerical rank if 0) * @return CURDecompositionResult * @throws DimensionError empty matrix * @throws MathError if k exceeds min(m,n) */ template CURDecompositionResult cur_decomposition( const BaseMatrix& A, std::size_t k = 0) { std::size_t m = A.rows(); std::size_t n = A.cols(); if (m == 0 || n == 0) { assert(false && "DimensionError: cur_decomposition: empty matrix"); throw DimensionError("cur_decomposition: empty matrix"); } auto [U_svd, sigma, Vt] = svd_decomposition(A); std::size_t s = sigma.size(); // min(m, n) // Rank determination (automatic if k=0) if (k == 0) { T sigma_max = (s > 0) ? sigma[0] : T(0); T tol = static_cast(std::max(m, n)) * sigma_max * std::numeric_limits::epsilon(); k = 0; for (std::size_t i = 0; i < s; ++i) { if (sigma[i] > tol) ++k; else break; } if (k == 0) k = 1; // at least 1 column/row } if (k > std::min(m, n)) throw MathError("cur_decomposition: k exceeds min(m, n)"); // Column selection by leverage score (sum of squares of the top-k components of the right singular vectors) // col_score[j] = Σ_{i> col_scores(n); for (std::size_t j = 0; j < n; ++j) { T score = T(0); for (std::size_t i = 0; i < k && i < s; ++i) score += Vt(i, j) * Vt(i, j); col_scores[j] = { score, j }; } std::sort(col_scores.begin(), col_scores.end(), [](const auto& a, const auto& b) { return a.first > b.first; }); std::size_t c = std::min(k, n); std::vector col_idx(c); for (std::size_t i = 0; i < c; ++i) col_idx[i] = col_scores[i].second; std::sort(col_idx.begin(), col_idx.end()); // Row selection by leverage score (sum of squares of the top-k components of the left singular vectors) // row_score[i] = Σ_{j> row_scores(m); for (std::size_t i = 0; i < m; ++i) { T score = T(0); for (std::size_t j = 0; j < k && j < s; ++j) score += U_svd(i, j) * U_svd(i, j); row_scores[i] = { score, i }; } std::sort(row_scores.begin(), row_scores.end(), [](const auto& a, const auto& b) { return a.first > b.first; }); std::size_t r = std::min(k, m); std::vector row_idx(r); for (std::size_t i = 0; i < r; ++i) row_idx[i] = row_scores[i].second; std::sort(row_idx.begin(), row_idx.end()); // C: the selected columns of A (m×c) Matrix C_mat(m, c, T(0)); for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < c; ++j) C_mat(i, j) = A(i, col_idx[j]); // R: the selected rows of A (r×n) Matrix R_mat(r, n, T(0)); for (std::size_t i = 0; i < r; ++i) for (std::size_t j = 0; j < n; ++j) R_mat(i, j) = A(row_idx[i], j); // U = C⁺ * A * R⁺ // C⁺ (c×m) and R⁺ (n×r) are pseudo-inverses via SVD // W = the selected-rows × selected-columns submatrix of A (r×c) // U = W⁺ (the pseudo-inverse of W) Matrix W(r, c, T(0)); for (std::size_t i = 0; i < r; ++i) for (std::size_t j = 0; j < c; ++j) W(i, j) = A(row_idx[i], col_idx[j]); // SVD of W → pseudo-inverse auto [Uw, sw, Vwt] = svd_decomposition(W); T sw_max = (sw.size() > 0) ? sw[0] : T(0); T pinv_tol = static_cast(std::max(r, c)) * sw_max * std::numeric_limits::epsilon(); // W⁺ = Vw * diag(1/σ) * Uw^T (c×r) std::size_t sw_size = sw.size(); Matrix U_mat(c, r, T(0)); for (std::size_t i = 0; i < c; ++i) for (std::size_t j = 0; j < r; ++j) { T val = T(0); for (std::size_t t = 0; t < sw_size; ++t) { if (sw[t] > pinv_tol) val += Vwt(t, i) * (T(1) / sw[t]) * Uw(j, t); } U_mat(i, j) = val; } return { std::move(C_mat), std::move(U_mat), std::move(R_mat), std::move(col_idx), std::move(row_idx) }; } //===================================================================== // NMF (Non-negative Matrix Factorization) //===================================================================== /** * @brief Result of NMF * * A ≈ W * H (W: m×k, H: k×n, W,H ≥ 0) */ template struct NMFResult { Matrix W; ///< m×k basis matrix (nonnegative) Matrix H; ///< k×n coefficient matrix (nonnegative) T residual; ///< Frobenius-norm residual ‖A - WH‖_F std::size_t iterations; ///< number of iterations performed }; /** * @brief NMF (Non-negative Matrix Factorization) * * Decompose a nonnegative matrix A (m×n) into A ≈ W * H (W: m×k, H: k×n, W,H ≥ 0). * Uses the Lee-Seung multiplicative update rule. * * @param A input matrix (m×n, all elements ≥ 0) * @param k number of factors (1 ≤ k ≤ min(m, n)) * @param max_iter maximum number of iterations (default 200) * @param tol convergence threshold (relative change of residual, default 1e-4) * @return NMFResult * @throws DimensionError empty matrix, invalid k * @throws MathError if negative elements are present */ template NMFResult nmf( const BaseMatrix& A, std::size_t k, std::size_t max_iter = 200, T tol = T(1e-4)) { std::size_t m = A.rows(); std::size_t n = A.cols(); if (m == 0 || n == 0) { assert(false && "DimensionError: nmf: empty matrix"); throw DimensionError("nmf: empty matrix"); } if (k == 0 || k > std::min(m, n)) { assert(false && "DimensionError: nmf: k must satisfy 1 <= k <= min(m, n)"); throw DimensionError("nmf: k must satisfy 1 <= k <= min(m, n)"); } // Nonnegativity check for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < n; ++j) if (A(i, j) < T(0)) throw MathError("nmf: matrix must be non-negative"); const T eps_small = std::numeric_limits::epsilon(); // Initialize W, H via SVD-based NNDSVD // (Non-Negative Double SVD: Boutsidis & Gallopoulos 2008) auto [U_svd, sigma_svd, Vt_svd] = svd_decomposition(A); Matrix W(m, k, T(0)); Matrix H(k, n, T(0)); // 0-th factor: sqrt(σ_0) * |u_0|, sqrt(σ_0) * |v_0| if (sigma_svd.size() > 0 && sigma_svd[0] > T(0)) { T sq = std::sqrt(sigma_svd[0]); for (std::size_t i = 0; i < m; ++i) W(i, 0) = sq * std::abs(U_svd(i, 0)); for (std::size_t j = 0; j < n; ++j) H(0, j) = sq * std::abs(Vt_svd(0, j)); } else { // For a zero matrix: initialize with a small positive value for (std::size_t i = 0; i < m; ++i) W(i, 0) = eps_small; for (std::size_t j = 0; j < n; ++j) H(0, j) = eps_small; } // t-th factor (t >= 1): NNDSVD for (std::size_t t = 1; t < k; ++t) { if (t >= sigma_svd.size() || sigma_svd[t] <= T(0)) { for (std::size_t i = 0; i < m; ++i) W(i, t) = eps_small; for (std::size_t j = 0; j < n; ++j) H(t, j) = eps_small; continue; } // u⁺, u⁻, v⁺, v⁻ Vector up(m, T(0)), un(m, T(0)); Vector vp(n, T(0)), vn(n, T(0)); for (std::size_t i = 0; i < m; ++i) { T val = U_svd(i, t); if (val > T(0)) up[i] = val; else un[i] = -val; } for (std::size_t j = 0; j < n; ++j) { T val = Vt_svd(t, j); if (val > T(0)) vp[j] = val; else vn[j] = -val; } T up_norm = T(0), un_norm = T(0); T vp_norm = T(0), vn_norm = T(0); for (std::size_t i = 0; i < m; ++i) { up_norm += up[i]*up[i]; un_norm += un[i]*un[i]; } for (std::size_t j = 0; j < n; ++j) { vp_norm += vp[j]*vp[j]; vn_norm += vn[j]*vn[j]; } up_norm = std::sqrt(up_norm); un_norm = std::sqrt(un_norm); vp_norm = std::sqrt(vp_norm); vn_norm = std::sqrt(vn_norm); T mp = up_norm * vp_norm; T mn = un_norm * vn_norm; T sq = std::sqrt(sigma_svd[t]); if (mp >= mn) { T scale_w = (up_norm > T(0)) ? sq * std::sqrt(mp) / up_norm : T(0); T scale_h = (vp_norm > T(0)) ? sq * std::sqrt(mp) / vp_norm : T(0); for (std::size_t i = 0; i < m; ++i) W(i, t) = std::max(up[i] * scale_w, eps_small); for (std::size_t j = 0; j < n; ++j) H(t, j) = std::max(vp[j] * scale_h, eps_small); } else { T scale_w = (un_norm > T(0)) ? sq * std::sqrt(mn) / un_norm : T(0); T scale_h = (vn_norm > T(0)) ? sq * std::sqrt(mn) / vn_norm : T(0); for (std::size_t i = 0; i < m; ++i) W(i, t) = std::max(un[i] * scale_w, eps_small); for (std::size_t j = 0; j < n; ++j) H(t, j) = std::max(vn[j] * scale_h, eps_small); } } // Lee-Seung multiplicative update rule T prev_residual = std::numeric_limits::max(); std::size_t iter = 0; for (; iter < max_iter; ++iter) { // Update H: H ← H * (W^T A) / (W^T W H + ε) // WtA = W^T * A (k×n) Matrix WtA(k, n, T(0)); for (std::size_t i = 0; i < k; ++i) for (std::size_t j = 0; j < n; ++j) for (std::size_t t = 0; t < m; ++t) WtA(i, j) += W(t, i) * A(t, j); // WtW = W^T * W (k×k) Matrix WtW(k, k, T(0)); for (std::size_t i = 0; i < k; ++i) for (std::size_t j = 0; j < k; ++j) for (std::size_t t = 0; t < m; ++t) WtW(i, j) += W(t, i) * W(t, j); // WtWH = WtW * H (k×n) Matrix WtWH(k, n, T(0)); for (std::size_t i = 0; i < k; ++i) for (std::size_t j = 0; j < n; ++j) for (std::size_t t = 0; t < k; ++t) WtWH(i, j) += WtW(i, t) * H(t, j); for (std::size_t i = 0; i < k; ++i) for (std::size_t j = 0; j < n; ++j) H(i, j) = H(i, j) * WtA(i, j) / (WtWH(i, j) + eps_small); // Update W: W ← W * (A H^T) / (W H H^T + ε) // AHt = A * H^T (m×k) Matrix AHt(m, k, T(0)); for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < k; ++j) for (std::size_t t = 0; t < n; ++t) AHt(i, j) += A(i, t) * H(j, t); // HHt = H * H^T (k×k) Matrix HHt(k, k, T(0)); for (std::size_t i = 0; i < k; ++i) for (std::size_t j = 0; j < k; ++j) for (std::size_t t = 0; t < n; ++t) HHt(i, j) += H(i, t) * H(j, t); // WHHt = W * HHt (m×k) Matrix WHHt(m, k, T(0)); for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < k; ++j) for (std::size_t t = 0; t < k; ++t) WHHt(i, j) += W(i, t) * HHt(t, j); for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < k; ++j) W(i, j) = W(i, j) * AHt(i, j) / (WHHt(i, j) + eps_small); // Compute the residual ‖A - WH‖_F T residual = T(0); for (std::size_t i = 0; i < m; ++i) { for (std::size_t j = 0; j < n; ++j) { T wh = T(0); for (std::size_t t = 0; t < k; ++t) wh += W(i, t) * H(t, j); T d = A(i, j) - wh; residual += d * d; } } residual = std::sqrt(residual); // Convergence test if (prev_residual > T(0) && std::abs(prev_residual - residual) / prev_residual < tol) { prev_residual = residual; ++iter; break; } prev_residual = residual; } return { std::move(W), std::move(H), prev_residual, iter }; } //===================================================================== // UTV Decomposition //===================================================================== /** * @brief Result of the UTV decomposition * * A = U * T * V^T * U: m×m orthogonal matrix * T: m×n upper triangular matrix (diagonal approximates the singular values, descending) * V: n×n orthogonal matrix * rank: numerical rank */ template struct UTVDecompositionResult { Matrix U; ///< m×m orthogonal matrix Matrix T_mat; ///< m×n upper triangular matrix Matrix V; ///< n×n orthogonal matrix std::size_t rank; ///< numerical rank }; /** * @brief UTV Decomposition * * Decompose a matrix A (m×n) into A = U * T * V^T. * Built on column-pivoted QR decomposition, * T's diagonal approximates the singular values. A lower-cost rank determination than SVD. * * Algorithm: * 1. Column-pivoted QR: A*P = Q*R * 2. Refine V with the right singular vectors of R: R = U_R * Σ * V_R^T * 3. U = Q * U_R, T = Σ (singular values on the diagonal), V = P * V_R * * @param A input matrix (m×n) * @param tol rank-determination threshold (0: automatic) * @return UTVDecompositionResult * @throws DimensionError empty matrix */ template UTVDecompositionResult utv_decomposition( const BaseMatrix& A, T tol = T(0)) { std::size_t m = A.rows(); std::size_t n = A.cols(); if (m == 0 || n == 0) { assert(false && "DimensionError: utv_decomposition: empty matrix"); throw DimensionError("utv_decomposition: empty matrix"); } std::size_t k = std::min(m, n); // Column-pivoted QR: select columns in order of largest column norm // Working copy of A Matrix W(A); std::vector piv(n); for (std::size_t i = 0; i < n; ++i) piv[i] = i; // Compute the column norms Vector col_norms(n, T(0)); for (std::size_t j = 0; j < n; ++j) for (std::size_t i = 0; i < m; ++i) col_norms[j] += W(i, j) * W(i, j); // Householder QR with column pivoting Vector tau(k, T(0)); // Householder coefficients for (std::size_t step = 0; step < k; ++step) { // Search for the column with the largest norm std::size_t max_col = step; T max_norm = col_norms[step]; for (std::size_t j = step + 1; j < n; ++j) { if (col_norms[j] > max_norm) { max_norm = col_norms[j]; max_col = j; } } // Column swap if (max_col != step) { for (std::size_t i = 0; i < m; ++i) std::swap(W(i, step), W(i, max_col)); std::swap(col_norms[step], col_norms[max_col]); std::swap(piv[step], piv[max_col]); } // Eliminate the lower triangle of column step via Householder transformation // v = W(step:m, step), β = householder_beta T norm_x = T(0); for (std::size_t i = step; i < m; ++i) norm_x += W(i, step) * W(i, step); norm_x = std::sqrt(norm_x); if (norm_x < std::numeric_limits::epsilon()) { tau[step] = T(0); continue; } T sign = (W(step, step) >= T(0)) ? T(1) : T(-1); T alpha = -sign * norm_x; T v0 = W(step, step) - alpha; tau[step] = -v0 / alpha; // equivalent to β = 2 / (v^T v) // Normalize v (v[0] = 1) if (std::abs(v0) > std::numeric_limits::epsilon()) { T inv_v0 = T(1) / v0; for (std::size_t i = step + 1; i < m; ++i) W(i, step) *= inv_v0; } W(step, step) = alpha; // Apply the Householder transformation to the remaining columns for (std::size_t j = step + 1; j < n; ++j) { T dot = W(step, j); for (std::size_t i = step + 1; i < m; ++i) dot += W(i, step) * W(i, j); dot *= tau[step]; W(step, j) -= dot; for (std::size_t i = step + 1; i < m; ++i) W(i, j) -= W(i, step) * dot; } // Update the column norms for (std::size_t j = step + 1; j < n; ++j) col_norms[j] -= W(step, j) * W(step, j); } // Extract R (k×n upper triangular) Matrix R(k, n, T(0)); for (std::size_t i = 0; i < k; ++i) for (std::size_t j = i; j < n; ++j) R(i, j) = W(i, j); // Build Q (m×m): accumulation of the Householder transformations Matrix Q = Matrix::identity(m); for (std::size_t step = k; step-- > 0; ) { if (std::abs(tau[step]) < std::numeric_limits::epsilon()) continue; // v = [1, W(step+1:m, step)] for (std::size_t j = 0; j < m; ++j) { T dot = Q(step, j); for (std::size_t i = step + 1; i < m; ++i) dot += W(i, step) * Q(i, j); dot *= tau[step]; Q(step, j) -= dot; for (std::size_t i = step + 1; i < m; ++i) Q(i, j) -= W(i, step) * dot; } } // SVD of R: R = U_R * Σ * V_R^T auto [U_R, sigma, V_Rt] = svd_decomposition(R); // Rank determination if (tol <= T(0)) { T sigma_max = (sigma.size() > 0) ? sigma[0] : T(0); tol = static_cast(std::max(m, n)) * sigma_max * std::numeric_limits::epsilon(); } std::size_t rank = 0; for (std::size_t i = 0; i < sigma.size(); ++i) { if (sigma[i] > tol) ++rank; else break; } // Build the T matrix (m×n): top k×n is Σ (singular values on the diagonal + zeros above) Matrix T_mat(m, n, T(0)); for (std::size_t i = 0; i < sigma.size(); ++i) T_mat(i, i) = sigma[i]; // U = Q * U_R_ext: Q (m×m) * U_R_ext (m×m) // U_R is the left singular vectors of SVD(R) (k×k or k×min(k,n)) // U_R_ext: U_R top-left, I_{m-k} bottom-right std::size_t ur_rows = U_R.rows(); std::size_t ur_cols = U_R.cols(); Matrix U_full(m, m, T(0)); for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < m; ++j) { T val = T(0); for (std::size_t t = 0; t < m; ++t) { T ur_val; if (t < ur_rows && j < ur_cols) ur_val = U_R(t, j); else ur_val = (t == j) ? T(1) : T(0); val += Q(i, t) * ur_val; } U_full(i, j) = val; } // V = P * V_R: apply the pivot permutation // V_Rt is the transposed right singular vectors of SVD(R). When R is k×n, // V_Rt is min(k,n)×n or n×n (depending on the SVD implementation) // V_R = V_Rt^T: V_R(i,j) = V_Rt(j,i) Matrix V_full(n, n, T(0)); std::size_t vrt_rows = V_Rt.rows(); std::size_t vrt_cols = V_Rt.cols(); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) { T vr_val; if (j < vrt_rows && i < vrt_cols) vr_val = V_Rt(j, i); else vr_val = (i == j) ? T(1) : T(0); V_full(piv[i], j) = vr_val; } return { std::move(U_full), std::move(T_mat), std::move(V_full), rank }; } //===================================================================== // RRQR (Rank-Revealing QR) //===================================================================== /** * @brief Result of RRQR * * A * P = Q * R (Q: m×m orthogonal, R: m×n upper triangular, P: n×n permutation) * R's diagonal approximates the singular values and reveals the rank. * A simplified version of Strong RRQR (Gu-Eisenstat): column-pivoted QR + * rank determination in post-processing. */ template struct RRQRResult { Matrix Q; ///< m×m orthogonal matrix Matrix R; ///< m×n upper triangular matrix std::vector perm; ///< column permutation (perm[j] = original column index) std::size_t rank; ///< numerical rank }; /** * @brief RRQR (Rank-Revealing QR) * * Apply column-pivoted QR decomposition to a matrix A (m×n) so that * R(k,k) becomes a good approximation of the singular values. * * @param A input matrix (m×n) * @param tol rank-determination threshold (0: automatic = max(m,n) * ‖A‖ * ε) * @return RRQRResult * @throws DimensionError empty matrix */ template RRQRResult rrqr(const BaseMatrix& A, T tol = T(0)) { std::size_t m = A.rows(); std::size_t n = A.cols(); if (m == 0 || n == 0) { assert(false && "DimensionError: rrqr: empty matrix"); throw DimensionError("rrqr: empty matrix"); } std::size_t k = std::min(m, n); // Working copy Matrix W(A); std::vector piv(n); for (std::size_t i = 0; i < n; ++i) piv[i] = i; // Column norms Vector col_norms(n, T(0)); for (std::size_t j = 0; j < n; ++j) for (std::size_t i = 0; i < m; ++i) col_norms[j] += W(i, j) * W(i, j); // Householder coefficients Vector tau(k, T(0)); for (std::size_t step = 0; step < k; ++step) { // Select the column with the largest norm std::size_t max_col = step; T max_norm = col_norms[step]; for (std::size_t j = step + 1; j < n; ++j) { if (col_norms[j] > max_norm) { max_norm = col_norms[j]; max_col = j; } } // Column swap if (max_col != step) { for (std::size_t i = 0; i < m; ++i) std::swap(W(i, step), W(i, max_col)); std::swap(col_norms[step], col_norms[max_col]); std::swap(piv[step], piv[max_col]); } // Householder transformation T norm_x = T(0); for (std::size_t i = step; i < m; ++i) norm_x += W(i, step) * W(i, step); norm_x = std::sqrt(norm_x); if (norm_x < std::numeric_limits::epsilon()) { tau[step] = T(0); continue; } T sign = (W(step, step) >= T(0)) ? T(1) : T(-1); T alpha = -sign * norm_x; T v0 = W(step, step) - alpha; tau[step] = -v0 / alpha; if (std::abs(v0) > std::numeric_limits::epsilon()) { T inv_v0 = T(1) / v0; for (std::size_t i = step + 1; i < m; ++i) W(i, step) *= inv_v0; } W(step, step) = alpha; // Apply to the remaining columns for (std::size_t j = step + 1; j < n; ++j) { T dot = W(step, j); for (std::size_t i = step + 1; i < m; ++i) dot += W(i, step) * W(i, j); dot *= tau[step]; W(step, j) -= dot; for (std::size_t i = step + 1; i < m; ++i) W(i, j) -= W(i, step) * dot; } // Update the column norms for (std::size_t j = step + 1; j < n; ++j) { col_norms[j] -= W(step, j) * W(step, j); if (col_norms[j] < T(0)) col_norms[j] = T(0); } } // Extract R (m×n) Matrix R_mat(m, n, T(0)); for (std::size_t i = 0; i < k; ++i) for (std::size_t j = i; j < n; ++j) R_mat(i, j) = W(i, j); // Build Q (m×m) Matrix Q_mat = Matrix::identity(m); for (std::size_t step = k; step-- > 0; ) { if (std::abs(tau[step]) < std::numeric_limits::epsilon()) continue; for (std::size_t j = 0; j < m; ++j) { T dot = Q_mat(step, j); for (std::size_t i = step + 1; i < m; ++i) dot += W(i, step) * Q_mat(i, j); dot *= tau[step]; Q_mat(step, j) -= dot; for (std::size_t i = step + 1; i < m; ++i) Q_mat(i, j) -= W(i, step) * dot; } } // Rank determination if (tol <= T(0)) { // |R(0,0)| ≈ σ_max T r_max = (k > 0) ? std::abs(R_mat(0, 0)) : T(0); tol = static_cast(std::max(m, n)) * r_max * std::numeric_limits::epsilon(); } std::size_t rank = 0; for (std::size_t i = 0; i < k; ++i) { if (std::abs(R_mat(i, i)) > tol) ++rank; else break; } return { std::move(Q_mat), std::move(R_mat), std::move(piv), rank }; } //===================================================================== // Block LU Decomposition //===================================================================== /** * @brief Result of the block LU decomposition * * A = P * L * U (L: lower triangular, U: upper triangular, P: row permutation) * Computes in block units, leveraging matrix-matrix multiplication equivalent to * BLAS Level 3 (gemm) to improve cache efficiency. */ template struct BlockLUResult { Matrix L; ///< n×n lower triangular matrix (diagonal = 1) Matrix U; ///< n×n upper triangular matrix std::vector pivots; ///< row permutation std::size_t block_size; ///< block size used }; /** * @brief Block LU Decomposition * * Perform LU decomposition of a square matrix A (n×n) in block units. * At each step, factor an nb-column panel with ordinary LU, and * update the remaining part with matrix-matrix multiplication (gemm-equivalent). * * @param A input matrix (n×n, square) * @param nb block size (0: automatic = max(1, n/4)) * @return BlockLUResult * @throws DimensionError empty matrix, non-square matrix * @throws MathError singular matrix */ template BlockLUResult block_lu_decomposition( const BaseMatrix& A, std::size_t nb = 0) { std::size_t n = A.rows(); if (n == 0 || A.cols() != n) { assert(false && "DimensionError: block_lu_decomposition: requires a non-empty square matrix"); throw DimensionError("block_lu_decomposition: requires a non-empty square matrix"); } if (nb == 0) nb = std::max(1, n / 4); nb = std::min(nb, n); // Working copy: keep LU together Matrix LU(A); std::vector piv(n); for (std::size_t i = 0; i < n; ++i) piv[i] = i; // Relative threshold for singularity: ||A||_max * n * eps T a_max = T(0); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) a_max = std::max(a_max, std::abs(A(i, j))); const T sing_tol = a_max * static_cast(n) * std::numeric_limits::epsilon(); for (std::size_t jb = 0; jb < n; jb += nb) { std::size_t jend = std::min(jb + nb, n); std::size_t bsize = jend - jb; // Panel LU: factor columns jb..jend-1 with partial pivoting for (std::size_t j = jb; j < jend; ++j) { // Pivot selection std::size_t pivot_row = j; T pivot_val = std::abs(LU(j, j)); for (std::size_t i = j + 1; i < n; ++i) { T av = std::abs(LU(i, j)); if (av > pivot_val) { pivot_val = av; pivot_row = i; } } piv[j] = pivot_row; // Row swap (across all columns) if (pivot_row != j) { for (std::size_t c = 0; c < n; ++c) std::swap(LU(j, c), LU(pivot_row, c)); } // Singularity check if (std::abs(LU(j, j)) <= sing_tol) throw MathError("block_lu_decomposition: singular matrix"); // Compute column j of L (rows j+1..n-1) T inv_diag = T(1) / LU(j, j); for (std::size_t i = j + 1; i < n; ++i) LU(i, j) *= inv_diag; // Update the remaining columns in the panel (rank-1 update) for (std::size_t c = j + 1; c < jend; ++c) for (std::size_t i = j + 1; i < n; ++i) LU(i, c) -= LU(i, j) * LU(j, c); } if (jend < n) { // Forward substitution for U12: L11^{-1} * A12 → U12 // The panel LU already applied the pivot row swaps to all columns, but // the rank-1 update is only within the panel columns. Reflect L11's effect on the right columns. for (std::size_t j = jb + 1; j < jend; ++j) for (std::size_t c = jend; c < n; ++c) for (std::size_t k = jb; k < j; ++k) LU(j, c) -= LU(j, k) * LU(k, c); // Update the remaining block (gemm): A22 -= L21 * U12 for (std::size_t i = jend; i < n; ++i) for (std::size_t c = jend; c < n; ++c) { T sum = T(0); for (std::size_t t = jb; t < jend; ++t) sum += LU(i, t) * LU(t, c); LU(i, c) -= sum; } } } // Separate L and U Matrix L = Matrix::identity(n); Matrix U(n, n, T(0)); for (std::size_t i = 0; i < n; ++i) { for (std::size_t j = 0; j < i; ++j) L(i, j) = LU(i, j); for (std::size_t j = i; j < n; ++j) U(i, j) = LU(i, j); } return { std::move(L), std::move(U), std::move(piv), nb }; } /** * @brief Solve a system of linear equations using block LU: Ax = b */ template Vector block_lu_solve( const BlockLUResult& lu, const BaseVector& b_basevec) { Vector b(b_basevec); // 1 copy via BaseVector ctor std::size_t n = lu.L.rows(); if (b.size() != n) { assert(false && "DimensionError: block_lu_solve: dimension mismatch"); throw DimensionError("block_lu_solve: dimension mismatch"); } // Apply the pivot permutation Vector x(b); for (std::size_t i = 0; i < n; ++i) { if (lu.pivots[i] != i) std::swap(x[i], x[lu.pivots[i]]); } // Forward substitution: L * y = P * b for (std::size_t i = 1; i < n; ++i) for (std::size_t j = 0; j < i; ++j) x[i] -= lu.L(i, j) * x[j]; // Backward substitution: U * x = y for (std::size_t i = n; i-- > 0; ) { for (std::size_t j = i + 1; j < n; ++j) x[i] -= lu.U(i, j) * x[j]; x[i] /= lu.U(i, i); } return x; } //===================================================================== // HSS decomposition (Hierarchically Semi-Separable) //===================================================================== /** * @brief HSS node: represents each node of the hierarchically semi-separable structure * * Leaf node: holds the diagonal block D * Internal node: holds the generator matrices U, V (off-diagonal low-rank representation) and * the transformation matrix B (sibling coupling), R, W (compression matrices) * * Off-diagonal block A(i,j) ≈ U_i * B_{ij} * V_j^T (i≠j) */ template struct HSSNode { bool is_leaf = true; std::size_t row_start = 0; ///< starting row index std::size_t row_size = 0; ///< number of rows std::size_t col_start = 0; ///< starting column index std::size_t col_size = 0; ///< number of columns // Leaf node: diagonal block Matrix D; // All nodes: generator matrices (for low-rank representation) Matrix U; ///< row-direction generator matrix Matrix V; ///< column-direction generator matrix // Internal node: coupling matrix between sibling nodes Matrix B; ///< sibling coupling matrix // Internal node: child-to-parent transformation matrices Matrix R; ///< compression of U: U_parent = [R_left * U_left; R_right * U_right] Matrix W; ///< compression of V: V_parent = [W_left * V_left; W_right * V_right] // Child nodes std::unique_ptr> left; std::unique_ptr> right; }; /** * @brief Result of the HSS decomposition */ template struct HSSResult { HSSNode root; ///< root of the HSS tree std::size_t leaf_size; ///< maximum size of a leaf node std::size_t rank; ///< rank of the off-diagonal low-rank approximation }; namespace detail_hss { /** * @brief Build the HSS tree (partitioning only; matrix values not set) */ template std::unique_ptr> build_tree( std::size_t row_start, std::size_t row_size, std::size_t col_start, std::size_t col_size, std::size_t leaf_size) { auto node = std::make_unique>(); node->row_start = row_start; node->row_size = row_size; node->col_start = col_start; node->col_size = col_size; if (row_size <= leaf_size && col_size <= leaf_size) { node->is_leaf = true; } else { node->is_leaf = false; std::size_t rmid = row_size / 2; std::size_t cmid = col_size / 2; node->left = build_tree(row_start, rmid, col_start, cmid, leaf_size); node->right = build_tree(row_start + rmid, row_size - rmid, col_start + cmid, col_size - cmid, leaf_size); } return node; } /** * @brief Extract a subblock of the matrix */ template Matrix extract_block(const BaseMatrix& A, std::size_t r0, std::size_t nr, std::size_t c0, std::size_t nc) { Matrix B(nr, nc, T(0)); for (std::size_t i = 0; i < nr; ++i) for (std::size_t j = 0; j < nc; ++j) B(i, j) = A(r0 + i, c0 + j); return B; } /** * @brief Low-rank approximation: A ≈ U * S * V^T up to rank via truncated SVD * @return {U_r (m×rank), V_r (n×rank)} — U_r includes the singular values */ template std::pair, Matrix> low_rank_approx( const BaseMatrix& A, std::size_t rank) { std::size_t m = A.rows(), n = A.cols(); if (m == 0 || n == 0) { return { Matrix(m, 0, T(0)), Matrix(n, 0, T(0)) }; } auto [U_svd, sigma, Vt_svd] = svd_decomposition(A); std::size_t k = std::min({ rank, m, n, static_cast(sigma.size()) }); // Further limit the effective rank: truncate tiny singular values T threshold = (k > 0 ? sigma[0] : T(0)) * static_cast(std::max(m, n)) * std::numeric_limits::epsilon(); std::size_t eff_k = 0; for (std::size_t i = 0; i < k; ++i) { if (sigma[i] > threshold) ++eff_k; else break; } k = std::max(eff_k, 1); // at least 1 // U_r = U(:, 0:k) * diag(S(0:k)) Matrix Ur(m, k, T(0)); for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < k; ++j) Ur(i, j) = U_svd(i, j) * sigma[j]; // V_r = V(:, 0:k) — Vt_svd is V^T, so V(i,j) = Vt_svd(j,i) Matrix Vr(n, k, T(0)); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < k; ++j) Vr(i, j) = Vt_svd(j, i); return { std::move(Ur), std::move(Vr) }; } /** * @brief Set the matrix values into the HSS tree (bottom-up, leaf→root) * * Leaf node: D = the diagonal block of A, U/V are the low-rank approximation of the off-diagonal rows * Internal node: compress the children's U/V to compute R/W, B is the sibling coupling */ template void compress_node( HSSNode& node, const BaseMatrix& A, std::size_t rank) { std::size_t n = A.rows(); if (node.is_leaf) { // Diagonal block node.D = extract_block(A, node.row_start, node.row_size, node.col_start, node.col_size); // Off-diagonal rows: among this node's rows, the columns other than the diagonal block // Exclude the diagonal-block columns from all columns of rows row_start..row_start+row_size-1 std::size_t off_cols = n - node.col_size; if (off_cols == 0) { // The whole matrix is one block → no off-diagonal node.U = Matrix(node.row_size, 1, T(0)); node.V = Matrix(1, 1, T(1)); // dummy return; } // Construct the off-diagonal row block Matrix off_row(node.row_size, off_cols, T(0)); std::size_t jj = 0; for (std::size_t j = 0; j < n; ++j) { if (j >= node.col_start && j < node.col_start + node.col_size) continue; for (std::size_t i = 0; i < node.row_size; ++i) off_row(i, jj) = A(node.row_start + i, j); ++jj; } auto [Ur, Vr_unused] = low_rank_approx(off_row, rank); node.U = std::move(Ur); // Off-diagonal column block: rows other than the diagonal block, this node's columns std::size_t off_rows = n - node.row_size; Matrix off_col(off_rows, node.col_size, T(0)); std::size_t ii = 0; for (std::size_t i = 0; i < n; ++i) { if (i >= node.row_start && i < node.row_start + node.row_size) continue; for (std::size_t j = 0; j < node.col_size; ++j) off_col(ii, j) = A(i, node.col_start + j); ++ii; } auto [Uc, sigma_c, Vtc] = svd_decomposition(off_col); std::size_t k = std::min({ rank, off_rows, node.col_size, static_cast(sigma_c.size()) }); T threshold = (k > 0 ? sigma_c[0] : T(0)) * static_cast(std::max(off_rows, node.col_size)) * std::numeric_limits::epsilon(); std::size_t eff_k = 0; for (std::size_t i = 0; i < k; ++i) { if (sigma_c[i] > threshold) ++eff_k; else break; } k = std::max(eff_k, 1); // V = V(:, 0:k) — Vtc is V^T, so V(i,j) = Vtc(j,i) Matrix Vr(node.col_size, k, T(0)); for (std::size_t i = 0; i < node.col_size; ++i) for (std::size_t j = 0; j < k; ++j) Vr(i, j) = Vtc(j, i); node.V = std::move(Vr); return; } // Internal node: compress the child nodes first compress_node(*node.left, A, rank); compress_node(*node.right, A, rank); auto& lc = *node.left; auto& rc = *node.right; // Sibling coupling matrix B: off-diagonal block between siblings // A(left_rows, right_cols) ≈ U_left * B_lr * V_right^T // → B_lr = U_left^+ * A(left_rows, right_cols) * V_right^{+T} // where U_left^+ = (U^T U)^{-1} U^T (pseudo-inverse) Matrix A_lr = extract_block(A, lc.row_start, lc.row_size, rc.col_start, rc.col_size); // B = pinv(U_left) * A_lr * pinv(V_right)^T // pinv(X) via SVD auto compute_pinv = [](const Matrix& X) -> Matrix { if (X.rows() == 0 || X.cols() == 0) return Matrix(X.cols(), X.rows(), T(0)); auto [U_p, sigma_p, Vt_p] = svd_decomposition(X); std::size_t m = X.rows(), nn = X.cols(); std::size_t k = std::min(m, nn); T thr = sigma_p[0] * static_cast(std::max(m, nn)) * std::numeric_limits::epsilon(); // pinv = V * S^{-1} * U^T = Vt^T * S^{-1} * U^T Matrix result(nn, m, T(0)); for (std::size_t i = 0; i < nn; ++i) for (std::size_t j = 0; j < m; ++j) { T sum = T(0); for (std::size_t t = 0; t < k; ++t) { if (sigma_p[t] > thr) sum += Vt_p(t, i) * (T(1) / sigma_p[t]) * U_p(j, t); } result(i, j) = sum; } return result; }; Matrix Ul_pinv = compute_pinv(lc.U); Matrix Vr_pinv = compute_pinv(rc.V); // B = Ul_pinv * A_lr * Vr_pinv^T std::size_t brows = Ul_pinv.rows(), bcols = Vr_pinv.rows(); Matrix temp(brows, A_lr.cols(), T(0)); for (std::size_t i = 0; i < brows; ++i) for (std::size_t j = 0; j < A_lr.cols(); ++j) for (std::size_t t = 0; t < A_lr.rows(); ++t) temp(i, j) += Ul_pinv(i, t) * A_lr(t, j); node.B = Matrix(brows, bcols, T(0)); for (std::size_t i = 0; i < brows; ++i) for (std::size_t j = 0; j < bcols; ++j) for (std::size_t t = 0; t < A_lr.cols(); ++t) node.B(i, j) += temp(i, t) * Vr_pinv(j, t); // Synthesize the parent's U, V generator matrices from the children // Construct the off-diagonal row block std::size_t total_rows = node.row_size; std::size_t off_cols = n - node.col_size; if (off_cols == 0) { std::size_t uk = lc.U.cols(); node.U = Matrix(total_rows, uk, T(0)); node.R = Matrix(uk, uk, T(0)); node.W = Matrix(uk, uk, T(0)); node.V = Matrix(node.col_size, 1, T(0)); return; } Matrix off_row(total_rows, off_cols, T(0)); std::size_t jj = 0; for (std::size_t j = 0; j < n; ++j) { if (j >= node.col_start && j < node.col_start + node.col_size) continue; for (std::size_t i = 0; i < total_rows; ++i) off_row(i, jj) = A(node.row_start + i, j); ++jj; } auto [Ur_parent, Vr_unused2] = low_rank_approx(off_row, rank); node.U = std::move(Ur_parent); // Off-diagonal column block std::size_t off_rows = n - node.row_size; Matrix off_col(off_rows, node.col_size, T(0)); std::size_t ii = 0; for (std::size_t i = 0; i < n; ++i) { if (i >= node.row_start && i < node.row_start + node.row_size) continue; for (std::size_t j = 0; j < node.col_size; ++j) off_col(ii, j) = A(i, node.col_start + j); ++ii; } auto [Uc2, sigma_c2, Vtc2] = svd_decomposition(off_col); std::size_t k2 = std::min({ rank, off_rows, node.col_size, static_cast(sigma_c2.size()) }); T thr2 = (k2 > 0 ? sigma_c2[0] : T(0)) * static_cast(std::max(off_rows, node.col_size)) * std::numeric_limits::epsilon(); std::size_t eff2 = 0; for (std::size_t i = 0; i < k2; ++i) { if (sigma_c2[i] > thr2) ++eff2; else break; } k2 = std::max(eff2, 1); Matrix Vr_parent(node.col_size, k2, T(0)); for (std::size_t i = 0; i < node.col_size; ++i) for (std::size_t j = 0; j < k2; ++j) Vr_parent(i, j) = Vtc2(j, i); node.V = std::move(Vr_parent); // R: U_parent ≈ [R_left * U_left; R_right * U_right] // R_left = pinv(U_left) * U_parent(left_rows, :) std::size_t lrows = lc.row_size; Matrix Up_left(lrows, node.U.cols(), T(0)); for (std::size_t i = 0; i < lrows; ++i) for (std::size_t j = 0; j < node.U.cols(); ++j) Up_left(i, j) = node.U(i, j); Matrix Ul_pinv2 = compute_pinv(lc.U); node.R = Matrix(Ul_pinv2.rows(), Up_left.cols(), T(0)); for (std::size_t i = 0; i < node.R.rows(); ++i) for (std::size_t j = 0; j < node.R.cols(); ++j) for (std::size_t t = 0; t < lrows; ++t) node.R(i, j) += Ul_pinv2(i, t) * Up_left(t, j); // W: V_parent ≈ [W_left * V_left; W_right * V_right] std::size_t lcols = lc.col_size; Matrix Vp_left(lcols, node.V.cols(), T(0)); for (std::size_t i = 0; i < lcols; ++i) for (std::size_t j = 0; j < node.V.cols(); ++j) Vp_left(i, j) = node.V(i, j); Matrix Vl_pinv = compute_pinv(lc.V); node.W = Matrix(Vl_pinv.rows(), Vp_left.cols(), T(0)); for (std::size_t i = 0; i < node.W.rows(); ++i) for (std::size_t j = 0; j < node.W.cols(); ++j) for (std::size_t t = 0; t < lcols; ++t) node.W(i, j) += Vl_pinv(i, t) * Vp_left(t, j); } /** * @brief Reconstruct the dense matrix from the HSS representation (for verification) */ template void reconstruct_node( const HSSNode& node, Matrix& A) { if (node.is_leaf) { // Write the diagonal block for (std::size_t i = 0; i < node.row_size; ++i) for (std::size_t j = 0; j < node.col_size; ++j) A(node.row_start + i, node.col_start + j) = node.D(i, j); return; } // Recursively reconstruct the diagonal parts of the child nodes reconstruct_node(*node.left, A); reconstruct_node(*node.right, A); auto& lc = *node.left; auto& rc = *node.right; // Off-diagonal block: A(left, right) = U_left * B * V_right^T for (std::size_t i = 0; i < lc.row_size; ++i) for (std::size_t j = 0; j < rc.col_size; ++j) { T val = T(0); for (std::size_t p = 0; p < node.B.rows(); ++p) for (std::size_t q = 0; q < node.B.cols(); ++q) val += lc.U(i, p) * node.B(p, q) * rc.V(j, q); A(lc.row_start + i, rc.col_start + j) = val; } // A(right, left) = U_right * B^T * V_left^T // Sibling coupling of B^T: A_rl = U_right * B_rl * V_left^T // B_rl is the coupling matrix of A(right, left) Matrix A_rl = extract_block( A, // note: A does not yet hold the original values (during reconstruction) rc.row_start, rc.row_size, lc.col_start, lc.col_size); // Direct computation: low-rank approximation of A(right, left) // B_rl = pinv(U_right) * A_orig(right, left) * pinv(V_left)^T // However, since the original matrix is unavailable during reconstruction, a different approach is needed // Use the symmetric B: A(right, left) = U_right * B^T * V_left^T for (std::size_t i = 0; i < rc.row_size; ++i) for (std::size_t j = 0; j < lc.col_size; ++j) { T val = T(0); for (std::size_t p = 0; p < node.B.cols(); ++p) for (std::size_t q = 0; q < node.B.rows(); ++q) val += rc.U(i, p) * node.B(q, p) * lc.V(j, q); A(rc.row_start + i, lc.col_start + j) = val; } } } // namespace detail_hss /** * @brief HSS decomposition (Hierarchically Semi-Separable) * * Decompose a square matrix A into a hierarchically semi-separable structure. * Approximate the off-diagonal blocks with low rank and represent them as a hierarchical tree. * * Effective for matrices with low-rank structure such as BEM, integral equations, and kernel matrices. * * @param A input matrix (n×n, square) * @param leaf_size maximum size of a leaf node (0: automatic = max(2, n/4)) * @param rank maximum rank of the off-diagonal low-rank approximation (0: automatic = max(1, leaf_size/2)) * @return HSSResult * @throws DimensionError empty matrix, non-square matrix */ template HSSResult hss_decomposition( const BaseMatrix& A, std::size_t leaf_size = 0, std::size_t rank = 0) { std::size_t n = A.rows(); if (n == 0 || A.cols() != n) { assert(false && "DimensionError: hss_decomposition: requires a non-empty square matrix"); throw DimensionError("hss_decomposition: requires a non-empty square matrix"); } if (n == 1) { // 1×1 matrix: single leaf HSSResult result; result.leaf_size = 1; result.rank = 1; result.root.is_leaf = true; result.root.row_start = 0; result.root.row_size = 1; result.root.col_start = 0; result.root.col_size = 1; result.root.D = Matrix(A); // 1×1 deep copy result.root.U = Matrix(1, 1, T(1)); result.root.V = Matrix(1, 1, T(1)); return result; } if (leaf_size == 0) leaf_size = std::max(2, n / 4); leaf_size = std::min(leaf_size, n); if (rank == 0) rank = std::max(1, leaf_size / 2); // Build the HSS tree auto root = detail_hss::build_tree(0, n, 0, n, leaf_size); // Bottom-up compression detail_hss::compress_node(*root, A, rank); HSSResult result; result.root = std::move(*root); result.leaf_size = leaf_size; result.rank = rank; return result; } /** * @brief Reconstruct the dense matrix from the HSS representation (for verification) * * @param hss the result of the HSS decomposition * @return the reconstructed dense matrix */ template Matrix hss_reconstruct(const HSSResult& hss) { std::size_t n = hss.root.row_size; Matrix A(n, n, T(0)); detail_hss::reconstruct_node(hss.root, A); return A; } //===================================================================== // Dulmage-Mendelsohn decomposition (sparse matrix structural decomposition) //===================================================================== /** * @brief Result of the Dulmage-Mendelsohn decomposition * * Reorder a matrix A(m×n) with row permutation P and column permutation Q so that * P * A * Q^T = block upper triangular (BTF) * . * * Coarse decomposition: * HR (horizontal rough): the part containing unmatched rows * SR (square rough): the square part with a perfect matching * HC (horizontal coarse): the part containing unmatched columns * * The square part SR is further block-upper-triangularized via strongly connected components (SCC). */ template struct DulmageMendelsohnResult { std::vector row_perm; ///< row permutation (P) std::vector col_perm; ///< column permutation (Q) std::vector block_starts; ///< block boundaries [0, b1, b2, ..., n_blocks] std::size_t n_blocks; ///< number of blocks // Boundaries of the coarse decomposition std::size_t hr_end; ///< end row of the HR part (0 = no HR) std::size_t sr_end; ///< end row of the SR part std::size_t hc_start; ///< start column of the HC part (n = no HC) }; namespace detail_dm { /** * @brief Find the maximum matching of a bipartite graph via Hopcroft-Karp * * @param adj adj[row] = {col1, col2, ...} (nonzero columns) * @param m number of rows * @param n number of columns * @param match_r match_r[row] = matched column (SIZE_MAX = unmatched) * @param match_c match_c[col] = matched row (SIZE_MAX = unmatched) * @return matching size */ inline std::size_t hopcroft_karp( const std::vector>& adj, std::size_t m, std::size_t n, std::vector& match_r, std::vector& match_c) { const std::size_t NIL = SIZE_MAX; match_r.assign(m, NIL); match_c.assign(n, NIL); std::vector dist(m + 1); std::size_t matching = 0; // BFS: assign alternating-path distance labels from unmatched rows auto bfs = [&]() -> bool { std::queue Q; for (std::size_t r = 0; r < m; ++r) { if (match_r[r] == NIL) { dist[r] = 0; Q.push(r); } else { dist[r] = SIZE_MAX; } } dist[m] = SIZE_MAX; // distance of the NIL node while (!Q.empty()) { std::size_t r = Q.front(); Q.pop(); if (dist[r] < dist[m]) { for (std::size_t c : adj[r]) { std::size_t r2 = match_c[c]; std::size_t idx = (r2 == NIL) ? m : r2; if (dist[idx] == SIZE_MAX) { dist[idx] = dist[r] + 1; if (idx != m) Q.push(idx); } } } } return dist[m] != SIZE_MAX; }; // DFS: follow alternating paths to find augmenting paths std::function dfs = [&](std::size_t r) -> bool { if (r == m) return true; // reached NIL for (std::size_t c : adj[r]) { std::size_t r2 = match_c[c]; std::size_t idx = (r2 == NIL) ? m : r2; if (dist[idx] == dist[r] + 1) { if (dfs(idx)) { match_c[c] = r; match_r[r] = c; return true; } } } dist[r] = SIZE_MAX; return false; }; while (bfs()) { for (std::size_t r = 0; r < m; ++r) { if (match_r[r] == NIL) { if (dfs(r)) ++matching; } } } return matching; } /** * @brief Find rows/columns reachable from the matching via BFS */ inline void reachable_from_unmatched_rows( const std::vector>& adj, std::size_t m, const std::vector& match_r, const std::vector& match_c, std::vector& row_reached, std::vector& col_reached) { const std::size_t NIL = SIZE_MAX; std::queue Q; // Start from unmatched rows for (std::size_t r = 0; r < m; ++r) { if (match_r[r] == NIL) { row_reached[r] = true; Q.push(r); } } // Search for rows/columns reachable via alternating paths while (!Q.empty()) { std::size_t r = Q.front(); Q.pop(); for (std::size_t c : adj[r]) { if (!col_reached[c]) { col_reached[c] = true; // Traverse the matched edge in reverse if (match_c[c] != NIL && !row_reached[match_c[c]]) { row_reached[match_c[c]] = true; Q.push(match_c[c]); } } } } } /** * @brief Find rows/columns reachable in the column→row direction from unmatched columns via BFS */ inline void reachable_from_unmatched_cols( const std::vector>& adj_t, std::size_t n, const std::vector& match_r, const std::vector& match_c, std::vector& row_reached, std::vector& col_reached) { const std::size_t NIL = SIZE_MAX; std::queue Q; for (std::size_t c = 0; c < n; ++c) { if (match_c[c] == NIL) { col_reached[c] = true; Q.push(c); } } while (!Q.empty()) { std::size_t c = Q.front(); Q.pop(); for (std::size_t r : adj_t[c]) { if (!row_reached[r]) { row_reached[r] = true; if (match_r[r] != NIL && !col_reached[match_r[r]]) { col_reached[match_r[r]] = true; Q.push(match_r[r]); } } } } } /** * @brief Strongly connected components of the graph composed of matched rows only (Tarjan) * * Nodes = matched rows (in 1:1 correspondence with matched columns) * Edge: row r → column match_r[r] → (nonzero) row r' (r' ≠ r, r' is a matched row) */ inline std::vector scc_order( const std::vector>& adj, const std::vector& sq_rows, const std::vector& match_r, const std::vector& match_c, std::vector& block_starts) { std::size_t ns = sq_rows.size(); if (ns == 0) return {}; // Mapping from row number → index within SQ std::unordered_map row_to_idx; for (std::size_t i = 0; i < ns; ++i) row_to_idx[sq_rows[i]] = i; // Adjacency list of the SQ graph std::vector> g(ns); for (std::size_t i = 0; i < ns; ++i) { std::size_t r = sq_rows[i]; for (std::size_t c : adj[r]) { std::size_t r2 = match_c[c]; if (r2 != SIZE_MAX && r2 != r) { auto it = row_to_idx.find(r2); if (it != row_to_idx.end()) g[i].push_back(it->second); } } } // Tarjan's SCC std::vector order; order.reserve(ns); std::vector idx_arr(ns, -1), low(ns, -1); std::vector on_stack(ns, false); std::stack st; int counter = 0; std::function strongconnect = [&](std::size_t v) { idx_arr[v] = low[v] = counter++; st.push(v); on_stack[v] = true; for (std::size_t w : g[v]) { if (idx_arr[w] < 0) { strongconnect(w); low[v] = std::min(low[v], low[w]); } else if (on_stack[w]) { low[v] = std::min(low[v], idx_arr[w]); } } if (low[v] == idx_arr[v]) { std::size_t start = order.size(); while (true) { std::size_t w = st.top(); st.pop(); on_stack[w] = false; order.push_back(w); if (w == v) break; } block_starts.push_back(start); } }; for (std::size_t i = 0; i < ns; ++i) { if (idx_arr[i] < 0) strongconnect(i); } // Tarjan outputs SCCs in reverse topological order, so reverse them std::reverse(order.begin(), order.end()); // Recompute block_starts with reversal too std::reverse(block_starts.begin(), block_starts.end()); std::size_t nb = block_starts.size(); for (std::size_t i = 0; i < nb; ++i) block_starts[i] = ns - block_starts[i]; // block_starts is a cumulative value like [size1, size1+size2, ...] // Fix to the correct form std::vector bs_new; bs_new.push_back(0); // Compute the size of each block std::vector sizes(nb); for (std::size_t i = 0; i < nb; ++i) { sizes[i] = block_starts[i] - (i > 0 ? block_starts[i - 1] : 0); } // Simplification: rebuild order in SCC order // After reversing Tarjan's output, order[0..] is in topological order // Rebuild block_starts from order bs_new.clear(); bs_new.push_back(0); // Reimplement Tarjan: produce correct block boundaries iteratively // The current order is already reversed. Recompute the block boundaries std::vector comp(ns, -1); { // Recompute the SCCs (concisely with Kosaraju) // 1st pass: compute the finish order via DFS std::vector finish_order; finish_order.reserve(ns); std::vector visited(ns, false); std::function dfs1 = [&](std::size_t v) { visited[v] = true; for (std::size_t w : g[v]) if (!visited[w]) dfs1(w); finish_order.push_back(v); }; for (std::size_t i = 0; i < ns; ++i) if (!visited[i]) dfs1(i); // Transposed graph std::vector> gt(ns); for (std::size_t v = 0; v < ns; ++v) for (std::size_t w : g[v]) gt[w].push_back(v); // 2nd pass: DFS the transposed graph in reverse finish order int comp_id = 0; std::fill(visited.begin(), visited.end(), false); std::function dfs2 = [&](std::size_t v, int c) { visited[v] = true; comp[v] = c; for (std::size_t w : gt[v]) if (!visited[w]) dfs2(w, c); }; for (std::size_t i = ns; i-- > 0; ) { std::size_t v = finish_order[i]; if (!visited[v]) { dfs2(v, comp_id); ++comp_id; } } } // comp[i] is the SCC ID in topological order // Reorder by SCC ID int max_comp = *std::max_element(comp.begin(), comp.end()); std::vector> scc_groups(max_comp + 1); for (std::size_t i = 0; i < ns; ++i) scc_groups[comp[i]].push_back(i); order.clear(); block_starts.clear(); block_starts.push_back(0); for (int c = 0; c <= max_comp; ++c) { if (scc_groups[c].empty()) continue; for (std::size_t idx : scc_groups[c]) order.push_back(idx); block_starts.push_back(order.size()); } return order; } } // namespace detail_dm /** * @brief Dulmage-Mendelsohn decomposition * * Construct a bipartite graph from the nonzero structure of a matrix A (m×n), and * reorder into block upper triangular form via maximum matching + strongly connected component decomposition. * * @param A input matrix * @param tol nonzero-detection threshold (nonzero when |a_{ij}| > tol) * @return DulmageMendelsohnResult * @throws DimensionError empty matrix */ template DulmageMendelsohnResult dulmage_mendelsohn( const BaseMatrix& A, T tol = std::numeric_limits::epsilon()) { std::size_t m = A.rows(), n = A.cols(); if (m == 0 || n == 0) { assert(false && "DimensionError: dulmage_mendelsohn: requires a non-empty matrix"); throw DimensionError("dulmage_mendelsohn: requires a non-empty matrix"); } // Adjacency list (row → nonzero columns) std::vector> adj(m); std::vector> adj_t(n); // transposed for (std::size_t i = 0; i < m; ++i) for (std::size_t j = 0; j < n; ++j) if (std::abs(A(i, j)) > tol) { adj[i].push_back(j); adj_t[j].push_back(i); } // Maximum matching std::vector match_r, match_c; detail_dm::hopcroft_karp(adj, m, n, match_r, match_c); // Coarse decomposition: HR (reachable from unmatched rows), HC (reachable from unmatched columns) std::vector hr_rows(m, false), hr_cols(n, false); detail_dm::reachable_from_unmatched_rows(adj, m, match_r, match_c, hr_rows, hr_cols); std::vector hc_rows(m, false), hc_cols(n, false); detail_dm::reachable_from_unmatched_cols(adj_t, n, match_r, match_c, hc_rows, hc_cols); // Classification: HR, SR, HC std::vector hr_r, sr_r, hc_r; std::vector hr_c, sr_c, hc_c; for (std::size_t r = 0; r < m; ++r) { if (hr_rows[r]) hr_r.push_back(r); else if (hc_rows[r]) hc_r.push_back(r); else sr_r.push_back(r); } for (std::size_t c = 0; c < n; ++c) { if (hr_cols[c]) hr_c.push_back(c); else if (hc_cols[c]) hc_c.push_back(c); else sr_c.push_back(c); } // Block-upper-triangularize the SR part via SCC std::vector scc_block_starts; std::vector scc_order; if (!sr_r.empty()) { scc_order = detail_dm::scc_order(adj, sr_r, match_r, match_c, scc_block_starts); } // Construct the row/column permutations std::vector row_perm, col_perm; std::vector block_starts; // HR part for (std::size_t r : hr_r) row_perm.push_back(r); for (std::size_t c : hr_c) col_perm.push_back(c); std::size_t hr_end_row = hr_r.size(); std::size_t hr_end_col = hr_c.size(); // SR part (SCC order) block_starts.push_back(row_perm.size()); if (!sr_r.empty()) { for (std::size_t i = 0; i < scc_order.size(); ++i) { std::size_t sq_idx = scc_order[i]; std::size_t r = sr_r[sq_idx]; row_perm.push_back(r); col_perm.push_back(match_r[r]); } // Add a block boundary for (std::size_t i = 1; i < scc_block_starts.size(); ++i) block_starts.push_back(hr_end_row + scc_block_starts[i]); } std::size_t sr_end_row = row_perm.size(); // HC part for (std::size_t r : hc_r) row_perm.push_back(r); for (std::size_t c : hc_c) col_perm.push_back(c); // Add a trailing boundary only if there is an HC block if (!hc_r.empty()) block_starts.push_back(row_perm.size()); DulmageMendelsohnResult result; result.row_perm = std::move(row_perm); result.col_perm = std::move(col_perm); result.block_starts = std::move(block_starts); result.n_blocks = result.block_starts.size() - 1; result.hr_end = hr_end_row; result.sr_end = sr_end_row; result.hc_start = hr_end_col + sr_r.size(); // HC start column return result; } //===================================================================== // Smith Normal Form //===================================================================== /** * @brief Result of the Smith Normal Form * * Decompose an integer matrix A (m×n) into A = U * D * V. * - U: m×m unimodular matrix (det = ±1) * - D: m×n diagonal matrix (invariant factors: d_1 | d_2 | ... | d_r, the rest are 0) * - V: n×n unimodular matrix (det = ±1) * * Exact computation with integer types (int, long long, etc.). */ template struct SmithNormalFormResult { Matrix U; ///< left unimodular matrix Matrix D; ///< diagonal matrix (invariant factors) Matrix V; ///< right unimodular matrix std::vector invariant_factors; ///< invariant factors d_1, d_2, ..., d_r std::size_t rank; ///< rank (number of nonzero invariant factors) }; /** * @brief Smith Normal Form * * Transform an integer matrix A (m×n) into diagonal form via elementary row/column operations. * A = U * D * V (U, V: unimodular, D: diagonal) * * Algorithm: successive diagonalization via the extended Euclidean algorithm * * @param A input matrix (integer elements) * @return SmithNormalFormResult * @throws DimensionError empty matrix */ template requires sangi::BaseMatrixLike SmithNormalFormResult> smith_normal_form(const MA& A) { using T = sangi::element_t; // rectangular OK (m×n integer matrix) std::size_t m = A.rows(), n = A.cols(); if (m == 0 || n == 0) { assert(false && "DimensionError: smith_normal_form: requires a non-empty matrix"); throw DimensionError("smith_normal_form: requires a non-empty matrix"); } // Working copy Matrix D(A); Matrix U = Matrix::identity(m); Matrix V = Matrix::identity(n); std::size_t min_mn = std::min(m, n); for (std::size_t k = 0; k < min_mn; ++k) { // Pivot selection: find the nonzero element with the smallest absolute value in D(k:m, k:n) bool found_nonzero = true; while (found_nonzero) { // Bring the smallest nonzero element to the pivot found_nonzero = false; T min_val = T(0); std::size_t pi = k, pj = k; for (std::size_t i = k; i < m; ++i) for (std::size_t j = k; j < n; ++j) { T av = D(i, j); if (av < T(0)) av = -av; if (av > T(0) && (min_val == T(0) || av < min_val)) { min_val = av; pi = i; pj = j; } } if (min_val == T(0)) break; // the rest are all zero // Move the pivot to (k, k) if (pi != k) { for (std::size_t j = 0; j < n; ++j) std::swap(D(k, j), D(pi, j)); for (std::size_t j = 0; j < m; ++j) std::swap(U(j, k), U(j, pi)); } if (pj != k) { for (std::size_t i = 0; i < m; ++i) std::swap(D(i, k), D(i, pj)); // Column swap of D → V_new = C^{-1}*V, C is a column permutation → row swap of V for (std::size_t j = 0; j < n; ++j) std::swap(V(k, j), V(pj, j)); } // If D(k,k) is negative, sign-flip the row if (D(k, k) < T(0)) { for (std::size_t j = 0; j < n; ++j) D(k, j) = -D(k, j); for (std::size_t j = 0; j < m; ++j) U(j, k) = -U(j, k); } T pivot = D(k, k); // Eliminate column k: make D(i, k) divisible by D(k, k) bool changed = false; for (std::size_t i = k + 1; i < m; ++i) { if (D(i, k) == T(0)) continue; T q = D(i, k) / pivot; // row i -= q * row k for (std::size_t j = 0; j < n; ++j) D(i, j) -= q * D(k, j); for (std::size_t j = 0; j < m; ++j) U(j, k) += q * U(j, i); if (D(i, k) != T(0)) changed = true; } // Eliminate row k: make D(k, j) divisible by D(k, k) for (std::size_t j = k + 1; j < n; ++j) { if (D(k, j) == T(0)) continue; T q = D(k, j) / pivot; // column j -= q * column k → row j of V += q * row k for (std::size_t i = 0; i < m; ++i) D(i, j) -= q * D(i, k); for (std::size_t c = 0; c < n; ++c) V(k, c) += q * V(j, c); if (D(k, j) != T(0)) changed = true; } if (changed) { found_nonzero = true; // a remainder appeared → loop again } else { // Check whether columns and rows from k+1 onward are zero bool all_zero = true; for (std::size_t i = k + 1; i < m && all_zero; ++i) if (D(i, k) != T(0)) all_zero = false; for (std::size_t j = k + 1; j < n && all_zero; ++j) if (D(k, j) != T(0)) all_zero = false; if (all_zero) break; found_nonzero = true; } } } // Guarantee the divisibility condition: d_i | d_{i+1} // Extended GCD + 2×2 row/column transform to map diag(a,b) → diag(gcd, lcm) // // R * diag(a,b) * C = diag(g, l) // R = [[s, t], [-b/g, a/g]], C = [[1, c01], [1, c11]] // c01 = -t*b/g, c11 = s*a/g // R^{-1} = [[a/g, -t], [b/g, s]] // C^{-1} = [[c11, -c01], [-1, 1]] // // A = U*D*V → A = (U*R^{-1}) * diag(g,l) * (C^{-1}*V) auto ext_gcd = [](T a, T b, T& s, T& t) -> T { T old_r = a, r = b; T old_s = T(1); s = T(0); T old_t = T(0); t = T(1); while (r != T(0)) { T q = old_r / r; T tmp; tmp = r; r = old_r - q * r; old_r = tmp; tmp = s; s = old_s - q * s; old_s = tmp; tmp = t; t = old_t - q * t; old_t = tmp; } s = old_s; t = old_t; return old_r; }; bool modified = true; while (modified) { modified = false; for (std::size_t i = 0; i + 1 < min_mn; ++i) { T a = D(i, i), b = D(i + 1, i + 1); if (a == T(0) || b == T(0)) continue; T abs_a = (a < T(0)) ? -a : a; T abs_b = (b < T(0)) ? -b : b; if (abs_b % abs_a == T(0)) continue; T s_coeff, t_coeff; T g = ext_gcd(a, b, s_coeff, t_coeff); if (g < T(0)) { g = -g; s_coeff = -s_coeff; t_coeff = -t_coeff; } T l = a / g * b; if (l < T(0)) l = -l; // Update D's diagonal elements directly D(i, i) = g; D(i + 1, i + 1) = l; // U_new = U_old * R^{-1}, R^{-1} = [[a/g, -t], [b/g, s]] T ri00 = a / g, ri01 = -t_coeff; T ri10 = b / g, ri11 = s_coeff; for (std::size_t r = 0; r < m; ++r) { T ui = U(r, i), uj = U(r, i + 1); U(r, i) = ui * ri00 + uj * ri10; U(r, i + 1) = ui * ri01 + uj * ri11; } // V_new = C^{-1} * V_old (2×2 transform on rows i, i+1) // C = [[1, -tb/g], [1, sa/g]] // C^{-1} = [[sa/g, tb/g], [-1, 1]] T ci00 = (s_coeff * a) / g; T ci01 = (t_coeff * b) / g; for (std::size_t c = 0; c < n; ++c) { T vi = V(i, c), vj = V(i + 1, c); V(i, c) = ci00 * vi + ci01 * vj; V(i + 1, c) = -vi + vj; } modified = true; } } // Normalize the diagonal elements to positive for (std::size_t i = 0; i < min_mn; ++i) { if (D(i, i) < T(0)) { for (std::size_t j = 0; j < n; ++j) D(i, j) = -D(i, j); for (std::size_t j = 0; j < m; ++j) U(j, i) = -U(j, i); } } // Extract the invariant factors std::vector inv_factors; std::size_t r = 0; for (std::size_t i = 0; i < min_mn; ++i) { if (D(i, i) != T(0)) { inv_factors.push_back(D(i, i)); ++r; } } return { std::move(U), std::move(D), std::move(V), std::move(inv_factors), r }; } //===================================================================== // Jordan Normal Form //===================================================================== /** * @brief Result of the Jordan Normal Form * * A = P * J * P^{-1} * J is a Jordan-block diagonal matrix (real approximation) * * Note: numerically unstable. For educational use / small matrices. */ template struct JordanResult { Matrix J; ///< Jordan matrix Matrix P; ///< transformation matrix (generalized eigenvectors) std::vector eigenvalues; ///< eigenvalues (real part only, with multiplicity) std::vector block_sizes; ///< size of each Jordan block }; namespace detail_jordan { /** * @brief Extract eigenvalues from the Schur form (1×1 real blocks, 2×2 complex blocks) * @return list of eigenvalues (real part only) and the corresponding algebraic multiplicities */ template std::vector> extract_eigenvalues( const BaseMatrix& Sch, T tol) { std::size_t n = Sch.rows(); std::vector eigs; std::size_t i = 0; while (i < n) { if (i + 1 < n && std::abs(Sch(i + 1, i)) > tol) { // 2×2 block: complex eigenvalue → add the real part twice T a = Sch(i, i), b = Sch(i, i + 1); T c = Sch(i + 1, i), d = Sch(i + 1, i + 1); T real_part = (a + d) / T(2); eigs.push_back(real_part); eigs.push_back(real_part); i += 2; } else { eigs.push_back(Sch(i, i)); i += 1; } } // Grouping (merge nearby eigenvalues) std::vector> grouped; std::vector used(eigs.size(), false); T group_tol = tol * T(100); for (std::size_t j = 0; j < eigs.size(); ++j) { if (used[j]) continue; T val = eigs[j]; std::size_t count = 1; used[j] = true; for (std::size_t k = j + 1; k < eigs.size(); ++k) { if (!used[k] && std::abs(eigs[k] - val) < group_tol) { count++; used[k] = true; } } grouped.push_back({val, count}); } return grouped; } /** * @brief Compute the dimension of the matrix's null space (rank deficiency) */ template std::size_t null_space_dim(const BaseMatrix& M, T tol) { std::size_t m = M.rows(), n = M.cols(); if (m == 0 || n == 0) return n; auto [U, sigma, Vt] = svd_decomposition(M); std::size_t rank = 0; T threshold = sigma[0] * static_cast(std::max(m, n)) * std::numeric_limits::epsilon() * T(10); threshold = std::max(threshold, tol); for (std::size_t i = 0; i < sigma.size(); ++i) { if (sigma[i] > threshold) ++rank; } return n - rank; } /** * @brief Compute a basis of the null space of (A - λI)^k via SVD * @return basis vectors of the null space (column vectors) */ template Matrix null_space_basis(const BaseMatrix& M, T tol) { std::size_t m = M.rows(), n = M.cols(); if (m == 0 || n == 0) return Matrix(n, n, T(0)); auto [U, sigma, Vt] = svd_decomposition(M); T threshold = sigma[0] * static_cast(std::max(m, n)) * std::numeric_limits::epsilon() * T(10); threshold = std::max(threshold, tol); std::size_t rank = 0; for (std::size_t i = 0; i < sigma.size(); ++i) if (sigma[i] > threshold) ++rank; std::size_t null_dim = n - rank; if (null_dim == 0) return Matrix(n, 0, T(0)); // The last null_dim columns of V = the transpose of the last null_dim rows of Vt Matrix basis(n, null_dim, T(0)); for (std::size_t j = 0; j < null_dim; ++j) for (std::size_t i = 0; i < n; ++i) basis(i, j) = Vt(rank + j, i); return basis; } /** * @brief Compute the matrix power M^k */ template Matrix mat_power(const BaseMatrix& M_basemat, std::size_t k) { Matrix M(M_basemat); // 1 copy via BaseMatrix ctor std::size_t n = M.rows(); if (k == 0) return Matrix::identity(n); Matrix result = M; for (std::size_t i = 1; i < k; ++i) result = result * M; return result; } } // namespace detail_jordan /** * @brief Jordan Normal Form * * Decompose a square matrix A into A = P * J * P^{-1}. * J is a Jordan-block diagonal matrix. * * Numerically unstable, so for educational use / small matrices (n ≤ ~20). * Complex eigenvalues are approximated by their real part only. * * @param A input matrix (n×n, square) * @return JordanResult * @throws DimensionError empty matrix, non-square matrix */ template requires sangi::BaseMatrixLike JordanResult> jordan_normal_form(const MA& A) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); std::size_t n = A.rows(); if (n == 0 || A.cols() != n) { assert(false && "DimensionError: jordan_normal_form: requires a non-empty square matrix"); throw DimensionError("jordan_normal_form: requires a non-empty square matrix"); } const T eps = std::numeric_limits::epsilon(); const T tol = eps * T(n) * T(100); // Case 1×1 if (n == 1) { Matrix J(1, 1, A(0, 0)); Matrix P = Matrix::identity(1); return { std::move(J), std::move(P), {A(0, 0)}, {1} }; } // Obtain the eigenvalues via Schur decomposition auto [Sch, Q] = schur_decomposition(A); auto eig_groups = detail_jordan::extract_eigenvalues(Sch, tol); // Construct a Jordan chain for each eigenvalue Matrix J(n, n, T(0)); Matrix P(n, n, T(0)); std::vector all_eigenvalues; std::vector block_sizes; std::size_t col_offset = 0; for (auto& [lambda, alg_mult] : eig_groups) { // A - λI Matrix AmlI(A); for (std::size_t i = 0; i < n; ++i) AmlI(i, i) -= lambda; // Compute the null-space dimension of (A - λI)^k and // determine the structure of the Jordan blocks // null_dim(k) - null_dim(k-1) = the number of blocks of size k or larger std::vector null_dims; null_dims.push_back(0); // k=0: null_dim = 0 Matrix Mk = Matrix::identity(n); for (std::size_t k = 1; k <= alg_mult; ++k) { Mk = Mk * AmlI; std::size_t nd = detail_jordan::null_space_dim(Mk, tol); nd = std::min(nd, alg_mult); // do not exceed the algebraic multiplicity null_dims.push_back(nd); if (nd >= alg_mult) break; } // Derive the Jordan block structure // Δ_k = null_dim(k) - null_dim(k-1) // Number of blocks of size s = Δ_s - Δ_{s+1} std::size_t max_k = null_dims.size() - 1; std::vector deltas(max_k + 2, 0); for (std::size_t k = 1; k <= max_k; ++k) deltas[k] = null_dims[k] - null_dims[k - 1]; std::vector> blocks; // (size, count) for (std::size_t s = max_k; s >= 1; --s) { std::size_t count = deltas[s] - (s + 1 <= max_k ? deltas[s + 1] : 0); if (count > 0) blocks.push_back({s, count}); } // Fallback when the block sizes cannot be determined std::size_t total_in_blocks = 0; for (auto& [sz, cnt] : blocks) total_in_blocks += sz * cnt; if (total_in_blocks < alg_mult) { // Fill the rest with 1×1 blocks std::size_t remaining = alg_mult - total_in_blocks; bool found_size1 = false; for (auto& [sz, cnt] : blocks) { if (sz == 1) { cnt += remaining; found_size1 = true; break; } } if (!found_size1) blocks.push_back({1, remaining}); } // Construct the basis vectors of the Jordan chain // For each block size s: // choose the head vector of the chain from the null space of (A-λI)^s, and // construct the basis with v, (A-λI)v, (A-λI)²v, ... std::vector> chain_vectors; // Orthogonal projection to manage the already-used directions std::vector> used_vectors; // Process from large to small block size (blocks is already large→small) for (auto& [sz, cnt] : blocks) { Matrix Mk_sz = detail_jordan::mat_power(AmlI, sz); Matrix null_basis = detail_jordan::null_space_basis(Mk_sz, tol); // To exclude the directions used in the previous stage, // subtract the projection of null_basis onto used_vectors for (std::size_t b = 0; b < cnt; ++b) { // From the columns of null_basis, find a direction orthogonal to used_vectors Vector seed(n, T(0)); bool found = false; for (std::size_t ci = 0; ci < null_basis.cols(); ++ci) { // Candidate vector Vector v(n, T(0)); for (std::size_t i = 0; i < n; ++i) v[i] = null_basis(i, ci); // Subtract the projection onto the used vectors (Gram-Schmidt) for (auto& u : used_vectors) { T dot = T(0), norm_sq = T(0); for (std::size_t i = 0; i < n; ++i) { dot += v[i] * u[i]; norm_sq += u[i] * u[i]; } if (norm_sq > tol) { T coeff = dot / norm_sq; for (std::size_t i = 0; i < n; ++i) v[i] -= coeff * u[i]; } } // Norm check T norm = T(0); for (std::size_t i = 0; i < n; ++i) norm += v[i] * v[i]; if (norm > tol * tol) { // Normalize norm = std::sqrt(norm); for (std::size_t i = 0; i < n; ++i) v[i] /= norm; seed = v; found = true; break; } } if (!found) { // Fallback: choose from the unit vectors for (std::size_t i = 0; i < n; ++i) { Vector ei(n, T(0)); ei[i] = T(1); for (auto& u : used_vectors) { T dot = T(0), norm_sq = T(0); for (std::size_t k = 0; k < n; ++k) { dot += ei[k] * u[k]; norm_sq += u[k] * u[k]; } if (norm_sq > tol) { T coeff = dot / norm_sq; for (std::size_t k = 0; k < n; ++k) ei[k] -= coeff * u[k]; } } T norm = T(0); for (std::size_t k = 0; k < n; ++k) norm += ei[k] * ei[k]; if (norm > tol * tol) { norm = std::sqrt(norm); for (std::size_t k = 0; k < n; ++k) ei[k] /= norm; seed = ei; found = true; break; } } } // Jordan chain: v_s = seed, v_{s-1} = (A-λI)*v_s, ... // Do not normalize, to preserve the relation A*v_k = λ*v_k + v_{k-1} std::vector> chain(sz); chain[sz - 1] = seed; for (std::size_t k = sz - 1; k > 0; --k) { Vector next(n, T(0)); for (std::size_t i = 0; i < n; ++i) for (std::size_t j = 0; j < n; ++j) next[i] += AmlI(i, j) * chain[k][j]; chain[k - 1] = next; } // Store into P and set J for (std::size_t k = 0; k < sz; ++k) { for (std::size_t i = 0; i < n; ++i) P(i, col_offset + k) = chain[k][i]; J(col_offset + k, col_offset + k) = lambda; if (k + 1 < sz) J(col_offset + k, col_offset + k + 1) = T(1); all_eigenvalues.push_back(lambda); } block_sizes.push_back(sz); // Record the used vectors for (auto& cv : chain) used_vectors.push_back(cv); col_offset += sz; } } if (col_offset > n) break; } return { std::move(J), std::move(P), std::move(all_eigenvalues), std::move(block_sizes) }; } //===================================================================== // QZ decomposition (generalized real Schur decomposition) //===================================================================== namespace detail_qz { // Givens rotation parameters: c*a + s*b = r, -s*a + c*b = 0 template struct GivensParams { T c, s; }; template GivensParams givens_rotation(T a, T b) { // c*a + s*b = r (> 0), -s*a + c*b = 0 // c = a/r, s = b/r, r = sqrt(a² + b²) if (a == T(0) && b == T(0)) { return {T(1), T(0)}; } T r = std::sqrt(a * a + b * b); return {a / r, b / r}; } // Left Givens rotation: apply to rows i1, i2 template void apply_givens_left(Matrix& M, int i1, int i2, T c, T s, int col_start, int col_end) { for (int j = col_start; j < col_end; ++j) { T t1 = M(i1, j); T t2 = M(i2, j); M(i1, j) = c * t1 + s * t2; M(i2, j) = -s * t1 + c * t2; } } // Right Givens rotation: apply to columns j1, j2 template void apply_givens_right(Matrix& M, int j1, int j2, T c, T s, int row_start, int row_end) { for (int i = row_start; i < row_end; ++i) { T t1 = M(i, j1); T t2 = M(i, j2); M(i, j1) = c * t1 + s * t2; M(i, j2) = -s * t1 + c * t2; } } } // namespace detail_qz /** * @brief QZ decomposition (generalized real Schur decomposition) * * A = Q·S·Zᵀ, B = Q·T·Zᵀ * S: quasi-upper-triangular (real Schur form) * T: upper triangular * Q, Z: orthogonal matrices * * Generalized eigenvalues λᵢ = S(i,i)/T(i,i) (1×1 block) * Complex conjugate pairs correspond to 2×2 blocks of S * * Algorithm: * Phase 1: Hessenberg-Triangular reduction (Moler-Stewart) * Phase 2: Francis double-shift QZ iteration * * @tparam T element type * @param A input matrix (n×n) * @param B input matrix (n×n) * @return {S, T, Q, Z} */ template requires sangi::BaseMatrixLike && sangi::BaseMatrixLike && std::same_as, sangi::element_t> std::tuple>, Matrix>, Matrix>, Matrix>> qz_decomposition(const MA& A, const MB& B) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); SANGI_STATIC_ASSERT_SQUARE(MB); const int n = static_cast(A.rows()); if (A.rows() != A.cols() || B.rows() != B.cols() || A.rows() != B.rows()) { assert(false && "DimensionError: qz_decomposition: requires square matrices of same size"); throw DimensionError("qz_decomposition: requires square matrices of same size"); } if (n == 0) { return {A, B, Matrix(0, 0), Matrix(0, 0)}; } if (n == 1) { Matrix I1(1, 1, T(1)); return {A, B, I1, I1}; } const T eps = std::numeric_limits::epsilon(); // ============================================================ // Phase 1: Hessenberg-Triangular reduction // ============================================================ // Step 1a: QR decomposition of B auto [Q0, R] = qr_decomposition(B); // H = Q0ᵀ * A Matrix H(n, n, T(0)); for (int i = 0; i < n; ++i) { for (int j = 0; j < n; ++j) { T sum = T(0); for (int k = 0; k < n; ++k) { sum += Q0(k, i) * A(k, j); // Q0ᵀ(i,k) = Q0(k,i) } H(i, j) = sum; } } // Initialize Q, Z // Q = Q0 (accumulation of left orthogonal transforms) Matrix Q = Q0; Matrix Z(n, n, T(0)); for (int i = 0; i < n; ++i) Z(i, i) = T(1); // Step 1b: reduce H to upper Hessenberg (preserving R's upper-triangularity) for (int k = 0; k < n - 2; ++k) { for (int i = n - 1; i >= k + 2; --i) { // Left Givens: zero out H(i, k) (rows i-1, i) auto [c, s] = detail_qz::givens_rotation(H(i - 1, k), H(i, k)); detail_qz::apply_givens_left(H, i - 1, i, c, s, k, n); detail_qz::apply_givens_left(R, i - 1, i, c, s, 0, n); // Accumulate Q: Q ← Q · G (from the right) detail_qz::apply_givens_right(Q, i - 1, i, c, s, 0, n); // Fill-in at R(i, i-1) → zero out with a right Givens (columns i-1, i) auto [c2, s2] = detail_qz::givens_rotation(R(i, i), R(i, i - 1)); // Rotate columns i, i-1 → R(i, i-1) = 0 detail_qz::apply_givens_right(R, i, i - 1, c2, s2, 0, n); detail_qz::apply_givens_right(H, i, i - 1, c2, s2, 0, n); // Accumulate Z: Z ← Z · G (from the right) detail_qz::apply_givens_right(Z, i, i - 1, c2, s2, 0, n); } } // Clean up tiny values for (int j = 0; j < n; ++j) { for (int i = j + 2; i < n; ++i) { H(i, j) = T(0); } } for (int j = 0; j < n; ++j) { for (int i = j + 1; i < n; ++i) { R(i, j) = T(0); } } // ============================================================ // Phase 2: single-shift QZ iteration (Givens-based) // ============================================================ const int max_iter = 300 * n; int ihi = n; int total_iter = 0; int iter_since_deflation = 0; while (ihi > 1 && total_iter < max_iter) { // Deflation: find a location where H's subdiagonal element is sufficiently small int ilo = ihi - 1; while (ilo > 0) { T threshold = eps * (std::abs(H(ilo - 1, ilo - 1)) + std::abs(H(ilo, ilo))); if (threshold == T(0)) threshold = eps; if (std::abs(H(ilo, ilo - 1)) <= threshold) { H(ilo, ilo - 1) = T(0); break; } --ilo; } if (ilo == ihi - 1) { // 1×1 block: deflate --ihi; iter_since_deflation = 0; continue; } if (ilo == ihi - 2) { // 2×2 block int p = ilo; T b11 = R(p, p), b22 = R(p + 1, p + 1); if (std::abs(b11) > eps && std::abs(b22) > eps) { T b12 = R(p, p + 1); T m11 = H(p, p) / b11; T m12 = H(p, p + 1) / b22 - H(p, p) * b12 / (b11 * b22); T m21 = H(p + 1, p) / b11; T m22 = H(p + 1, p + 1) / b22 - H(p + 1, p) * b12 / (b11 * b22); T disc = (m11 - m22) * (m11 - m22) + T(4) * m12 * m21; if (disc >= T(0)) { // Real eigenvalues: one QZ step with the Wilkinson shift T delta = (m11 - m22) / T(2); T sign_d = (delta >= T(0)) ? T(1) : T(-1); T sq = std::sqrt(delta * delta + m12 * m21); T sigma = m22 - m21 * m12 / (delta + sign_d * sq); T x2 = H(p, p) - sigma * R(p, p); T y2 = H(p + 1, p); auto [c, s] = detail_qz::givens_rotation(x2, y2); detail_qz::apply_givens_left(H, p, p + 1, c, s, 0, n); detail_qz::apply_givens_left(R, p, p + 1, c, s, 0, n); detail_qz::apply_givens_right(Q, p, p + 1, c, s, 0, n); // R fill-in: zero out R(p+1, p) auto [c2, s2] = detail_qz::givens_rotation(R(p + 1, p + 1), R(p + 1, p)); detail_qz::apply_givens_right(R, p + 1, p, c2, s2, 0, n); detail_qz::apply_givens_right(H, p + 1, p, c2, s2, 0, n); detail_qz::apply_givens_right(Z, p + 1, p, c2, s2, 0, n); R(p + 1, p) = T(0); } } ihi -= 2; iter_since_deflation = 0; continue; } // Single-shift QZ step // Wilkinson shift: among the eigenvalues of the bottom-right 2×2 M = R⁻¹H, // the one closer to M(ihi-1,ihi-1) T sigma; { int p = ihi - 2; T r11 = R(p, p), r12 = R(p, p + 1), r22 = R(p + 1, p + 1); if (std::abs(r11) < eps) r11 = eps; if (std::abs(r22) < eps) r22 = eps; T m11 = H(p, p) / r11; T m12 = H(p, p + 1) / r22 - H(p, p) * r12 / (r11 * r22); T m21 = H(p + 1, p) / r11; T m22 = H(p + 1, p + 1) / r22 - H(p + 1, p) * r12 / (r11 * r22); // Eigenvalues of the 2×2 T tr = m11 + m22; T det = m11 * m22 - m12 * m21; T disc = tr * tr - T(4) * det; if (disc >= T(0)) { T sq = std::sqrt(disc); T ev1 = (tr + sq) / T(2); T ev2 = (tr - sq) / T(2); sigma = (std::abs(ev1 - m22) < std::abs(ev2 - m22)) ? ev1 : ev2; } else { // Complex eigenvalues: use the real part as the shift sigma = tr / T(2); } // Exceptional shift if (iter_since_deflation > 0 && iter_since_deflation % 10 == 0) { T sub = std::abs(H(ihi - 1, ihi - 2)); sigma = m22 + T(0.75) * sub; } } // Initial Givens: the first column of (H - σR) T x = H(ilo, ilo) - sigma * R(ilo, ilo); T y = H(ilo + 1, ilo); for (int k = ilo; k < ihi - 1; ++k) { // Left Givens: rows (k, k+1), [x, y] → [r, 0] auto [c, s] = detail_qz::givens_rotation(x, y); detail_qz::apply_givens_left(H, k, k + 1, c, s, 0, n); detail_qz::apply_givens_left(R, k, k + 1, c, s, 0, n); detail_qz::apply_givens_right(Q, k, k + 1, c, s, 0, n); // Right Givens: zero out the R(k+1, k) fill-in // apply_givens_right(M, j1, j2, c, s): // M(:,j2) = -s*M(:,j1) + c*M(:,j2) // R(k+1, j2=k) = -s*R(k+1, j1=k+1) + c*R(k+1, k) = 0 // → givens(R(k+1,k+1), R(k+1,k)) auto [c2, s2] = detail_qz::givens_rotation(R(k + 1, k + 1), R(k + 1, k)); detail_qz::apply_givens_right(R, k + 1, k, c2, s2, 0, n); detail_qz::apply_givens_right(H, k + 1, k, c2, s2, 0, n); detail_qz::apply_givens_right(Z, k + 1, k, c2, s2, 0, n); R(k + 1, k) = T(0); // For the next step: bulge (H(k+2, k) is nonzero) if (k + 2 < ihi) { x = H(k + 1, k); y = H(k + 2, k); } } ++total_iter; ++iter_since_deflation; } // Clean up tiny values for (int i = 1; i < n; ++i) { T threshold = eps * (std::abs(H(i - 1, i - 1)) + std::abs(H(i, i))); if (threshold == T(0)) threshold = eps; if (std::abs(H(i, i - 1)) <= threshold) { H(i, i - 1) = T(0); } } for (int j = 0; j < n; ++j) { for (int i = j + 1; i < n; ++i) { R(i, j) = T(0); } } return {H, R, Q, Z}; } //===================================================================== // Iwasawa Decomposition //===================================================================== /** * @brief Result of the Iwasawa decomposition * * Decompose a nonsingular matrix G in GL(n,R) into G = K * A * N. * - K: n×n orthogonal matrix (maximal compact subgroup O(n)) * - A: n×n positive diagonal matrix (abelian subgroup) * - N: n×n upper triangular unipotent matrix (diagonal = 1, nilpotent subgroup) * * Algorithm: separate the diagonal signs from R in the QR decomposition G = Q*R * K = Q*D, A = |diag(R)|, N = A^{-1}*D*R (D = diag(sign(R_ii))) */ template struct IwasawaResult { Matrix K; ///< orthogonal matrix Matrix A; ///< positive diagonal matrix Matrix N; ///< upper triangular unipotent matrix (diagonal = 1) }; /** * @brief Iwasawa Decomposition * * Decompose a nonsingular matrix G (n×n) into G = K * A * N. * * @param G input matrix (nonsingular square matrix) * @return IwasawaResult * @throws DimensionError empty matrix or non-square matrix * @throws MathError singular matrix */ template requires sangi::BaseMatrixLike IwasawaResult> iwasawa_decomposition(const MA& G) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); std::size_t n = G.rows(); if (n == 0) { assert(false && "DimensionError: iwasawa_decomposition: requires a non-empty matrix"); throw DimensionError("iwasawa_decomposition: requires a non-empty matrix"); } if (n != G.cols()) { assert(false && "DimensionError: iwasawa_decomposition: requires a square matrix"); throw DimensionError("iwasawa_decomposition: requires a square matrix"); } // QR decomposition: G = Q * R auto [Q, R] = qr_decomposition(G); // Singularity check: singular if a diagonal element of R is zero T eps = std::numeric_limits::epsilon() * T(100); T max_diag = T(0); for (std::size_t i = 0; i < n; ++i) { T ad = std::abs(R(i, i)); if (ad > max_diag) max_diag = ad; } T sing_tol = max_diag * T(n) * eps; for (std::size_t i = 0; i < n; ++i) { if (std::abs(R(i, i)) <= sing_tol) throw MathError("iwasawa_decomposition: matrix is singular or near-singular"); } // D = diag(sign(R_ii)), K = Q * D // A = diag(|R_ii|) // N = A^{-1} * D * R (upper triangular unipotent) Matrix K(Q); Matrix A(n, n, T(0)); Matrix N(n, n, T(0)); // Absorb the diagonal signs into Q → K, the diagonal absolute values → A for (std::size_t j = 0; j < n; ++j) { T sign_j = (R(j, j) >= T(0)) ? T(1) : T(-1); T abs_rjj = std::abs(R(j, j)); A(j, j) = abs_rjj; // K(:, j) = Q(:, j) * sign_j for (std::size_t i = 0; i < n; ++i) K(i, j) = Q(i, j) * sign_j; // N(j, :) = (1/abs_rjj) * sign_j * R(j, :) // N(j, j) = 1 (diagonal), N(j, k) = sign_j * R(j, k) / abs_rjj for k > j T inv_a = T(1) / abs_rjj; N(j, j) = T(1); for (std::size_t k = j + 1; k < n; ++k) N(j, k) = sign_j * R(j, k) * inv_a; } return { std::move(K), std::move(A), std::move(N) }; } // ==================================================================== // ColPivHouseholderQR — QR decomposition with column pivoting // ==================================================================== /** * @brief Result of the column-pivoted QR decomposition * * A * P = Q * R (P is a column permutation matrix) * The rank can be obtained via rank(). */ template struct ColPivQRResult { Matrix Q; ///< orthogonal matrix (m×m) Matrix R; ///< upper triangular matrix (m×n) std::vector perm; ///< column permutation (perm[j] = original column number) /// numerical rank (|R(i,i)| > threshold) [[nodiscard]] std::size_t rank(T threshold = T(-1)) const { const auto k = std::min(R.rows(), R.cols()); if (k == 0) return 0; if (threshold < T(0)) { threshold = std::abs(R(0, 0)) * static_cast(std::max(R.rows(), R.cols())) * std::numeric_limits::epsilon(); } std::size_t r = 0; for (std::size_t i = 0; i < k; ++i) { if (std::abs(R(i, i)) > threshold) ++r; else break; } return r; } /// Obtain the permutation matrix P as a dense matrix [[nodiscard]] Matrix permutationMatrix() const { const auto n = perm.size(); Matrix P(n, n, T(0)); for (std::size_t j = 0; j < n; ++j) P(perm[j], j) = T(1); return P; } }; /** * @brief Column-pivoted Householder QR decomposition * * Select the column with the largest column norm as the pivot and compute a rank-revealing QR. * A * P = Q * R where P is a column permutation. */ template requires sangi::BaseMatrixLike ColPivQRResult> col_piv_qr(const MA& a) { using T = sangi::element_t; // rectangular OK const auto m = a.rows(); const auto n = a.cols(); const auto k = std::min(m, n); Matrix R = a; Matrix Q(m, m, T(0)); for (std::size_t i = 0; i < m; ++i) Q(i, i) = T(1); std::vector perm(n); std::iota(perm.begin(), perm.end(), std::size_t(0)); // Column norms (cached) std::vector col_norms(n); for (std::size_t j = 0; j < n; ++j) { T s = T(0); for (std::size_t i = 0; i < m; ++i) s += R(i, j) * R(i, j); col_norms[j] = s; } for (std::size_t step = 0; step < k; ++step) { // Select the column with the largest residual column norm std::size_t best = step; T best_norm = col_norms[step]; for (std::size_t j = step + 1; j < n; ++j) { if (col_norms[j] > best_norm) { best_norm = col_norms[j]; best = j; } } // Swap the columns if (best != step) { std::swap(perm[step], perm[best]); std::swap(col_norms[step], col_norms[best]); for (std::size_t i = 0; i < m; ++i) std::swap(R(i, step), R(i, best)); } // Householder reflector T sigma = T(0); for (std::size_t i = step; i < m; ++i) sigma += R(i, step) * R(i, step); sigma = std::sqrt(sigma); if (sigma < std::numeric_limits::epsilon()) continue; T alpha = (R(step, step) >= T(0)) ? -sigma : sigma; T beta = R(step, step) - alpha; R(step, step) = alpha; // v = [1, R(step+1:m, step) / beta] T inv_beta = T(1) / beta; for (std::size_t i = step + 1; i < m; ++i) R(i, step) *= inv_beta; T tau = -beta / alpha; // 2 / (v^T v) // Update R: R -= tau * v * (v^T * R) for (std::size_t j = step + 1; j < n; ++j) { T d = R(step, j); for (std::size_t i = step + 1; i < m; ++i) d += R(i, step) * R(i, j); d *= tau; R(step, j) -= d; for (std::size_t i = step + 1; i < m; ++i) R(i, j) -= R(i, step) * d; } // Update Q: Q -= tau * Q * v * v^T (multiply from the right) for (std::size_t i = 0; i < m; ++i) { T d = Q(i, step); for (std::size_t j = step + 1; j < m; ++j) d += Q(i, j) * R(j, step); d *= tau; Q(i, step) -= d; for (std::size_t j = step + 1; j < m; ++j) Q(i, j) -= R(j, step) * d; } // Clear R's lower triangle (free the storage for the reflector vectors) for (std::size_t i = step + 1; i < m; ++i) R(i, step) = T(0); // Update the column norms for (std::size_t j = step + 1; j < n; ++j) col_norms[j] -= R(step, j) * R(step, j); } return { std::move(Q), std::move(R), std::move(perm) }; } /// Compute the least-squares solution via column-pivoted QR (handles rank deficiency) /// Do not build Q explicitly; keep the Householder vectors and compute Q^T*b directly /// Block Householder (WY representation) + AVX2 FMA template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> col_piv_qr_solve(const MA& A, const VB& b) { using T = sangi::element_t; SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); if (A.rows() != b.size()) { assert(false && "DimensionError: col_piv_qr_solve: dimension mismatch"); throw DimensionError("col_piv_qr_solve: dimension mismatch"); } const auto m = A.rows(); const auto n = A.cols(); const auto k = std::min(m, n); using PT = PacketTraits; constexpr std::size_t W = PT::size; Matrix R = A; T* Rp = R.data(); std::vector perm(n); std::iota(perm.begin(), perm.end(), std::size_t(0)); std::vector tau_vec(k, T(0)); // Column norms std::vector col_norms(n); for (std::size_t j = 0; j < n; ++j) { T s = T(0); const T* col = Rp + j; for (std::size_t i = 0; i < m; ++i) s += col[i * n] * col[i * n]; col_norms[j] = s; } // Select unblocked/panel factorization depending on size constexpr std::size_t NB = 16; constexpr std::size_t PANEL_THRESHOLD = 384; if (n < PANEL_THRESHOLD) { // --- Unblocked version: apply directly to all subsequent columns (AVX2) --- for (std::size_t step = 0; step < k; ++step) { std::size_t best = step; T best_norm = col_norms[step]; for (std::size_t j = step + 1; j < n; ++j) if (col_norms[j] > best_norm) { best_norm = col_norms[j]; best = j; } if (best != step) { std::swap(perm[step], perm[best]); std::swap(col_norms[step], col_norms[best]); for (std::size_t i = 0; i < m; ++i) std::swap(Rp[i * n + step], Rp[i * n + best]); } T sigma = T(0); for (std::size_t i = step; i < m; ++i) sigma += Rp[i*n+step] * Rp[i*n+step]; sigma = std::sqrt(sigma); if (sigma < std::numeric_limits::epsilon()) continue; T alpha = (Rp[step*n+step] >= T(0)) ? -sigma : sigma; T beta_v = Rp[step*n+step] - alpha; Rp[step*n+step] = alpha; T inv_beta = T(1) / beta_v; for (std::size_t i = step+1; i < m; ++i) Rp[i*n+step] *= inv_beta; T tau = -beta_v / alpha; tau_vec[step] = tau; std::size_t j = step + 1; if constexpr (W > 1) { auto vtau = PT::set1(tau); for (; j + W - 1 < n; j += W) { auto d_vec = PT::load(Rp + step*n + j); for (std::size_t i = step+1; i < m; ++i) d_vec = PT::fmadd(PT::set1(Rp[i*n+step]), PT::load(Rp + i*n + j), d_vec); d_vec = PT::mul(d_vec, vtau); PT::store(Rp + step*n + j, PT::sub(PT::load(Rp + step*n + j), d_vec)); for (std::size_t i = step+1; i < m; ++i) { auto vi = PT::set1(Rp[i*n+step]); PT::store(Rp + i*n + j, PT::sub(PT::load(Rp + i*n + j), PT::mul(vi, d_vec))); } } } for (; j < n; ++j) { T d = Rp[step*n + j]; for (std::size_t i = step+1; i < m; ++i) d += Rp[i*n+step] * Rp[i*n+j]; d *= tau; Rp[step*n + j] -= d; for (std::size_t i = step+1; i < m; ++i) Rp[i*n+j] -= Rp[i*n+step] * d; } for (std::size_t jj = step+1; jj < n; ++jj) col_norms[jj] -= Rp[step*n+jj] * Rp[step*n+jj]; } } else { // --- Panel factorization (F matrix) + trailing GEMM --- std::vector F_t(NB * n, T(0)); for (std::size_t panel_start = 0; panel_start < k; panel_start += NB) { std::size_t panel_end = std::min(panel_start + NB, k); std::size_t nb = panel_end - panel_start; for (std::size_t ll = 0; ll < nb; ++ll) std::fill(&F_t[ll * n], &F_t[ll * n + n], T(0)); for (std::size_t l = 0; l < nb; ++l) { std::size_t step = panel_start + l; std::size_t best = step; T best_norm = col_norms[step]; for (std::size_t j = step + 1; j < n; ++j) if (col_norms[j] > best_norm) { best_norm = col_norms[j]; best = j; } // When swapping a subsequent column into the panel: apply the lazy update first if (best != step) { if (best >= panel_end && l > 0) { // Apply the accumulated F to the best column before swapping for (std::size_t i = panel_start; i < step; ++i) { std::size_t i_local = i - panel_start; T c = F_t[i_local * n + best]; // V(i,ps+i_local)=1 for (std::size_t s = 0; s < i_local; ++s) c += Rp[i * n + (panel_start+s)] * F_t[s * n + best]; Rp[i * n + best] -= c; } for (std::size_t i = step; i < m; ++i) { T c = T(0); for (std::size_t s = 0; s < l; ++s) c += Rp[i * n + (panel_start+s)] * F_t[s * n + best]; Rp[i * n + best] -= c; } for (std::size_t s = 0; s < l; ++s) F_t[s * n + best] = T(0); } std::swap(perm[step], perm[best]); std::swap(col_norms[step], col_norms[best]); for (std::size_t i = 0; i < m; ++i) std::swap(Rp[i * n + step], Rp[i * n + best]); for (std::size_t s = 0; s < l; ++s) std::swap(F_t[s * n + step], F_t[s * n + best]); } // Householder T sigma = T(0); for (std::size_t i = step; i < m; ++i) sigma += Rp[i * n + step] * Rp[i * n + step]; sigma = std::sqrt(sigma); if (sigma < std::numeric_limits::epsilon()) continue; T alpha = (Rp[step * n + step] >= T(0)) ? -sigma : sigma; T beta_v = Rp[step * n + step] - alpha; Rp[step * n + step] = alpha; T inv_beta = T(1) / beta_v; for (std::size_t i = step + 1; i < m; ++i) Rp[i * n + step] *= inv_beta; T tau = -beta_v / alpha; tau_vec[step] = tau; // Apply directly to the remaining columns in the panel { std::size_t j = step + 1; if constexpr (W > 1) { auto vtau = PT::set1(tau); for (; j + W - 1 < panel_end; j += W) { auto d_vec = PT::load(Rp + step*n + j); for (std::size_t i = step+1; i < m; ++i) d_vec = PT::fmadd(PT::set1(Rp[i*n+step]), PT::load(Rp + i*n + j), d_vec); d_vec = PT::mul(d_vec, vtau); PT::store(Rp + step*n + j, PT::sub(PT::load(Rp + step*n + j), d_vec)); for (std::size_t i = step+1; i < m; ++i) { auto vi = PT::set1(Rp[i*n+step]); PT::store(Rp + i*n + j, PT::sub(PT::load(Rp + i*n + j), PT::mul(vi, d_vec))); } } } for (; j < panel_end; ++j) { T d = Rp[step*n + j]; for (std::size_t i = step+1; i < m; ++i) d += Rp[i*n+step] * Rp[i*n+j]; d *= tau; Rp[step*n + j] -= d; for (std::size_t i = step+1; i < m; ++i) Rp[i*n+j] -= Rp[i*n+step] * d; } } // Build F: subsequent columns j ∈ [panel_end, n) if (panel_end < n) { T y_buf[NB]; for (std::size_t s = 0; s < l; ++s) { std::size_t vs_col = panel_start + s; T dot = Rp[step * n + vs_col]; for (std::size_t i = step + 1; i < m; ++i) dot += Rp[i * n + vs_col] * Rp[i * n + step]; y_buf[s] = dot; } std::size_t j = panel_end; if constexpr (W > 1) { auto vtau = PT::set1(tau); for (; j + W - 1 < n; j += W) { auto z = PT::load(Rp + step*n + j); for (std::size_t i = step+1; i < m; ++i) z = PT::fmadd(PT::set1(Rp[i*n+step]), PT::load(Rp + i*n + j), z); for (std::size_t s = 0; s < l; ++s) z = PT::sub(z, PT::mul(PT::set1(y_buf[s]), PT::load(&F_t[s * n + j]))); PT::store(&F_t[l * n + j], PT::mul(vtau, z)); } } for (; j < n; ++j) { T z = Rp[step*n + j]; for (std::size_t i = step+1; i < m; ++i) z += Rp[i*n+step] * Rp[i*n+j]; for (std::size_t s = 0; s < l; ++s) z -= y_buf[s] * F_t[s * n + j]; F_t[l * n + j] = tau * z; } } // Norm update: panel columns for (std::size_t j = step + 1; j < panel_end; ++j) col_norms[j] -= Rp[step*n + j] * Rp[step*n + j]; // Norm update: subsequent columns (virtual R_eff) for (std::size_t j = panel_end; j < n; ++j) { T r_eff = Rp[step*n + j]; for (std::size_t s = 0; s < l; ++s) r_eff -= Rp[step*n + (panel_start+s)] * F_t[s * n + j]; r_eff -= F_t[l * n + j]; col_norms[j] -= r_eff * r_eff; } } // Trailing GEMM: R -= V * F_t^T (branchless optimization) if (panel_end < n) { // Triangular part: rows [panel_start, panel_end) for (std::size_t i = panel_start; i < panel_end; ++i) { std::size_t i_local = i - panel_start; std::size_t j = panel_end; if constexpr (W > 1) { for (; j + W - 1 < n; j += W) { auto acc = PT::load(&F_t[i_local * n + j]); // V(i,ps+i_local)=1 for (std::size_t ll = 0; ll < i_local; ++ll) acc = PT::fmadd(PT::set1(Rp[i*n + panel_start+ll]), PT::load(&F_t[ll * n + j]), acc); PT::store(Rp + i*n + j, PT::sub(PT::load(Rp + i*n + j), acc)); } } for (; j < n; ++j) { T s = F_t[i_local * n + j]; for (std::size_t ll = 0; ll < i_local; ++ll) s += Rp[i*n + panel_start+ll] * F_t[ll * n + j]; Rp[i*n + j] -= s; } } // Rectangular part: rows [panel_end, m) — all V(i,l) = R(i,ps+l) for (std::size_t i = panel_end; i < m; ++i) { std::size_t j = panel_end; if constexpr (W > 1) { for (; j + W - 1 < n; j += W) { auto acc = PT::set1(T(0)); for (std::size_t ll = 0; ll < nb; ++ll) acc = PT::fmadd(PT::set1(Rp[i*n + panel_start+ll]), PT::load(&F_t[ll * n + j]), acc); PT::store(Rp + i*n + j, PT::sub(PT::load(Rp + i*n + j), acc)); } } for (; j < n; ++j) { T s = T(0); for (std::size_t ll = 0; ll < nb; ++ll) s += Rp[i*n + panel_start+ll] * F_t[ll * n + j]; Rp[i*n + j] -= s; } } } } } // Rank determination T threshold = std::abs(Rp[0]) * T(m) * std::numeric_limits::epsilon(); std::size_t r = 0; for (std::size_t i = 0; i < k; ++i) { if (std::abs(Rp[i * n + i]) > threshold) ++r; else break; } // Compute Q^T * b directly via Householder reflections (AVX2) Vector qtb = b; T* qp = qtb.data(); for (std::size_t step = 0; step < k; ++step) { if (tau_vec[step] == T(0)) continue; const std::size_t len = m - step - 1; std::size_t i = step + 1; T d; if constexpr (W > 1) { auto acc = PT::set1(T(0)); std::size_t endw = step + 1 + (len & ~(W - 1)); for (; i < endw; i += W) acc = PT::fmadd(PT::load(Rp + i * n + step), // stride-n... non-contiguous PT::load(qp + i), acc); // this is contiguous but Rp is non-contiguous // Rp[i*n+step] is stride-n → AVX2 gather would be needed // → scalar fallback i = step + 1; d = qp[step]; for (; i < m; ++i) d += Rp[i * n + step] * qp[i]; } else { d = qp[step]; for (; i < m; ++i) d += Rp[i * n + step] * qp[i]; } d *= tau_vec[step]; qp[step] -= d; for (std::size_t i2 = step + 1; i2 < m; ++i2) qp[i2] -= Rp[i2 * n + step] * d; } // Backward substitution (AVX2 inner product) Vector y(n, T(0)); T* yp = y.data(); for (std::size_t ii = 0; ii < r; ++ii) { std::size_t i = r - 1 - ii; const T* row = Rp + i * n; std::size_t j = i + 1; if constexpr (W > 1) { auto acc = PT::set1(T(0)); std::size_t endw = i + 1 + ((r - i - 1) & ~(W - 1)); for (; j < endw; j += W) acc = PT::fmadd(PT::load(row + j), PT::load(yp + j), acc); T sum = PT::reduce_add(acc); for (; j < r; ++j) sum += row[j] * yp[j]; yp[i] = (qtb[i] - sum) / row[i]; } else { T sum = T(0); for (; j < r; ++j) sum += row[j] * yp[j]; yp[i] = (qtb[i] - sum) / row[i]; } } // Apply the inverse permutation Vector x(n, T(0)); for (std::size_t j = 0; j < n; ++j) x[perm[j]] = yp[j]; return x; } // ==================================================================== // FullPivLU — full-pivoting LU decomposition // ==================================================================== /** * @brief Result of the full-pivoting LU decomposition * * P * A * Q = L * U (P is a row permutation, Q is a column permutation) */ template struct FullPivLUResult { Matrix LU; ///< L and U combined (the diagonal is U) std::vector row_perm; ///< row permutation std::vector col_perm; ///< column permutation std::size_t pivots; ///< number of pivots (for sign computation) /// numerical rank [[nodiscard]] std::size_t rank(T threshold = T(-1)) const { const auto k = std::min(LU.rows(), LU.cols()); if (k == 0) return 0; if (threshold < T(0)) { threshold = std::abs(LU(0, 0)) * static_cast(std::max(LU.rows(), LU.cols())) * std::numeric_limits::epsilon(); } std::size_t r = 0; for (std::size_t i = 0; i < k; ++i) { if (std::abs(LU(i, i)) > threshold) ++r; else break; } return r; } /// determinant (square matrices only) [[nodiscard]] T determinant() const { const auto n = LU.rows(); if (n != LU.cols()) { assert(false && "DimensionError: FullPivLU::determinant: matrix must be square"); throw DimensionError("FullPivLU::determinant: matrix must be square"); } T det = (pivots % 2 == 0) ? T(1) : T(-1); for (std::size_t i = 0; i < n; ++i) det *= LU(i, i); return det; } /// Obtain the L matrix [[nodiscard]] Matrix matrixL() const { const auto m = LU.rows(); const auto k = std::min(m, LU.cols()); Matrix L(m, k, T(0)); for (std::size_t i = 0; i < m; ++i) { for (std::size_t j = 0; j < std::min(i, k); ++j) L(i, j) = LU(i, j); if (i < k) L(i, i) = T(1); } return L; } /// Obtain the U matrix [[nodiscard]] Matrix matrixU() const { const auto k = std::min(LU.rows(), LU.cols()); const auto n = LU.cols(); Matrix U(k, n, T(0)); for (std::size_t i = 0; i < k; ++i) for (std::size_t j = i; j < n; ++j) U(i, j) = LU(i, j); return U; } }; /** * @brief Full-pivoting LU decomposition * * Select pivots in both rows and columns to maximize numerical stability. * P * A * Q = L * U */ template requires sangi::BaseMatrixLike FullPivLUResult> full_piv_lu(const MA& a) { using T = sangi::element_t; // rectangular OK const auto m = a.rows(); const auto n = a.cols(); const auto k = std::min(m, n); Matrix LU = a; T* Lp = LU.data(); std::vector rperm(m), cperm(n); std::iota(rperm.begin(), rperm.end(), std::size_t(0)); std::iota(cperm.begin(), cperm.end(), std::size_t(0)); std::size_t swaps = 0; for (std::size_t step = 0; step < k; ++step) { // Search for the maximum over all elements T best_val = T(0); std::size_t best_i = step, best_j = step; for (std::size_t i = step; i < m; ++i) { const T* row = Lp + i * n; for (std::size_t j = step; j < n; ++j) { T v = std::abs(row[j]); if (v > best_val) { best_val = v; best_i = i; best_j = j; } } } if (best_val < std::numeric_limits::epsilon()) break; // Row swap if (best_i != step) { std::swap(rperm[step], rperm[best_i]); T* r1 = Lp + step * n; T* r2 = Lp + best_i * n; for (std::size_t j = 0; j < n; ++j) std::swap(r1[j], r2[j]); ++swaps; } // Column swap if (best_j != step) { std::swap(cperm[step], cperm[best_j]); for (std::size_t i = 0; i < m; ++i) std::swap(Lp[i * n + step], Lp[i * n + best_j]); ++swaps; } // Elimination T* pivot_row = Lp + step * n; T inv_pivot = T(1) / pivot_row[step]; for (std::size_t i = step + 1; i < m; ++i) { T* row = Lp + i * n; T factor = row[step] * inv_pivot; row[step] = factor; // element of L for (std::size_t j = step + 1; j < n; ++j) row[j] -= factor * pivot_row[j]; } } return { std::move(LU), std::move(rperm), std::move(cperm), swaps }; } /// Solve a system of linear equations via full-pivoting LU template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> full_piv_lu_solve(const MA& A, const VB& b) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); const auto m = A.rows(); const auto n = A.cols(); if (m != n) { assert(false && "DimensionError: full_piv_lu_solve: matrix must be square"); throw DimensionError("full_piv_lu_solve: matrix must be square"); } if (b.size() != m) { assert(false && "DimensionError: full_piv_lu_solve: dimension mismatch"); throw DimensionError("full_piv_lu_solve: dimension mismatch"); } auto result = full_piv_lu(A); const T* Lp = result.LU.data(); // P * b (apply the row permutation) Vector pb(m); for (std::size_t i = 0; i < m; ++i) pb[i] = b[result.row_perm[i]]; T* pp = pb.data(); // Forward substitution: L * y = P * b for (std::size_t i = 1; i < m; ++i) { const T* row = Lp + i * m; T sum = T(0); for (std::size_t j = 0; j < i; ++j) sum += row[j] * pp[j]; pp[i] -= sum; } // Backward substitution: U * z = y for (std::size_t ii = 0; ii < m; ++ii) { std::size_t i = m - 1 - ii; const T* row = Lp + i * m; T sum = T(0); for (std::size_t j = i + 1; j < m; ++j) sum += row[j] * pp[j]; pp[i] = (pp[i] - sum) / row[i]; } // Q^T * z → x (apply the inverse of the column permutation) Vector x(n); for (std::size_t j = 0; j < n; ++j) x[result.col_perm[j]] = pb[j]; return x; } // ==================================================================== // CompleteOrthogonalDecomposition — complete orthogonal decomposition // ==================================================================== /** * @brief Result of the complete orthogonal decomposition * * A * P = Q * [T 0; 0 0] * Z * Here T is r×r upper triangular, Q is m×m orthogonal, Z is n×n orthogonal, and P is a column permutation. * Used for the minimum-norm least-squares solution of a rank-deficient matrix. */ template struct CODResult { Matrix Q; ///< orthogonal matrix (m×m) Matrix T_mat; ///< upper triangular (r×r) Matrix Z; ///< orthogonal matrix (n×n) std::vector perm; ///< column permutation std::size_t r; ///< rank [[nodiscard]] std::size_t rank() const { return r; } }; /** * @brief Complete orthogonal decomposition * * After ColPivQR, apply a right Householder to the first r rows of R so that * R[:r,:] = T * Z^T (T is r×r upper triangular). */ template requires sangi::BaseMatrixLike CODResult> complete_orthogonal_decomposition(const MA& A) { using T = sangi::element_t; // rectangular OK auto qr = col_piv_qr(A); const auto m = A.rows(); const auto n = A.cols(); const auto r = qr.rank(); if (r == 0 || r == n) { // Rank 0 or full rank → Z = I Matrix Zmat(n, n, T(0)); for (std::size_t i = 0; i < n; ++i) Zmat(i, i) = T(1); Matrix Tmat(r, r, T(0)); for (std::size_t i = 0; i < r; ++i) for (std::size_t j = i; j < r; ++j) Tmat(i, j) = qr.R(i, j); return { std::move(qr.Q), std::move(Tmat), std::move(Zmat), std::move(qr.perm), r }; } // RZ decomposition: zero out the right part R[:r, r:] of R[:r,:] // For each row k (k = r-1 → 0), apply a Householder to the vector [R(k,k), R(k,r:n)] // to zero out R(k,r:n) Matrix Rwork(r, n, T(0)); for (std::size_t i = 0; i < r; ++i) for (std::size_t j = 0; j < n; ++j) Rwork(i, j) = qr.R(i, j); Matrix Zmat(n, n, T(0)); for (std::size_t i = 0; i < n; ++i) Zmat(i, i) = T(1); for (std::size_t kk = 0; kk < r; ++kk) { std::size_t k = r - 1 - kk; if (r >= n) continue; // Via Householder, [R(k,k), R(k,r), R(k,r+1), ...] → [sigma, 0, 0, ...] // The reflector vector v is scattered over positions k and r:n const std::size_t tail = n - r; T x0 = Rwork(k, k); T sigma_sq = x0 * x0; for (std::size_t j = r; j < n; ++j) sigma_sq += Rwork(k, j) * Rwork(k, j); T sigma = std::sqrt(sigma_sq); if (sigma < std::numeric_limits::epsilon()) continue; T alpha = (x0 >= T(0)) ? -sigma : sigma; T beta = x0 - alpha; if (std::abs(beta) < std::numeric_limits::epsilon()) continue; // v = [1, Rwork(k, r:n) / beta], v[0] corresponds to position k T inv_beta = T(1) / beta; std::vector vtail(tail); for (std::size_t j = 0; j < tail; ++j) vtail[j] = Rwork(k, r + j) * inv_beta; T tau = -beta / alpha; // = 2 / (1 + ||vtail||^2) Rwork(k, k) = alpha; for (std::size_t j = r; j < n; ++j) Rwork(k, j) = T(0); // Apply the reflector to the other rows of Rwork from the right (rows 0..k-1) // row[k] += tau * (row[k] + sum(vtail[j]*row[r+j])) → corrected for (std::size_t i = 0; i < k; ++i) { T d = Rwork(i, k); for (std::size_t j = 0; j < tail; ++j) d += vtail[j] * Rwork(i, r + j); d *= tau; Rwork(i, k) -= d; for (std::size_t j = 0; j < tail; ++j) Rwork(i, r + j) -= vtail[j] * d; } // Update Z: apply the reflector to column k and columns r:n of Z for (std::size_t i = 0; i < n; ++i) { T d = Zmat(i, k); for (std::size_t j = 0; j < tail; ++j) d += vtail[j] * Zmat(i, r + j); d *= tau; Zmat(i, k) -= d; for (std::size_t j = 0; j < tail; ++j) Zmat(i, r + j) -= vtail[j] * d; } } // T = Rwork[:r, :r] Matrix Tmat(r, r, T(0)); for (std::size_t i = 0; i < r; ++i) for (std::size_t j = i; j < r; ++j) Tmat(i, j) = Rwork(i, j); return { std::move(qr.Q), std::move(Tmat), std::move(Zmat), std::move(qr.perm), r }; } /// Compute the minimum-norm least-squares solution via complete orthogonal decomposition /// Do not build Q, Z explicitly; compute directly with the Householder vectors template requires sangi::BaseMatrixLike && sangi::BaseVectorLike && std::same_as, sangi::element_t> Vector> cod_solve(const MA& A, const VB& b) { using T = sangi::element_t; SANGI_STATIC_ASSERT_MATRIX_ROWS_EQ_VECTOR_SIZE(MA, VB); if (A.rows() != b.size()) { assert(false && "DimensionError: cod_solve: dimension mismatch"); throw DimensionError("cod_solve: dimension mismatch"); } const auto m = A.rows(); const auto n = A.cols(); const auto k = std::min(m, n); // --- Phase 1: ColPivQR + AVX2 (keep the Householder vectors in R's lower triangle) --- using PT = PacketTraits; constexpr std::size_t W = PT::size; Matrix R = A; T* Rp = R.data(); std::vector perm(n); std::iota(perm.begin(), perm.end(), std::size_t(0)); std::vector tau_vec(k, T(0)); std::vector col_norms(n); for (std::size_t j = 0; j < n; ++j) { T s = T(0); for (std::size_t i = 0; i < m; ++i) s += Rp[i * n + j] * Rp[i * n + j]; col_norms[j] = s; } // Select unblocked/panel factorization depending on size constexpr std::size_t NB_COD = 16; constexpr std::size_t COD_PANEL_THRESHOLD = 384; if (n < COD_PANEL_THRESHOLD) { // --- Unblocked version --- for (std::size_t step = 0; step < k; ++step) { std::size_t best = step; T best_norm = col_norms[step]; for (std::size_t j = step + 1; j < n; ++j) if (col_norms[j] > best_norm) { best_norm = col_norms[j]; best = j; } if (best != step) { std::swap(perm[step], perm[best]); std::swap(col_norms[step], col_norms[best]); for (std::size_t i = 0; i < m; ++i) std::swap(Rp[i*n+step], Rp[i*n+best]); } T sigma = T(0); for (std::size_t i = step; i < m; ++i) sigma += Rp[i*n+step]*Rp[i*n+step]; sigma = std::sqrt(sigma); if (sigma < std::numeric_limits::epsilon()) continue; T alpha = (Rp[step*n+step] >= T(0)) ? -sigma : sigma; T beta_v = Rp[step*n+step] - alpha; Rp[step*n+step] = alpha; T inv_beta = T(1) / beta_v; for (std::size_t i = step+1; i < m; ++i) Rp[i*n+step] *= inv_beta; T tau = -beta_v / alpha; tau_vec[step] = tau; std::size_t j = step + 1; if constexpr (W > 1) { auto vtau = PT::set1(tau); for (; j + W - 1 < n; j += W) { auto d_vec = PT::load(Rp + step*n + j); for (std::size_t i = step+1; i < m; ++i) d_vec = PT::fmadd(PT::set1(Rp[i*n+step]), PT::load(Rp + i*n + j), d_vec); d_vec = PT::mul(d_vec, vtau); PT::store(Rp + step*n + j, PT::sub(PT::load(Rp + step*n + j), d_vec)); for (std::size_t i = step+1; i < m; ++i) { auto vi = PT::set1(Rp[i*n+step]); PT::store(Rp + i*n + j, PT::sub(PT::load(Rp + i*n + j), PT::mul(vi, d_vec))); } } } for (; j < n; ++j) { T d = Rp[step*n+j]; for (std::size_t i = step+1; i < m; ++i) d += Rp[i*n+step]*Rp[i*n+j]; d *= tau; Rp[step*n+j] -= d; for (std::size_t i = step+1; i < m; ++i) Rp[i*n+j] -= Rp[i*n+step]*d; } for (std::size_t jj = step+1; jj < n; ++jj) col_norms[jj] -= Rp[step*n+jj]*Rp[step*n+jj]; } } else { // --- Panel factorization (F matrix) + trailing GEMM --- std::vector F_t_cod(NB_COD * n, T(0)); for (std::size_t panel_start = 0; panel_start < k; panel_start += NB_COD) { std::size_t panel_end = std::min(panel_start + NB_COD, k); std::size_t nb = panel_end - panel_start; for (std::size_t ll = 0; ll < nb; ++ll) std::fill(&F_t_cod[ll*n], &F_t_cod[ll*n+n], T(0)); for (std::size_t l = 0; l < nb; ++l) { std::size_t step = panel_start + l; std::size_t best = step; T best_norm = col_norms[step]; for (std::size_t j = step + 1; j < n; ++j) if (col_norms[j] > best_norm) { best_norm = col_norms[j]; best = j; } if (best != step) { if (best >= panel_end && l > 0) { for (std::size_t i = panel_start; i < step; ++i) { std::size_t i_local = i - panel_start; T c = F_t_cod[i_local * n + best]; for (std::size_t s = 0; s < i_local; ++s) c += Rp[i*n+(panel_start+s)] * F_t_cod[s*n+best]; Rp[i*n+best] -= c; } for (std::size_t i = step; i < m; ++i) { T c = T(0); for (std::size_t s = 0; s < l; ++s) c += Rp[i*n+(panel_start+s)] * F_t_cod[s*n+best]; Rp[i*n+best] -= c; } for (std::size_t s = 0; s < l; ++s) F_t_cod[s*n+best] = T(0); } std::swap(perm[step], perm[best]); std::swap(col_norms[step], col_norms[best]); for (std::size_t i = 0; i < m; ++i) std::swap(Rp[i*n+step], Rp[i*n+best]); for (std::size_t s = 0; s < l; ++s) std::swap(F_t_cod[s*n+step], F_t_cod[s*n+best]); } T sigma = T(0); for (std::size_t i = step; i < m; ++i) sigma += Rp[i*n+step]*Rp[i*n+step]; sigma = std::sqrt(sigma); if (sigma < std::numeric_limits::epsilon()) continue; T alpha = (Rp[step*n+step] >= T(0)) ? -sigma : sigma; T beta_v = Rp[step*n+step] - alpha; Rp[step*n+step] = alpha; T inv_beta = T(1) / beta_v; for (std::size_t i = step+1; i < m; ++i) Rp[i*n+step] *= inv_beta; T tau = -beta_v / alpha; tau_vec[step] = tau; { std::size_t j = step + 1; if constexpr (W > 1) { auto vtau = PT::set1(tau); for (; j + W - 1 < panel_end; j += W) { auto d_vec = PT::load(Rp + step*n + j); for (std::size_t i = step+1; i < m; ++i) d_vec = PT::fmadd(PT::set1(Rp[i*n+step]), PT::load(Rp + i*n + j), d_vec); d_vec = PT::mul(d_vec, vtau); PT::store(Rp + step*n + j, PT::sub(PT::load(Rp + step*n + j), d_vec)); for (std::size_t i = step+1; i < m; ++i) { auto vi = PT::set1(Rp[i*n+step]); PT::store(Rp + i*n + j, PT::sub(PT::load(Rp + i*n + j), PT::mul(vi, d_vec))); } } } for (; j < panel_end; ++j) { T d = Rp[step*n+j]; for (std::size_t i = step+1; i < m; ++i) d += Rp[i*n+step]*Rp[i*n+j]; d *= tau; Rp[step*n+j] -= d; for (std::size_t i = step+1; i < m; ++i) Rp[i*n+j] -= Rp[i*n+step]*d; } } if (panel_end < n) { T y_buf[NB_COD]; for (std::size_t s = 0; s < l; ++s) { std::size_t vs_col = panel_start + s; T dot = Rp[step*n+vs_col]; for (std::size_t i = step+1; i < m; ++i) dot += Rp[i*n+vs_col] * Rp[i*n+step]; y_buf[s] = dot; } std::size_t j = panel_end; if constexpr (W > 1) { auto vtau = PT::set1(tau); for (; j + W - 1 < n; j += W) { auto z = PT::load(Rp + step*n + j); for (std::size_t i = step+1; i < m; ++i) z = PT::fmadd(PT::set1(Rp[i*n+step]), PT::load(Rp + i*n + j), z); for (std::size_t s = 0; s < l; ++s) z = PT::sub(z, PT::mul(PT::set1(y_buf[s]), PT::load(&F_t_cod[s*n + j]))); PT::store(&F_t_cod[l*n + j], PT::mul(vtau, z)); } } for (; j < n; ++j) { T z = Rp[step*n+j]; for (std::size_t i = step+1; i < m; ++i) z += Rp[i*n+step]*Rp[i*n+j]; for (std::size_t s = 0; s < l; ++s) z -= y_buf[s] * F_t_cod[s*n+j]; F_t_cod[l*n+j] = tau * z; } } for (std::size_t j = step+1; j < panel_end; ++j) col_norms[j] -= Rp[step*n+j]*Rp[step*n+j]; for (std::size_t j = panel_end; j < n; ++j) { T r_eff = Rp[step*n+j]; for (std::size_t s = 0; s < l; ++s) r_eff -= Rp[step*n+(panel_start+s)] * F_t_cod[s*n+j]; r_eff -= F_t_cod[l*n+j]; col_norms[j] -= r_eff * r_eff; } } if (panel_end < n) { for (std::size_t i = panel_start; i < panel_end; ++i) { std::size_t i_local = i - panel_start; std::size_t j = panel_end; if constexpr (W > 1) { for (; j + W - 1 < n; j += W) { auto acc = PT::load(&F_t_cod[i_local*n + j]); for (std::size_t ll = 0; ll < i_local; ++ll) acc = PT::fmadd(PT::set1(Rp[i*n+panel_start+ll]), PT::load(&F_t_cod[ll*n + j]), acc); PT::store(Rp + i*n + j, PT::sub(PT::load(Rp + i*n + j), acc)); } } for (; j < n; ++j) { T s = F_t_cod[i_local*n+j]; for (std::size_t ll = 0; ll < i_local; ++ll) s += Rp[i*n+panel_start+ll] * F_t_cod[ll*n+j]; Rp[i*n+j] -= s; } } for (std::size_t i = panel_end; i < m; ++i) { std::size_t j = panel_end; if constexpr (W > 1) { for (; j + W - 1 < n; j += W) { auto acc = PT::set1(T(0)); for (std::size_t ll = 0; ll < nb; ++ll) acc = PT::fmadd(PT::set1(Rp[i*n+panel_start+ll]), PT::load(&F_t_cod[ll*n + j]), acc); PT::store(Rp + i*n + j, PT::sub(PT::load(Rp + i*n + j), acc)); } } for (; j < n; ++j) { T s = T(0); for (std::size_t ll = 0; ll < nb; ++ll) s += Rp[i*n+panel_start+ll] * F_t_cod[ll*n+j]; Rp[i*n+j] -= s; } } } } } // Rank determination T threshold = std::abs(Rp[0]) * T(m) * std::numeric_limits::epsilon(); std::size_t r = 0; for (std::size_t i = 0; i < k; ++i) { if (std::abs(Rp[i * n + i]) > threshold) ++r; else break; } // --- Phase 2: Q^T * b (computed directly via Householder reflections) --- Vector qtb = b; T* qp = qtb.data(); for (std::size_t step = 0; step < k; ++step) { if (tau_vec[step] == T(0)) continue; T d = qp[step]; for (std::size_t i = step + 1; i < m; ++i) d += Rp[i * n + step] * qp[i]; d *= tau_vec[step]; qp[step] -= d; for (std::size_t i = step + 1; i < m; ++i) qp[i] -= Rp[i * n + step] * d; } if (r == 0) { Vector x(n, T(0)); return x; } if (r == n) { // Full rank: no RZ needed, T^{-1} * qtb + inverse permutation Vector y(n, T(0)); for (std::size_t ii = 0; ii < r; ++ii) { std::size_t i = r - 1 - ii; y[i] = qtb[i]; for (std::size_t j = i + 1; j < r; ++j) y[i] -= Rp[i * n + j] * y[j]; y[i] /= Rp[i * n + i]; } Vector x(n, T(0)); for (std::size_t j = 0; j < n; ++j) x[perm[j]] = y[j]; return x; } // --- Phase 3: RZ decomposition + T^{-1} + Z application (keeping the Householder vectors) --- // Copy the upper triangular part of R[:r, :] into Rwork // (Rp's lower triangle holds the QR Householder vectors, so use a separate buffer) Matrix Rwork(r, n, T(0)); T* Rwp = Rwork.data(); for (std::size_t i = 0; i < r; ++i) for (std::size_t j = i; j < n; ++j) Rwp[i * n + j] = Rp[i * n + j]; // Save the RZ Householder vectors and tau std::vector> rz_vtail(r); std::vector rz_tau(r, T(0)); const std::size_t tail = n - r; for (std::size_t kk = 0; kk < r; ++kk) { std::size_t kv = r - 1 - kk; T x0 = Rwp[kv * n + kv]; T sigma_sq = x0 * x0; for (std::size_t j = r; j < n; ++j) sigma_sq += Rwp[kv * n + j] * Rwp[kv * n + j]; T sig = std::sqrt(sigma_sq); if (sig < std::numeric_limits::epsilon()) continue; T alph = (x0 >= T(0)) ? -sig : sig; T bet = x0 - alph; if (std::abs(bet) < std::numeric_limits::epsilon()) continue; T inv_b = T(1) / bet; rz_vtail[kv].resize(tail); for (std::size_t j = 0; j < tail; ++j) rz_vtail[kv][j] = Rwp[kv * n + r + j] * inv_b; T tau_rz = -bet / alph; rz_tau[kv] = tau_rz; Rwp[kv * n + kv] = alph; for (std::size_t j = r; j < n; ++j) Rwp[kv * n + j] = T(0); // Apply the reflector to the other rows of Rwork from the right for (std::size_t i = 0; i < kv; ++i) { T d = Rwp[i * n + kv]; for (std::size_t j = 0; j < tail; ++j) d += rz_vtail[kv][j] * Rwp[i * n + r + j]; d *= tau_rz; Rwp[i * n + kv] -= d; for (std::size_t j = 0; j < tail; ++j) Rwp[i * n + r + j] -= rz_vtail[kv][j] * d; } } // T^{-1} * qtb[:r] Vector w(n, T(0)); for (std::size_t ii = 0; ii < r; ++ii) { std::size_t i = r - 1 - ii; w[i] = qtb[i]; for (std::size_t j = i + 1; j < r; ++j) w[i] -= Rwp[i * n + j] * w[j]; w[i] /= Rwp[i * n + i]; } // Compute Z * w via Householder reflections (applied in reverse order) // Z = H_0 * H_1 * ... * H_{r-1} // Z * w = H_0(H_1(...(H_{r-1} * w)...)) // Each H_k acts on position k and r:n for (std::size_t kv = 0; kv < r; ++kv) { if (rz_tau[kv] == T(0)) continue; T d = w[kv]; for (std::size_t j = 0; j < tail; ++j) d += rz_vtail[kv][j] * w[r + j]; d *= rz_tau[kv]; w[kv] -= d; for (std::size_t j = 0; j < tail; ++j) w[r + j] -= rz_vtail[kv][j] * d; } // Inverse of the column permutation Vector x(n, T(0)); for (std::size_t j = 0; j < n; ++j) x[perm[j]] = w[j]; return x; } // ==================================================================== // RealQZ — generalized Schur (QZ) decomposition // ==================================================================== /** * @brief Result of the QZ decomposition * * Q^T * A * Z = S (quasi-upper triangular) * Q^T * B * Z = T (upper triangular) * Generalized eigenvalues = diag(S) / diag(T) */ template struct QZResult { Matrix S; ///< quasi-upper-triangular (real Schur form) Matrix TT; ///< upper triangular Matrix Q; ///< left orthogonal matrix Matrix Z; ///< right orthogonal matrix /// generalized eigenvalues (alpha/beta pairs) [[nodiscard]] std::vector, T>> eigenvalues() const { const auto n = S.rows(); std::vector, T>> eigs; eigs.reserve(n); for (std::size_t i = 0; i < n; ) { if (i + 1 < n && std::abs(S(i + 1, i)) > std::numeric_limits::epsilon() * 100 * (std::abs(S(i, i)) + std::abs(S(i + 1, i + 1)))) { // 2×2 block → complex eigenvalue pair T a = S(i, i), b = S(i, i + 1), c = S(i + 1, i), d = S(i + 1, i + 1); T tr = a + d, det = a * d - b * c; T disc = tr * tr - T(4) * det; T beta1 = TT(i, i), beta2 = TT(i + 1, i + 1); if (disc < T(0)) { T re = tr / T(2); T im = std::sqrt(-disc) / T(2); eigs.push_back({ {re, im}, beta1 }); eigs.push_back({ {re, -im}, beta2 }); } else { T sq = std::sqrt(disc); eigs.push_back({ {(tr + sq) / T(2), T(0)}, beta1 }); eigs.push_back({ {(tr - sq) / T(2), T(0)}, beta2 }); } i += 2; } else { eigs.push_back({ {S(i, i), T(0)}, TT(i, i) }); ++i; } } return eigs; } }; /** * @brief QZ decomposition (generalized Schur decomposition) * * Implementation: Hessenberg-triangularization + QZ iteration (single shift) * Q^T * A * Z = S, Q^T * B * Z = T */ template requires sangi::BaseMatrixLike && sangi::BaseMatrixLike && std::same_as, sangi::element_t> QZResult> real_qz(const MA& A, const MB& B) { using T = sangi::element_t; SANGI_STATIC_ASSERT_SQUARE(MA); SANGI_STATIC_ASSERT_SQUARE(MB); const auto n = A.rows(); if (n != A.cols() || n != B.rows() || n != B.cols()) { assert(false && "DimensionError: real_qz: A and B must be square with same size"); throw DimensionError("real_qz: A and B must be square with same size"); } if (n == 0) return { Matrix(), Matrix(), Matrix(), Matrix() }; if (n == 1) return { A, B, Matrix({{T(1)}}), Matrix({{T(1)}}) }; Matrix H = A; Matrix R = B; Matrix Q(n, n, T(0)), Z(n, n, T(0)); for (std::size_t i = 0; i < n; ++i) { Q(i, i) = T(1); Z(i, i) = T(1); } // Givens rotation helper: a,b → (c,s,r) s.t. [c s; -s c]^T [a;b] = [r;0] auto givens = [](T a, T b) -> std::tuple { T r = std::sqrt(a*a + b*b); if (r < std::numeric_limits::epsilon()) return {T(1), T(0), T(0)}; return {a/r, b/r, r}; }; // Left Givens: apply to rows i,j auto apply_left = [&](T cs, T sn, std::size_t i, std::size_t j, Matrix& M, std::size_t col_start, std::size_t col_end) { for (std::size_t c = col_start; c < col_end; ++c) { T t1 = M(i, c), t2 = M(j, c); M(i, c) = cs * t1 + sn * t2; M(j, c) = -sn * t1 + cs * t2; } }; // Right Givens: apply to columns i,j auto apply_right = [&](T cs, T sn, std::size_t i, std::size_t j, Matrix& M, std::size_t row_start, std::size_t row_end) { for (std::size_t r = row_start; r < row_end; ++r) { T t1 = M(r, i), t2 = M(r, j); M(r, i) = cs * t1 + sn * t2; M(r, j) = -sn * t1 + cs * t2; } }; // Step 1: upper-triangularize B (left Givens) for (std::size_t k = 0; k + 1 < n; ++k) { auto [cs, sn, r] = givens(R(k, k), R(k+1, k)); apply_left(cs, sn, k, k+1, R, k, n); apply_left(cs, sn, k, k+1, H, 0, n); apply_right(cs, sn, k, k+1, Q, 0, n); } // Step 2: reduce H to upper Hessenberg (right Givens + left Givens to re-triangularize R) for (std::size_t k = 0; k + 2 < n; ++k) { for (std::size_t i = n - 1; i > k + 1; --i) { // Zero out H(i, k): right Givens (columns i-1, i) auto [cs, sn, r] = givens(H(i-1, k), H(i, k)); apply_right(cs, sn, i-1, i, H, 0, n); apply_right(cs, sn, i-1, i, R, 0, n); apply_right(cs, sn, i-1, i, Z, 0, n); // Zero out R(i, i-1): left Givens (rows i-1, i) if (std::abs(R(i, i-1)) > std::numeric_limits::epsilon()) { auto [c2, s2, r2] = givens(R(i-1, i-1), R(i, i-1)); apply_left(c2, s2, i-1, i, R, i-1, n); apply_left(c2, s2, i-1, i, H, 0, n); apply_right(c2, s2, i-1, i, Q, 0, n); } } } // Step 3: QZ iteration const std::size_t max_qz_iter = 100 * n; std::size_t p = n; for (std::size_t iter = 0; iter < max_qz_iter && p > 1; ++iter) { // deflation check T tol = std::numeric_limits::epsilon() * (std::abs(H(p-2, p-2)) + std::abs(H(p-1, p-1))); if (std::abs(H(p-1, p-2)) <= std::max(tol, std::numeric_limits::min())) { H(p-1, p-2) = T(0); --p; continue; } // Wilkinson shift: the one closer to the generalized eigenvalue a22/b22 T b22_inv = (std::abs(R(p-1, p-1)) > std::numeric_limits::epsilon()) ? T(1) / R(p-1, p-1) : T(0); T shift = H(p-1, p-1) * b22_inv; // QZ step: start the Givens from the leading element of (H - shift * R) for (std::size_t k = 0; k + 1 < p; ++k) { T x, y; if (k == 0) { x = H(0, 0) - shift * R(0, 0); y = H(1, 0); } else { x = H(k, k-1); y = H(k+1, k-1); } auto [cs, sn, rv] = givens(x, y); // Left Givens (rows k, k+1) apply_left(cs, sn, k, k+1, H, 0, n); apply_left(cs, sn, k, k+1, R, 0, n); apply_right(cs, sn, k, k+1, Q, 0, n); // Zero out R(k+1, k): right Givens (columns k+1, k) auto [c2, s2, r2] = givens(R(k+1, k+1), R(k+1, k)); apply_right(c2, s2, k+1, k, R, 0, n); apply_right(c2, s2, k+1, k, H, 0, n); apply_right(c2, s2, k+1, k, Z, 0, n); } } return { std::move(H), std::move(R), std::move(Q), std::move(Z) }; } } // namespace algorithms } // namespace sangi #endif // SANGI_DECOMPOSITION_HPP