// Copyright (C) 2026 Kiyotsugu Arai // SPDX-License-Identifier: LGPL-3.0-or-later // decomposition.hpp // // 行列分解アルゴリズム // // このファイルでは、様々な行列分解アルゴリズムを実装します。 // LU分解、Cholesky分解、QR分解、SVD分解などが含まれます。 // // 主な機能: // - LU分解 // - Cholesky分解 // - QR分解（ハウスホルダー変換による） // - 特異値分解（SVD） // - LDL分解 // - 分解を使った行列計算（逆行列、行列式など） #ifndef CALX_DECOMPOSITION_HPP #define CALX_DECOMPOSITION_HPP #include #include #include #include #include #include #include #include #ifdef CALX_SVD_PHASE_TIMING #include #include #endif #include #include #include #include #include #include #include namespace calx { namespace algorithms { /** * @brief 行列が対称かどうかをチェック * @tparam T 要素の型 * @param a チェックする行列 * @param tolerance 許容誤差 * @return 対称行列ならtrue、そうでなければfalse */ template bool is_symmetric(const Matrix& a, decltype(numeric_traits::epsilon()) tolerance = numeric_traits::epsilon() * 10) { if (a.rows() != a.cols()) { return false; } for (typename Matrix::size_type i = 0; i < a.rows(); ++i) { for (typename Matrix::size_type j = i + 1; j < a.cols(); ++j) { if constexpr (numeric_traits::is_complex) { // Complex: エルミート性チェック a(i,j) == conj(a(j,i)) if (std::abs(a(i, j) - calx::conj(a(j, i))) > tolerance) { return false; } } else { if (std::abs(a(i, j) - a(j, i)) > tolerance) { return false; } } } } return true; } /** * @brief 行列が正定値かどうかをチェック * @tparam T 要素の型 * @param a チェックする行列 * @return 正定値行列ならtrue (Complex の場合はエルミート正定値) */ template bool is_positive_definite(const Matrix& a) { if (!is_symmetric(a)) { return false; } // 対角要素がすべて正かチェック（必要条件） for (typename Matrix::size_type i = 0; i < a.rows(); ++i) { if constexpr (numeric_traits::is_complex) { // エルミート行列の対角要素は実数、正であるべき if (a(i, i).real() <= 0) { return false; } } else { if (a(i, i) <= numeric_traits::zero()) { return false; } } } // Cholesky分解が可能かチェック（十分条件） try { cholesky_decomposition(a); return true; } catch (const MathError&) { return false; } } // ピボット選択戦略 enum class PivotStrategy { None, // ピボット選択なし (対角要素をそのまま使用) Partial, // 部分ピボット選択 (列内で絶対値最大, Rational は高さ最小) Full // 完全ピボット選択 (残り部分行列全体から選択) }; // ── ピボット選択の内部ヘルパー ── // numeric_traits::pivotBetter を使い、型に応じた最適な基準で選択する // (浮動小数点: abs 最大, Rational: 高さ最小) template struct PivotSelector { // 部分ピボット: 列 k の行 k..n-1 から最良を選択 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; } // 完全ピボット: 部分行列 (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 LU分解を計算 * @tparam T 要素の型 * @param a 分解する行列 * @param pivot ピボット戦略 (既定: Partial) * @return LU行列とピボット情報のペア * LU行列は、L（下三角部分）とU（上三角部分）を一つの行列に格納 * ピボット情報は行交換の記録 * 完全ピボットの場合は列交換も記録 (n個の行+n個の列) */ template std::pair, std::vector::size_type>> lu_decomposition(const Matrix& a, PivotStrategy pivot = PivotStrategy::Partial) { if (a.rows() != a.cols()) { throw DimensionError("lu_decomposition: matrix must be square"); } const auto n = a.rows(); #if CALX_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { Matrix lu = a; 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"); // LAPACKE ipiv は 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 = a; std::vector::size_type> pivots(n); // ブロック LU 分解 (LAPACK DGETRF 方式) — float/double で 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 符号反転バッファ (ループ全体で再利用) 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: パネル分解 (列 jb..jend-1) for (std::size_t j = jb; j < jend; ++j) { // 部分ピボット選択 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; // 行交換 (全行) 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]); } // 特異チェック if (std::abs(data[j * n + j]) <= std::numeric_limits::epsilon()) throw MathError("lu_decomposition: singular matrix detected"); // L 列スケーリング 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; // パネル内ランク 1 更新 (列 j+1..jend-1 のみ, 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 — L11 * U12 = A12 を前進代入で解く // L11 は lu(jb..jend-1, jb..jend-1) の単位下三角部分 // A12/U12 は lu(jb..jend-1, jend..n-1) — 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 行列更新 A22 -= L21 * U12 // L21 を符号反転してバッファにコピー、U12 は直接参照 { 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 ベース直接累積 DefaultComputePolicy::gemm( L21_buf.data(), &data[jb * n + jend], &data[jend * n + jend], remaining, remaining, nb, nb, n, n); } } return { lu, pivots }; } } // スカラー LU 分解 (小行列 or 非浮動小数点型) // 完全ピボット時は列交換も記録: pivots[0..n-1] = 行, pivots[n..2n-1] = 列 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) { // ピボット選択 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 (そのまま) pivots[k] = pivot_row; // 行交換 if (pivot_row != k) { for (typename Matrix::size_type j = 0; j < n; ++j) std::swap(lu(k, j), lu(pivot_row, j)); } // 列交換 (完全ピボットのみ) 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); } } } // 完全ピボット: 列交換情報を 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 LU分解を使って連立方程式を解く * @tparam T 要素の型 * @param a 係数行列 * @param b 右辺ベクトル * @param pivot ピボット戦略 (既定: Partial) * @return 解ベクトル */ template Vector lu_solve(const Matrix& a, const Vector& b, PivotStrategy pivot = PivotStrategy::Partial) { if (a.rows() != a.cols()) { throw DimensionError("lu_solve: matrix must be square"); } if (a.rows() != b.size()) { throw DimensionError("lu_solve: matrix and vector dimensions mismatch"); } const auto n = a.rows(); #if CALX_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分解 auto [lu, pivots] = lu_decomposition(a, pivot); const bool fullPivot = (pivot == PivotStrategy::Full); // 右辺ベクトルの準備 Vector x = b; // 行ピボット置換 for (std::size_t i = 0; i < n; ++i) { if (pivots[i] != i) { std::swap(x[i], x[pivots[i]]); } } // float/double: raw ポインタで前進・後退代入 if constexpr (std::is_same_v || std::is_same_v) { const T* __restrict ld = lu.data(); T* __restrict xd = x.data(); // 前進代入 (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; } // 後退代入 (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]; } // 完全ピボット: 列交換の逆置換 if (fullPivot) { Vector tmp = x; for (std::size_t i = 0; i < n; ++i) x[pivots[n + i]] = tmp[i]; } return x; } // 汎用型: 前進代入 (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]; } } // 後退代入 (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); } // 完全ピボット: 列交換の逆置換 if (fullPivot) { Vector tmp = x; for (std::size_t i = 0; i < n; ++i) x[pivots[n + i]] = tmp[i]; } return x; } /** * @brief ガウスの消去法で連立方程式を解く (拡大係数行列ベース) * * LU 分解を経由せず、拡大係数行列 [A|b] を直接前進消去+後退代入する。 * Rational では高さ最小ピボットにより係数膨張を抑制する。 * * @tparam T 要素の型 (double, Float, Rational 等) * @param a 係数行列 (n×n) * @param b 右辺ベクトル (n) * @param pivot ピボット戦略 (既定: Partial) * @return 解ベクトル */ template Vector gaussian_elimination(const Matrix& a, const Vector& b, PivotStrategy pivot = PivotStrategy::Partial) { if (a.rows() != a.cols()) throw DimensionError("gaussian_elimination: matrix must be square"); if (a.rows() != b.size()) throw DimensionError("gaussian_elimination: matrix and vector dimensions mismatch"); const auto n = a.rows(); // 拡大係数行列 [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]; } // 完全ピボット用の列順序追跡 std::vector colOrder(n); for (std::size_t i = 0; i < n; ++i) colOrder[i] = i; // 前進消去 for (std::size_t k = 0; k < n; ++k) { // ピボット選択 std::size_t pivotRow = k, pivotCol = k; if (pivot == PivotStrategy::Partial) { // 部分ピボット: 列 k の行 k..n-1 から選択 #ifdef CALX_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) { // 完全ピボット: 部分行列 (k..n-1, k..n-1) から選択 #ifdef CALX_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; } } } } } // 行交換 if (pivotRow != k) { for (std::size_t j = 0; j <= n; ++j) std::swap(aug(k, j), aug(pivotRow, j)); } // 列交換 (完全ピボットのみ, b列は交換しない) 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]); } // 特異チェック if (numeric_traits::abs(aug(k, k)) <= numeric_traits::epsilon()) throw MathError("gaussian_elimination: singular matrix detected"); // 消去 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); } } // 後退代入 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); } // 完全ピボット: 列交換の逆置換 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 LU分解を使って逆行列を計算 * @tparam T 要素の型 * @param a 元の行列 * @param pivot ピボット戦略 (既定: Partial) * @return 逆行列 */ template Matrix lu_inverse(const Matrix& a, PivotStrategy pivot = PivotStrategy::Partial) { if (a.rows() != a.cols()) { throw DimensionError("lu_inverse: matrix must be square"); } const auto n = a.rows(); // 単位行列の列ベクトルごとに方程式を解く Matrix inverse(n, n); Vector e(n, T(0)); Vector x(n); for (typename Matrix::size_type j = 0; j < n; ++j) { // 単位行列の列ベクトルを設定 for (typename Matrix::size_type i = 0; i < n; ++i) { e[i] = (i == j) ? T(1) : T(0); } // 方程式を解く x = lu_solve(a, e, pivot); // 結果を逆行列に格納 for (typename Matrix::size_type i = 0; i < n; ++i) { inverse(i, j) = x[i]; } } return inverse; } /** * @brief LU分解を使って行列式を計算 * @tparam T 要素の型 * @param a 元の行列 * @param pivot ピボット戦略 (既定: Partial) * @return 行列式の値 */ template T lu_determinant(const Matrix& a, PivotStrategy pivot = PivotStrategy::Partial) { if (a.rows() != a.cols()) { throw DimensionError("lu_determinant: matrix must be square"); } const auto n = a.rows(); // LU分解 auto [lu, pivots] = lu_decomposition(a, pivot); // 行列式を計算 T det = T(1); int sign = 1; // ピボット交換による符号変化を計算 for (typename Matrix::size_type i = 0; i < n; ++i) { if (pivots[i] != i) { sign = -sign; } } // 対角成分の積 for (typename Matrix::size_type i = 0; i < n; ++i) { det *= lu(i, i); } return det * T(sign); } /** * @brief Bareiss アルゴリズムで行列式を計算 (整数型向け、除算なし) * * Sylvester の恒等式により、消去過程の除算が常に割り切れることを利用し、 * 整数型の行列式を中間結果も整数のまま O(n³) で計算する。 * 浮動小数点型でも使用可能だが、lu_determinant の方が効率的。 * * @tparam T 要素の型 (Int, int, long long 等) * @param a 正方行列 * @return 行列式 det(A) */ template T bareiss_determinant(const Matrix& a) { if (a.rows() != a.cols()) 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) { // 部分ピボット選択 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 Cholesky分解を計算 * @tparam T 要素の型 * @param a 分解する行列（対称正定値行列） * @return Cholesky分解のL行列（下三角行列） */ template Matrix cholesky_decomposition(const Matrix& a) { if (a.rows() != a.cols()) { throw DimensionError("cholesky_decomposition: matrix must be square"); } const auto n = a.rows(); #if CALX_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { Matrix L = a; 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"); // 上三角部分をゼロクリア (LAPACKE は下三角のみ書き込むが上三角は元のまま) 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 ブロック Cholesky — is_symmetric チェック不要 // (非 PD は sqrt で検出、対称性は呼び出し側の責任) if constexpr (std::is_same_v || std::is_same_v) { const std::size_t NB = (n < 256) ? 32 : 64; if (n >= 32) { // A を丸ごとコピー (上三角は読まない) Matrix L = a; T* __restrict data = L.data(); // バッファを事前確保 (ループ全体で再利用) // trsm_cols: TRSM 用列バッファ (列が連続) std::vector trsm_cols(n * NB); // L_CB_data: SYRK 用行優先バッファ (行が連続, 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: 対角ブロック Cholesky (nb×nb) using PT = PacketTraits; constexpr std::size_t PS = PT::size; for (std::size_t j = jb; j < jend; ++j) { // 対角要素: 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]; // 列 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; // k ループで dot product を SIMD 化 // 各行 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 — 列バッファで axpy_simd 適用 { 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; } // TRSM 結果を L に書き戻しつつ、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 // 下三角のみ直接計算 (転置バッファ不要, 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; // SYRK タイルは常に 64 (NB とは独立) for (std::size_t ib = 0; ib < m; ib += TB) { const std::size_t i_end = (std::min)(ib + TB, m); // 非対角タイル (jb2 < ib): 2×4 マイクロカーネル for (std::size_t jb2 = 0; jb2 < ib; jb2 += TB) { const std::size_t j_end = (std::min)(jb2 + TB, m); // 2行ずつ処理: j 方向のロードを共有 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); } } // 端数行 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); } } } // 対角タイル: 下三角のみ 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 小行列 (n < 32): スカラー Cholesky (in-place) { Matrix L = a; 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; } } // 汎用型: 対称/エルミート性チェック + スカラー 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) { // エルミート: a(i,j) == conj(a(j,i)) if (std::abs(a(i, j) - calx::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 -= calx::conj(L(i, k)) * L(i, k); else sum -= L(i, k) * L(i, k); } if constexpr (numeric_traits::is_complex) { // エルミート正定値なら対角要素の sum は実正 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) * calx::conj(L(i, k)); else sum -= L(j, k) * L(i, k); } L(j, i) = sum * inv_diag; } } return L; } /** * @brief Cholesky分解を使って連立方程式を解く * @tparam T 要素の型 * @param a 係数行列（対称正定値行列） * @param b 右辺ベクトル * @return 解ベクトル */ template Vector cholesky_solve(const Matrix& a, const Vector& b) { if (a.rows() != a.cols()) { throw DimensionError("cholesky_solve: matrix must be square"); } if (a.rows() != b.size()) { throw DimensionError("cholesky_solve: matrix and vector dimensions mismatch"); } const auto n = a.rows(); #if CALX_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分解 Matrix L = cholesky_decomposition(a); // float/double: raw ポインタで前進・後退代入 if constexpr (std::is_same_v || std::is_same_v) { const T* __restrict ld = L.data(); const T* __restrict bd = b.data(); // 前進代入 (Ly = b) — 行方向読み (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]; } // 後退代入 (L^T x = y) — 列方向 axpy (行読みで contiguous) 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; } // 汎用型: 前進代入 (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); } // 後退代入 (L^H x = y) ※実数の場合 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 -= calx::conj(L(j, i)) * x[j]; else sum -= L(j, i) * x[j]; } // L(i,i) は Cholesky により常に実正なので conj 不要 x[i] = sum / L(i, i); } return x; } // ==================================================================== // Householder 変換 単独 API // ==================================================================== /** * @brief Householder ベクトルと係数を計算 * * 入力ベクトル x に対し、(I - beta * v * v^T) * x = alpha * e1 * を満たす v, beta, alpha を返す。 * * @param x 入力ベクトル (サイズ >= 1) * @return {v, beta, alpha} — v: Householder ベクトル, beta: 係数, alpha: 結果の第1成分 */ template std::tuple, T, T> householder_vector(const Vector& x) { const auto n = x.size(); if (n == 0) { throw DimensionError("householder_vector: empty vector"); } const T zero = numeric_traits::zero(); const T one = numeric_traits::one(); const T two = one + one; // 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 += calx::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 は e1 方向 return {v, zero, x[0]}; } // ||x|| を計算 auto x_norm_real = std::sqrt(std::abs(calx::conj(x[0]) * x[0] + sigma)); T x_norm = T(x_norm_real); // 符号: 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 = calx::conj(v[0]) * v[0] + sigma; T beta = two / v_norm_sq; return {v, beta, alpha}; } else { if (sigma == zero && x[0] >= zero) { // x は既に正の e1 方向 → 恒等変換 return {v, zero, x[0]}; } if (sigma == zero && x[0] < zero) { // x = -alpha * e1 → 符号反転 v[0] = zero; return {v, two, -x[0]}; } T x_norm = static_cast(std::sqrt( static_cast(x[0] * x[0] + sigma))); // 数値安定性のため符号を選択 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 Householder 反射を左から適用: A ← (I - beta * v * v^H) * A * * 行列 A の指定行範囲 [row_start, row_start+v.size()) に適用。 * @param v Householder ベクトル * @param beta 係数 * @param A 対象行列 (in-place 更新) * @param row_start 適用開始行 (デフォルト 0) * @param col_start 適用開始列 (デフォルト 0) */ template void apply_householder_left( const Vector& v, T beta, Matrix& A, std::size_t row_start = 0, std::size_t col_start = 0) { const auto vn = v.size(); const auto cols = A.cols(); const T zero = numeric_traits::zero(); // 各列 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 += calx::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 Householder 反射を右から適用: A ← A * (I - beta * v * v^H) * * 行列 A の指定列範囲 [col_start, col_start+v.size()) に適用。 */ template void apply_householder_right( Matrix& A, const Vector& v, T beta, std::size_t row_start = 0, std::size_t col_start = 0) { const auto vn = v.size(); const auto rows = A.rows(); const T zero = numeric_traits::zero(); // 各行 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 * calx::conj(v[j]); else A(i, col_start + j) -= factor * v[j]; } } } /** * @brief Householder QR 分解による直接法ソルバー: Ax = b * * m×n 行列 A (m >= n) に対し、最小二乗解を返す。 * m == n の場合は正則系の直接解。 * * @param A 係数行列 (m×n, m >= n) * @param b 右辺ベクトル (サイズ m) * @return x 解ベクトル (サイズ n) */ template Vector householder_solve(const Matrix& A, const Vector& b) { const auto m = A.rows(); const auto n = A.cols(); if (m < n) { throw DimensionError("householder_solve: requires m >= n (overdetermined or square)"); } if (b.size() != m) { throw DimensionError("householder_solve: b size must equal A rows"); } // R = A のコピー, d = b のコピー (Q^T b を直接計算) Matrix R = A; Vector d = b; auto min_mn = std::min(m, n); // ブロック Householder QR (WY 表現) — float/double で 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(); // Householder 係数 tau とバッファを事前確保 (ループ全体で再利用) 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; const std::size_t panel_rows = m - jb; // Phase 1: パネル分解 (列 jb..jend-1) // 各列 j でスカラー Householder, パネル内残列に SIMD 適用 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; // 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; // パネル内残列に Householder 適用 (SIMD) // v = [1, rdata[j+1..m-1, j]] // PS 列同時に dot + axpy を処理 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 (neg で 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))); } } // スカラー残端 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]; } // d にも適用 { 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: WY ブロック表現で trailing 列を一括更新 // Q_block = I - V * T * V^T, T は nb×nb 上三角 // // (b) V を行優先バッファに展開 (T ファクタ構築で使うため先に実行) // VT_rm = V^T in row-major (nb × pr, lda=pr) — gemm step (c) 用 // V_rm = V in row-major (pr × nb, lda=nb) — 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) T ファクタ構築 (VT_rm の連続バッファを使用) 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 // VT_rm を使用: v_kk は VT_rm[kk*pr + kk:], v_jj は VT_rm[jj*pr + jj:] const std::size_t len_j = m - j; for (std::size_t kk = 0; kk < jj; ++kk) { // 連続メモリでの内積 (VT_rm[kk, jj:jj+len_j] と 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 (下三角 nb×nb × nb×trailing) // Q^T = I - V * T^T * V^T なので T^T を適用 // インプレース下三角 trmv: 下から上に計算 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 // W を符号反転して 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) // → A_trail (pr × trailing, ldc=n) に累積加算 DefaultComputePolicy::gemm( V_rm.data(), W_buf.data(), &rdata[jb * n + jend], pr, trailing, nb, nb, trailing, n); } // 後退代入 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; } } // スカラー Householder QR (小行列 or 非浮動小数点型) for (std::size_t k = 0; k < min_mn; ++k) { // 列 k の k 行目以下から Householder ベクトルを構築 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; // R に左から適用 apply_householder_left(v, beta, R, k, k); // d にも適用: 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]; } } // 特異判定用: R の対角の最大絶対値 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(); // 後退代入: 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) 単独 API // ==================================================================== /** * @brief 二重対角化の結果を格納する構造体 * * A = U * B * V^T * B は上二重対角行列 (対角 d, 超対角 e) */ template struct BidiagonalResult { Matrix U; ///< 左直交行列 (m×m) Vector d; ///< 対角要素 (サイズ min(m,n)) Vector e; ///< 超対角要素 (サイズ min(m,n)-1, e[k] = B(k, k+1)) Matrix V; ///< 右直交行列 (n×n) }; /** * @brief Householder 変換による二重対角化 * * m×n 行列 A (m >= n) を上二重対角行列 B に変換する。 * A = U * B * V^T を満たす U (m×m), B (上二重対角), V (n×n) を返す。 * * m < n の場合は A^T を二重対角化してから転置で戻す。 * * @param A 入力行列 * @return BidiagonalResult {U, d, e, V} */ template BidiagonalResult bidiagonalize(const Matrix& A) { const size_t m = A.rows(); const size_t n = A.cols(); if (m == 0 || n == 0) { throw DimensionError("bidiagonalize: empty matrix"); } if (m < n) { throw DimensionError( "bidiagonalize: requires m >= n (use A^T for wide matrices)"); } // 以下 m >= n const size_t min_mn = n; // 作業用コピー Matrix W(A); // U (m×m), V (n×n) を単位行列で初期化 Matrix U = Matrix::identity(m); Matrix V = Matrix::identity(n); const T zero = numeric_traits::zero(); const T one = numeric_traits::one(); // 対角・超対角要素 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) { // --- 左 Householder: W(k:m, k) の k+1 行以降をゼロ化 --- { 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) // 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 += calx::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; } // 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) * calx::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); } } // --- 右 Householder: 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)); // 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 += calx::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; } // 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 += calx::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); } } // 結果を Vector に変換 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 二重対角行列を陽に構築するユーティリティ * * @param d 対角要素 (サイズ p) * @param e 超対角要素 (サイズ p-1) * @return Matrix 上二重対角行列 (p×p) */ template Matrix build_bidiagonal_matrix(const Vector& d, const Vector& e) { const size_t p = d.size(); if (p > 1 && e.size() != p - 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 分解 // ==================================================================== /** * @brief QR分解を計算（ハウスホルダー反射による） * @tparam T 要素の型 * @param a 分解する行列 * @return QとRのペア */ template std::pair, Matrix> qr_decomposition(const Matrix& a) { const auto m = a.rows(); const auto n = a.cols(); #if CALX_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { const auto k = std::min(m, n); Matrix R = a; std::vector tau(k); lapack_int info; // QR 分解 (Householder 形式) 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"); // Q を陽に生成: m×m 行列に Householder ベクトルをコピー 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"); // 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 = a; const T zero = numeric_traits::zero(); const T one = numeric_traits::one(); const T two = one + one; // 単位行列でQを初期化 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; } } // ハウスホルダー反射による計算 for (typename Matrix::size_type k = 0; k < std::min(m, n); ++k) { // ハウスホルダーベクトルの計算 Vector x(m - k); for (typename Matrix::size_type i = 0; i < m - k; ++i) { x[i] = R(k + i, k); } // ノルム計算 (Complex 対応: dot は conj(x)*x を返すので実部を取る) auto dot_xx = dot(x, x); auto x_norm_real = std::sqrt(std::abs(dot_xx)); T x_norm = T(x_norm_real); // ゼロベクトルのチェック if (x_norm_real < numeric_traits::epsilon()) { continue; } // 符号付きノルム (Complex の場合は 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; } // 最初の要素だけ修正（x[0] - alpha） x[0] -= alpha; // ハウスホルダーベクトルのノルムの二乗 auto x_norm_squared = dot(x, x); // 数値的安定性のためのチェック if (std::abs(x_norm_squared) < numeric_traits::epsilon()) { continue; } // ハウスホルダー行列の適用 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 += calx::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]; } } // 同様に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 += calx::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は列ベクトルで構成されており、実際には転置が必要 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 QR分解を使って連立方程式を解く * @tparam T 要素の型 * @param a 係数行列 * @param b 右辺ベクトル * @return 解ベクトル */ template Vector qr_solve(const Matrix& a, const Vector& b) { if (a.rows() != b.size()) { throw DimensionError("qr_solve: matrix and vector dimensions mismatch"); } // float/double: householder_solve を使用 (Q を明示的に形成しない) 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分解 auto [Q, R] = qr_decomposition(a); // 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) } } // 後退代入 (Rx = Q^T * b) Vector x(n, T(0)); // 最小二乗解（過剰決定系の場合） 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]; } // ゼロ対角成分のチェック if (std::abs(R(i, i)) < std::numeric_limits::epsilon()) { // 非正則行列の場合 // 特異値分解（SVD）に切り替えることも考慮 throw MathError("qr_solve: matrix is singular or nearly singular"); } x[i] = sum / R(i, i); } return x; } /** * @brief 列ピボッティング付きQR分解（Householder反射による） * * AP = QR の形に分解する。 * 各ステップで残余列のノルムが最大の列を選択し、列交換を行う。 * これにより R の対角要素は |R(0,0)| >= |R(1,1)| >= ... となり、 * 数値的ランク判定が可能になる。 * * @tparam T 要素の型 * @param a 分解する m×n 行列 * @return (Q, R, perm) のタプル * - Q: m×m 直交行列 * - R: m×n 上三角行列 * - perm: 列置換ベクトル（perm[j] = 元の列インデックス） * * 置換の意味: A の perm[j] 番目の列 = Q*R の j 番目の列 * すなわち A * P = Q * R （P は置換行列） */ template std::tuple, Matrix, std::vector::size_type>> qr_col_pivoting(const Matrix& 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; // Q を単位行列で初期化 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); // 置換ベクトル（恒等置換で初期化） std::vector perm(n); for (size_type j = 0; j < n; ++j) perm[j] = j; // 各列の残余ノルムの二乗を事前計算 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) { // 最大ノルム列を探索（step 以降の列から） 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; } } // 列交換 if (max_col != step) { // R の列を交換 for (size_type i = 0; i < m; ++i) { std::swap(R(i, step), R(i, max_col)); } // ノルムを交換 std::swap(col_norms_sq[step], col_norms_sq[max_col]); // 置換を記録 std::swap(perm[step], perm[max_col]); } // Householder ベクトルの計算 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; } // R に Householder 変換を適用 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]; } } // Q に Householder 変換を適用 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]; } } // 残余ノルムの更新（step+1 以降の列） 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); } } // 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 列ピボッティング付きQR分解の数値的ランクを計算 * * R の対角要素から数値的ランクを判定する。 * |R(i,i)| > threshold のとき、i番目の列は独立とみなす。 * * @param R qr_col_pivoting で得た上三角行列 * @param threshold 閾値（デフォルト: eps * max(m,n) * |R(0,0)|） * @return 数値的ランク */ 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)) { // デフォルト閾値: 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 列ピボッティング付きQR分解を使って連立方程式を解く * * A*x = b を列ピボッティング付きQR分解で解く。 * ランク落ちの場合はランク分の変数のみを求め、残りは0とする。 * * @param a 係数行列 (m×n) * @param b 右辺ベクトル (m) * @return 解ベクトル (n) */ template Vector qr_col_pivoting_solve(const Matrix& a, const Vector& b) { if (a.rows() != b.size()) { 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); // 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]; } } // 数値的ランクを判定 auto r = qr_rank(R); // 後退代入（ランク 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); } // 置換を逆適用して元の列順に戻す Vector x(n, T(0)); for (typename Matrix::size_type j = 0; j < n; ++j) { x[perm[j]] = y[j]; } return x; } // ==================================================================== // 三重対角化 (Tridiagonalization) 単独 API // ==================================================================== /** * @brief 三重対角化の結果を格納する構造体 * * 対称行列 A = Q * T * Q^T * T は三重対角行列 (対角 d, 副対角 e) */ template struct TridiagonalResult { Matrix Q; ///< 直交行列 (n×n) Vector d; ///< 対角要素 (サイズ n) Vector e; ///< 副対角要素 (サイズ n-1, e[k] = T(k, k+1) = T(k+1, k)) }; /** * @brief Householder 変換による対称行列の三重対角化 * * n×n 対称行列 A を三重対角行列 T に変換する。 * A = Q * T * Q^T を満たす Q (直交行列), T (三重対角) を返す。 * * 参考: Golub & Van Loan "Matrix Computations" 4th ed. §8.3 * * @param A 対称行列 (n×n) * @return TridiagonalResult {Q, d, e} */ template TridiagonalResult tridiagonalize(const Matrix& A) { const auto n = A.rows(); if (n != A.cols()) { throw DimensionError("tridiagonalize: matrix must be square"); } if (n == 0) { throw DimensionError("tridiagonalize: empty matrix"); } // 対称性チェック (緩い) for (std::size_t i = 0; i < n; ++i) for (std::size_t j = i + 1; j < n; ++j) { double aij = static_cast(A(i, j)); double aji = static_cast(A(j, i)); double scale = std::max(std::abs(aij), 1.0); if (std::abs(aij - aji) > scale * static_cast(n) * static_cast(std::numeric_limits::epsilon()) + 1e-14) throw MathError("tridiagonalize: matrix must be symmetric"); } // 作業用コピー Matrix W(A); // Householder ベクトル・係数・対応する k を保存 struct HouseholderInfo { std::size_t k; Vector v; T beta; }; std::vector hh_list; // Householder 三重対角化 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; // 既に三重対角形 } T alpha = std::sqrt(x0 * x0 + sigma); if (x0 > T(0)) alpha = -alpha; T v0 = x0 - alpha; T beta = T(2) / (v0 * v0 + sigma); // 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 }); // 対称 Householder 更新: W ← (I - β vvᵀ) W (I - β vvᵀ) // Golub & Van Loan の効率的公式: // 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]; // 副対角要素を設定 W(k + 1, k) = alpha; W(k, k + 1) = alpha; } // 対角・副対角要素を抽出 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); // Q の逆蓄積: Q = H_0 * H_1 * ... * H_{n-3} // Q = I, 逆順に Q ← (I - β v vᵀ) * Q // ただし各 H_k は行 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 三重対角化のコンパクト結果 (Householder ベクトルを保持、Q は形成しない) * * eigen_symmetric の逆変換フェーズで Q を陽に形成する代わりに * Householder ベクトルを Z に直接適用することで O(n³) を節約する。 */ template struct TridiagonalCompact { struct Reflector { std::size_t k; Vector v; T beta; }; std::vector refs; Vector d; Vector e; }; /** * @brief Householder 三重対角化 (コンパクト形式) * * Q を陽に形成せず、Householder ベクトルと係数のリストを返す。 * eigen_symmetric の逆変換で Householder 列を Z に直接適用するために使用。 * SYMV に下三角アクセス最適化を適用。 * * @param A 対称行列 (n×n) — 対称性は呼び出し元で検証済みであること * @return TridiagonalCompact {refs, d, e} */ template TridiagonalCompact tridiagonalize_compact(const Matrix& A) { const auto n = A.rows(); if (n == 0) 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 Householder 反射列を行列 Z に左から適用 * * Z ← Q * Z = H_0 * H_1 * ... * H_{m-1} * Z * H_k = I - β_k * v_k * v_kᵀ (行 k+1:n に作用) * * 内側から適用: まず H_{m-1}, 次に H_{m-2}, ..., 最後に H_0 * Q を陽に形成せずに直接 Z を変換する。 * * 列優先ループ順序: 各列を L1 キャッシュに保持しながら * 全リフレクタを適用 (Z が 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; // リフレクタのデータをフラットバッファに事前抽出 // (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()}; } // 列ブロッキング + 4列アンロール: // v を一度ロードして 4 列に同時適用 (レジスタ再利用 + 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 三重対角行列を陽に構築するユーティリティ * * @param d 対角要素 (サイズ n) * @param e 副対角要素 (サイズ n-1) * @return Matrix 三重対角行列 (n×n) */ template Matrix build_tridiagonal_matrix(const Vector& d, const Vector& e) { const size_t n = d.size(); if (n > 1 && e.size() != n - 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; } // 対称行列の固有値分解を計算する関数（巡回ヤコビ法） // 全固有値・固有ベクトルを直交回転で計算する // 入力: A (n×n 対称行列) // 出力: { eigenvalues (昇順), eigenvectors (列ベクトル) } template std::tuple, Matrix> symmetric_eigen(const Matrix& A) { 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 = A; for (int sweep = 0; sweep < max_sweep; ++sweep) { // 非対角要素のフロベニウスノルム 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; // 閾値 (Rutishauser): 最初の 4 sweep は小要素をスキップ const T threshold = (sweep < 4) ? T(0.2) * off_norm / T(n * n) : T(0); // 巡回ヤコビ: 全 (p, q) ペアを行順に走査 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); // 閾値未満はスキップ if (g * g <= threshold) continue; // 既に十分小さい場合もスキップ if (g <= eps * (std::abs(S(p, p)) + std::abs(S(q, q)))) { S(p, q) = S(q, p) = T(0); continue; } // 安定な回転パラメータ計算 (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 パラメータ // S の更新: S = J^T S J (対角と行列要素) 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); } // 固有ベクトルの更新 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); } } } } // 固有値の抽出と昇順ソート Vector eigenvalues(n); for (size_t i = 0; i < n; ++i) eigenvalues[i] = S(i, i); // 選択ソート (固有ベクトルも同時に並べ替え) 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 特異値分解（SVD）を計算 * @tparam T 要素の型 * @param a 分解する行列 * @param transV {true: V^T を返す, false: V を返す} * @return {U, Σ, V^T} のタプル */ // --------------------------------------------------------------- // Golub-Kahan SVD (二重対角化 + 暗黙的シフト QR 反復) // // 旧実装は A^T*A のヤコビ法で条件数が二乗され、 // double でも特異値の精度が sqrt(eps) ≈ 1e-8 に劣化していた。 // 本実装は A を直接 Householder 変換で二重対角化し、 // Golub-Kahan QR 反復で特異値を求める。 // 参考: Golub & Van Loan "Matrix Computations" 4th ed. §8.6 // --------------------------------------------------------------- namespace svd_detail { // Givens 回転: [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; } } // ----- 対称三重対角 QL 反復 (D&C のベースケース) ----- // d[0..n-1] 対角, e[0..n-1] 副対角 (e[n-1]=0 sentinel) // Q[n×n] 行優先: 呼び出し側で単位行列に初期化 // 出力: d = 固有値 (昇順), Q の列 = 固有ベクトル 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); } } // 昇順ソート + Q 列の並び替え 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]); } } } // ----- 世俗方程式ソルバー ----- // f(λ) = 1 + ρ * Σ z2[i]/(d[i]-λ) = 0 を (lo, hi) 内で解く // z2[i] = z[i]² (事前計算) 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; } // ブラケット更新 if (f * rho < T(0)) lo = lam; else hi = lam; if (std::abs(f) <= eps * T(n)) break; // Newton ステップ (ブラケットガード付き) 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; } // ----- 対称三重対角 Divide-and-Conquer 固有値分解 ----- // d[0..n-1] 対角 (出力: 固有値 昇順) // e[0..n-1] 副対角 (e[n-1]=0 sentinel, 破壊) // Q[n×n] 行優先: 出力は固有ベクトル列 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); // 再帰: 部分固有ベクトル行列 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 ベクトル: Q1 の最終行 + 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]; // d[0..k-1] と 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(); // ||z||, 最大 |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) { // === デフレーション (LAPACK dlaed2 相当) === const T tol = T(8) * eps * std::max(d_max, std::abs(rho) * z_norm); // deflated[i]: true なら固有値は ds[i], 固有ベクトルは e_i std::vector deflated(n, false); size_t n_defl = 0; // Type 1: |z[i]| が小さい → デフレーション for (size_t i = 0; i < n; ++i) { if (std::abs(zs[i]) <= tol) { deflated[i] = true; ++n_defl; } } // Type 2: 隣接 d[i] ≈ d[i+1] → Givens で z[i] を消去 for (size_t i = 0; i + 1 < n; ++i) { if (deflated[i]) continue; if (std::abs(ds[i + 1] - ds[i]) <= tol) { // 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)) { T c = zs[i + 1] / r; T s = zs[i] / r; zs[i] = T(0); zs[i + 1] = r; } deflated[i] = true; ++n_defl; } } // 非デフレーション成分のインデックス 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; } // デフレーションされた固有値・固有ベクトルを設定 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) { // 非デフレーション部分の d, z を抽出 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]; } // 世俗方程式を解く (n_active 個の固有値) 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 再計算 (LAPACK dlaed3 相当) === // z̃[i]² = Π_j (λ[j]-d[i]) / Π_{k≠i} (d[k]-d[i]) // log 空間で計算してオーバーフローを回避 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), 正のはず T abs_val = std::exp(log_abs / T(2)); z_new[i] = std::copysign(abs_val, za[i]); } // 固有ベクトル: 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; } } } // === 昇順ソート (lam[]) === // 固有値の順序が世俗方程式・デフレーション後に乱れている可能性 // → ソートして W の列も入れ替え 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: マージのみ for (size_t i = 0; i < n; ++i) { lam[i] = ds[i]; W[i * n + i] = T(1); } } // W の行を元の順序に戻す: 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]; } // 固有値を出力 for (size_t i = 0; i < n; ++i) d[i] = lam[i]; // マージ: 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 反復 (二重対角 SVD) ----- // d[0..n-1] 対角要素 (入出力: 特異値) // e[0..n-1] 超対角要素 (e[0]=0, 破壊) // Uc[n*n], Vc[n*n] 列優先: 単位行列で初期化済み前提 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); // 負の対角要素を正に修正 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]; } } // 列優先 Givens ヘルパー (連続メモリアクセス) 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; // ゼロ対角要素のデフレーション 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 シフト 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 std::tuple, Vector, Matrix> svd_decomposition(const Matrix& A, bool transV = true) { const size_t m = A.rows(); const size_t n = A.cols(); // m < n の場合: A^T の SVD から導出 if (m < n) { auto [V, s, Ut] = svd_decomposition(A.transpose(), true); // SVD(A^T) = V * S * U^T → SVD(A) = U * S * V^T if (transV) return { Ut.transpose(), s, V.transpose() }; else return { Ut.transpose(), s, V }; } // 以下 m >= n const size_t min_mn = n; if (min_mn == 0) { return { Matrix(m, 0), Vector(0), Matrix(0, n) }; } #if CALX_HAS_MKL if constexpr (std::is_same_v || std::is_same_v) { Matrix Acopy = A; Vector s(min_mn); Matrix U(m, m, T(0)); Matrix Vt(n, n, T(0)); std::vector superb(min_mn - 1); lapack_int info; if constexpr (std::is_same_v) info = LAPACKE_sgesvd(LAPACK_ROW_MAJOR, 'A', 'A', (lapack_int)m, (lapack_int)n, Acopy.data(), (lapack_int)n, s.data(), U.data(), (lapack_int)m, Vt.data(), (lapack_int)n, superb.data()); else info = LAPACKE_dgesvd(LAPACK_ROW_MAJOR, 'A', 'A', (lapack_int)m, (lapack_int)n, Acopy.data(), (lapack_int)n, s.data(), U.data(), (lapack_int)m, 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 // =============================================================== // float/double 高速パス: 遅延蓄積 + 列優先 QR + WY 逆変換 // =============================================================== 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 二重対角化 // 左 reflector: W(k+1:m, k) に正規化ベクトル, tau_left[k] // 右 reflector: W(k, k+2:n) に正規化ベクトル, tau_right[k] #ifdef CALX_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) { // --- 左 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; // 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; } } // --- 右 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 CALX_SVD_PHASE_TIMING auto _svd_t1 = std::chrono::high_resolution_clock::now(); #endif // Phase 2: 二重対角 → 特異値・特異ベクトル const T eps = std::numeric_limits::epsilon(); // 列優先 n×n 行列 (Phase 3 で使用) std::vector Uc(n * n, T(0)); // U_bidiag 列優先 std::vector Vc(n * n, T(0)); // V_bidiag 列優先 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 → 対称三重対角 D&C ===== // 元の二重対角要素を保存 std::vector bd(d.begin(), d.begin() + n); std::vector be(e.begin(), e.begin() + n); // T = B^T*B: 対称三重対角 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 固有値分解: td → 固有値(昇順), Q_eig → 固有ベクトル std::vector Q_eig(n * n); svd_detail::tridiag_dc_eig(td.data(), te.data(), n, Q_eig.data()); // 特異値: σ = sqrt(λ) for (size_t i = 0; i < n; ++i) d[i] = std::sqrt(std::max(td[i], T(0))); // V = Q_eig → Vc (列優先), U = B*V*Σ^{-1} → Uc (列優先) 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; } } // σ ≈ 0 の場合は Uc の列 j は 0 のまま (identity から初期化済み) } } else { // ===== Golub-Kahan QR 反復 ===== const int max_iter = 100 * static_cast(n); // 負の対角要素を正に修正 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]; // Uc の列 i を反転 T* col = &Uc[i * n]; for (size_t j = 0; j < n; ++j) col[j] = -col[j]; } } // 列優先 Givens ヘルパー (連続メモリアクセス) 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; // ゼロ対角要素のデフレーション 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 シフト 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) { // 右 Givens (列 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); // 左 Givens (行 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 CALX_SVD_PHASE_TIMING auto _svd_t2 = std::chrono::high_resolution_clock::now(); #endif // Phase 3: 逆変換 // 負の特異値を正に 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]; } } // 降順ソート (列優先なので列交換) 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]); // 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]); } } } // Uc, Vc を行優先に変換 → 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]; // 列優先→行優先 Vb(i, j) = Vc[j * n + i]; } // U = Q_left * Ub: 左 Householder reflectors を Ub に逆順適用して m×n に拡張 // result (m×n): 先頭 n 行 = Ub, 残り m-n 行 = 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); // 左 reflectors を WY ブロックで逆順適用: H_{n-1}, ..., H_0 // Q = H_0*H_1*...*H_{n-1}, ブロック右から適用 { 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; } } // 右 reflectors を WY ブロックで逆順適用: 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 CALX_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 // 結果を構築 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 // =============================================================== // 汎用パス (非 float/double) — Complex 対応 // =============================================================== // 実数型: Complex の場合は underlying real type, そうでなければ T using real_t = decltype(std::abs(T())); const T zero = numeric_traits::zero(); const T one = numeric_traits::one(); 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[] は二重対角要素 — bidiagonalize 中は T 型、後で実数化 std::vector d_cx(n, zero); std::vector e_cx(n, zero); for (size_t k = 0; k < n; ++k) { // --- 左 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) { // 符号選択 (Complex: 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 += calx::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 * calx::conj(W(j, k)); else U(i, j) -= dot_val * W(j, k); } } d_cx[k] = alpha; } else { d_cx[k] = W(k, k); } } // --- 右 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) * calx::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) * calx::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); } } // d_cx[], e_cx[] を実数化し、位相を 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) // B を実正二重対角に変換: B = D_L * B_real * D_R^H // d[k] と e[k+1] を交互に実正化し、位相を U, V に吸収 for (size_t k = 0; k < n; ++k) { // d_cx[k] を実正化 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 = calx::conj(phase); // U の列 k に位相吸収: U[:,k] *= phase for (size_t i = 0; i < m; ++i) U(i, k) *= phase; // e_cx[k+1] に伝播: e_cx[k+1] *= pconj if (k + 1 < n) e_cx[k + 1] = pconj * e_cx[k + 1]; } // e_cx[k+1] を実正化 // V[:,k+1] *= conj(phase) で B の列 k+1 に conj(phase) がかかる 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 = calx::conj(phase); // V[:,k+1] *= pconj → B の列 k+1 に pconj がかかる // e[k+1] *= pconj = |e|*phase * conj(phase)/|phase|^2 → 実正 for (size_t i = 0; i < n; ++i) V(i, k + 1) *= pconj; // d_cx[k+1] にも pconj が伝播 d_cx[k + 1] = pconj * d_cx[k + 1]; } } } } else { // 実数型: d_cx, e_cx は real_t として直接使える for (size_t i = 0; i < n; ++i) { d[i] = real_t(d_cx[i]); e[i] = real_t(e_cx[i]); } // 負の対角要素を正に修正 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 反復 (d[], e[] は実数、Givens 回転も実数) 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) = calx::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 }; } } /** * @brief SVDの結果から元の行列を復元 * @tparam T 要素の型 * @param U 左特異ベクトル行列 (m x k) * @param sigma 特異値ベクトル (k) * @param V 右特異ベクトル行列 (n x k) * @return 復元された行列 A (m x n) */ template Matrix reconstruct_matrix_from_svd( const Matrix& U, // U const Vector& s, // Σの対角成分 const Matrix& Vt) // V^T { const auto m = U.rows(); const auto n = Vt.cols(); // V^Tの列数 const auto r = s.size(); // 通常 min(m, n) Matrix result(m, n, numeric_traits::zero()); // 再構成 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 改良版SVD分解を使って連立方程式を解く * @tparam T 要素の型 * @param a 係数行列 * @param b 右辺ベクトル * @param rcond 条件数の逆数の閾値（デフォルト値はマシンイプシロンの100倍） * @return 解ベクトル */ template Vector svd_solve(const Matrix& a, const Vector& b, T rcond = T(numeric_traits::epsilon() * 100)) { if (a.rows() != b.size()) { 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分解 auto [U, sigma, Vt] = svd_decomposition(a); // 2. 最大特異値を見つける (sigma は常に実数値) 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. 閾値の計算 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. U^H * b の計算 (Complex: 共役転置) 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] += calx::conj(U(j, i)) * b[j]; else Utb[i] += U(j, i) * b[j]; } } // 5. 解の計算 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) { // 閾値以下の特異値に対して、連続的に重みを減らす 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 LDL分解を計算 * @tparam T 要素の型 * @param a 分解する行列（対称行列） * @return LとDのペア（LとDを別々に返す） */ template std::pair, Vector> ldl_decomposition(const Matrix& a) { if (a.rows() != a.cols()) { 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); // 単位行列でLを初期化 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); } } // LDL分解の計算 for (typename Matrix::size_type j = 0; j < n; ++j) { // 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; // 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]; } // ゼロ対角要素のチェック 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 LDL分解を使って連立方程式を解く * @tparam T 要素の型 * @param a 係数行列（対称行列） * @param b 右辺ベクトル * @return 解ベクトル */ template Vector ldl_solve(const Matrix& a, const Vector& b) { if (a.rows() != a.cols()) { throw DimensionError("ldl_solve: matrix must be square"); } if (a.rows() != b.size()) { throw DimensionError("ldl_solve: matrix and vector dimensions mismatch"); } const auto n = a.rows(); // LDL分解 auto [L, D] = ldl_decomposition(a); // 前進代入 (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; } // 対角スケーリング (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]; } // 後退代入 (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 ベクトルの内積を計算 * @tparam T 要素の型 * @tparam V ベクトル型 * @param a 1つ目のベクトル * @param b 2つ目のベクトル * @return 内積の値 */ template requires concepts::VectorOf T dot(const V& a, const V& b) { if (a.size() != b.size()) { 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 ベクトルのノルムを計算 * @tparam T 要素の型 * @tparam V ベクトル型 * @param v ベクトル * @return ノルムの値 */ template requires concepts::VectorOf T norm(const V& v) { return std::sqrt(dot(v, v)); } /** * @brief 行列の階数 (rank) を計算 * SVD の特異値のうち閾値以上のものを数える。 * @param a 入力行列 * @param tol 閾値 (負の場合は自動設定: max(m,n) * eps * sigma_max) * @return 階数 */ template size_t rank(const Matrix& a, T tol = T(-1)) { auto [U, sigma, Vt] = svd_decomposition(a); const size_t k = sigma.size(); if (k == 0) return 0; // 閾値の自動設定 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 行列の条件数 (condition number) を計算 * 2-ノルム条件数: sigma_max / sigma_min * @param a 入力行列 (正方行列) * @return 条件数 (特異行列の場合は +infinity) */ template T conditionNumber(const Matrix& 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 { // LU 分解済み行列で Ax = b を解く (前方・後退代入) template Vector lu_solve_factored( const Matrix& lu, const std::vector::size_type>& pivots, const Vector& b) { const auto n = lu.rows(); Vector x = b; // ピボット置換 + 前方代入 (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]; } // 後退代入 (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; } // LU 分解済み行列で A^T x = b を解く template Vector lu_solve_transpose_factored( const Matrix& lu, const std::vector::size_type>& pivots, const Vector& b) { const auto n = lu.rows(); Vector x = b; // 前方代入 (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]; } // 後退代入 (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 の対角は 1 } // 逆ピボット置換 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 Hager-Higham アルゴリズムによる ‖A⁻¹‖₁ の推定 * * LU 分解済み行列を受け取り、A⁻¹ を陽に構築せずに * ‖A⁻¹‖₁ を O(n²) で推定する。 * 参考: Higham (1988) "FORTRAN codes for estimating the one-norm of * a real or complex matrix, with applications to condition estimation" * * @param lu LU 分解済み行列 (lu_decomposition の出力) * @param pivots ピボット情報 * @return ‖A⁻¹‖₁ の推定値 (下界) */ template T estimate_inv_norm1( const Matrix& lu, const std::vector::size_type>& pivots) { const auto n = lu.rows(); if (n == 0) return T(0); // Hager-Higham: 最大 5 回反復 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)) // 収束判定: ‖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 1-ノルム条件数の推定: κ₁(A) = ‖A‖₁ · ‖A⁻¹‖₁ * * LU 分解 + Hager-Higham 推定で O(n³ + n²) ≈ O(n³) だが、 * SVD ベースの conditionNumber() より大幅に高速。 * 特異行列の場合は LU 分解が例外を投げる。 * * @param A 正方行列 * @return κ₁(A) の推定値 */ template T condition_number_1norm(const Matrix& A) { if (A.rows() != A.cols()) { 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 ∞-ノルム条件数の推定: κ∞(A) = ‖A‖∞ · ‖A⁻¹‖∞ = κ₁(A^T) * * @param A 正方行列 * @return κ∞(A) の推定値 */ template T condition_number_inf(const Matrix& A) { return condition_number_1norm(A.transpose()); } /** * @brief 逆条件数の推定: rcond₁(A) = 1 / κ₁(A) * * LAPACK の dgecon に相当。0 に近いほど特異に近い。 * * @param A 正方行列 * @return 1/κ₁(A) の推定値 (特異行列の場合は LU が例外) */ template T rcond_1norm(const Matrix& A) { T cond = condition_number_1norm(A); if (cond == T(0)) return T(0); return T(1) / cond; } //===================================================================== // Hessenberg 分解 //===================================================================== /** * @brief Hessenberg 分解: A = Q H Qᵀ * * Householder 反射で正方行列 A を上 Hessenberg 行列 H に変換する。 * H(i,j) = 0 for i > j+1。Q は直交行列。 * * @tparam T 要素の型 (double, float 等) * @param A 入力行列 (n×n 正方行列) * @return {H, Q} — H: 上 Hessenberg 行列, Q: 直交行列 (A = Q*H*Qᵀ) */ template std::pair, Matrix> hessenberg_decomposition(const Matrix& A) { const auto n = A.rows(); if (n != A.cols()) { 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}; } // 列 k = 0, 1, ..., n-3 について Householder 反射 for (std::size_t k = 0; k < n - 2; ++k) { // Householder ベクトル v を構築 (H(k+1:n, k) を対象) const std::size_t m = n - k - 1; // ベクトル長 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; // すでにゼロ → スキップ } // sign(v[0]) * ||v|| を v[0] に加算 T alpha = (v[0] >= T(0)) ? -norm_v : norm_v; v[0] -= alpha; // 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 (左から): 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ᵀ) (右から): 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ᵀ) (右から蓄積) 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]; } } } // サブ対角以下の微小値をクリーンアップ 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 分解 (実 Schur) //===================================================================== /** * @brief 実 Schur 分解: A = Q T Qᵀ * * T は準上三角行列 (実 Schur 形式): * - 実固有値は 1×1 対角ブロック * - 複素共役対は 2×2 対角ブロック * Q は直交行列。 * * アルゴリズム: Hessenberg 分解 → Francis ダブルシフト QR 反復 * * @tparam T 要素の型 * @param A 入力行列 (n×n 正方行列) * @return {T, Q} — T: 準上三角行列, Q: 直交行列 (A = Q*T*Qᵀ) */ template std::pair, Matrix> schur_decomposition(const Matrix& A) { const auto n = A.rows(); if (n != A.cols()) { throw DimensionError("schur_decomposition: matrix must be square"); } if (n <= 1) { return {A, Matrix(1, 1, T(1))}; } #if CALX_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 分解 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 反復 // 作業範囲は [ilo, ihi) (0-indexed) int ihi = static_cast(n); int total_iter = 0; int iter_since_deflation = 0; // 例外シフト判定用 while (ihi > 1 && total_iter < max_iter) { // デフレーション: サブ対角要素が十分小さい箇所を探す 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 ブロック (実固有値): デフレート --ihi; iter_since_deflation = 0; continue; } if (ilo == ihi - 2) { // 2×2 ブロック: 固有値を計算して判定 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)) { // 実固有値 → Wilkinson シフト付き QR で対角化 // シフト: 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 回転 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; // 左から回転 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; } // 右から回転 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; } // 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 ブロック (複素共役対 or 対角化済み): デフレート ihi -= 2; iter_since_deflation = 0; continue; } // Francis ダブルシフト QR ステップ auto compute_exceptional_shift = [&]() -> std::pair { // 例外シフト (LAPACK スタイル): // 通常のシフトで収束しない場合、アドホックな // シフトでパターンを崩す 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 シフト: 右下 2×2 ブロックの固有値 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 // 暗黙的 QR ステップ (バルジチェース) // 初期ベクトル: (H - σ₂I)(H - σ₁I)e₁ の最初の3要素 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); // 初期ベクトル縮退検出: // x≈0, y≈0 だとバルジチェースが実質行/列の // 入替のみとなり収束しない → 例外シフトで回避 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) { // 3×1 (or 2×1 at end) Householder 反射でバルジを追いかける int p = std::min(3, ihi - k); // Householder ベクトル 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; // 2 / (1 + v2² + v3²) と同等 // 左から反射: 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; } // 右から反射: 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; } // 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; } // 次の反復用の x, y, z を更新 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; } // サブ対角の微小値をクリーンアップ 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); } } return {Sch, Q}; } //===================================================================== // 極分解 //===================================================================== /** * @brief 極分解: A = U_polar * H_polar * * U_polar: 直交行列 (最近直交行列) * H_polar: 対称半正定値行列 * * SVD ベース: A = UΣVᵀ → U_polar = UVᵀ, H_polar = VΣVᵀ * * @tparam T 要素の型 * @param A 入力行列 (m×n, m >= n) * @return {U_polar, H_polar} */ template std::pair, Matrix> polar_decomposition(const Matrix& A) { const auto m = A.rows(); const auto n = A.cols(); if (m < n) { 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 (m != n) { 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 高木分解の結果を格納する構造体 * * 複素対称行列 A = U * D * U^T (注: U^T, NOT U^H) * U: ユニタリ行列, D: 実非負対角行列 */ template struct TakagiResult { Matrix> U; ///< ユニタリ行列 (n×n) Vector d; ///< 実非負対角要素 (= 特異値) }; /** * @brief 高木分解: A = U * diag(d) * U^T * * 複素対称行列 A (A^T = A, 注: A^H ≠ A 一般) を分解する。 * d は A の特異値 (実非負)、U はユニタリ行列。 * * アルゴリズム: A = X + iY として実 2n×2n 行列 * M = [[X, -Y], [Y, X]] * の SVD を利用。M の特異値は A の特異値の重複ペア。 * 参考: Horn & Johnson "Matrix Analysis" 2nd ed., §4.4 * * @param A 複素対称行列 (n×n, A^T = A) * @return TakagiResult {U, d} */ template TakagiResult takagi_factorization(const Matrix>& A) { const auto n = A.rows(); if (n != A.cols()) { throw DimensionError("takagi_factorization: matrix must be square"); } if (n == 0) { throw DimensionError("takagi_factorization: empty matrix"); } // 対称性チェック (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)"); } // 実部 X と虚部 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; } // 実 2n×2n 行列 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); // 特異値は重複ペアで現れる: σ₁, σ₁, σ₂, σ₂, ... // 偶数インデックスの特異値を取得 Vector d(n); for (std::size_t i = 0; i < n; ++i) d[i] = sigma_real[2 * i]; // U_real の上半分と下半分からユニタリ行列 U を構築 // U_real の列 2k を使用: 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)); } } // 位相補正: 高木分解の関係 A * conj(u_k) = σ_k * u_k を利用 // もし z_k が正しい u_k から位相 φ だけずれている (z_k = u_k * e^{iφ}) なら: // A * conj(z_k) = A * conj(u_k) * e^{-iφ} = σ_k * u_k * e^{-iφ} // = σ_k * z_k * e^{-2iφ} // よって w_k = A * conj(z_k) / (σ_k * z_k) = e^{-2iφ} // → φ = -arg(w_k) / 2, 補正: 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)); // z_k の最大成分で位相を推定 std::size_t max_idx = 0; T max_abs = T(0); for (std::size_t i = 0; i < n; ++i) { T a = calx::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 分解 (Cosine-Sine Decomposition) //===================================================================== /** * @brief CS 分解の結果 * * 直交行列 Q を以下のように分解する: * Q = [[U1, 0], [0, U2]] * Σ * [[V1, 0], [0, V2]]^T * * p ≤ q のとき、中央行列 Σ (m×m) は: * Σ = [[C, -S, 0 ], * [S, C, 0 ], * [0, 0, I_{q-p}]] * ここで C = diag(cos θ_i), S = diag(sin θ_i), C² + S² = I */ template struct CSDecompositionResult { Matrix U1; ///< p×p 直交行列 Matrix U2; ///< q×q 直交行列 (q = m - p) Matrix V1; ///< p×p 直交行列 Matrix V2; ///< q×q 直交行列 Vector theta; ///< r = min(p,q) 個の角度 θ_i ∈ [0, π/2] }; namespace detail_cs { // 直交補完: M (n×n) の既知 n_known 列に直交する残りの列を求める 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) { // 候補ベクトル e_{trial} Vector v(n, T(0)); v[trial] = T(1); // 既知列への射影を除去 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); } // ノルム計算 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 分解 (Cosine-Sine Decomposition) * * 直交行列 Q (m×m) を分割サイズ p で分解する。 * Q の左上ブロック Q11 (p×p) の SVD を基に、 * ユニタリブロック対角因子と余弦・正弦対角行列に分解する。 * * @param Q 直交行列 (m×m) * @param p 分割サイズ (0 の場合は m/2)。0 < p < m * @return CSDecompositionResult * @throws DimensionError 非正方行列、空行列、不正な分割サイズ */ namespace detail_cs { // p ≤ q 用の内部実装: // SVD(Q11) から U1, V1, theta を求め、Q21, Q12, Q22 から U2, V2 を構築 template void solve_smaller_block( const Matrix& Q11, const Matrix& Q12, const Matrix& Q21, const Matrix& Q22, std::size_t p, std::size_t q, Matrix& U1, Matrix& U2, Matrix& V1, Matrix& V2, Vector& theta) { 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): 列 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): 列 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; } } // 残りの列を 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 (追加分) template CSDecompositionResult cs_decomposition(const Matrix& Q, std::size_t p = 0) { std::size_t m = Q.rows(); if (m < 2 || Q.cols() != m) throw DimensionError("cs_decomposition: requires a square matrix of size >= 2"); if (p == 0) p = m / 2; if (p == 0 || p >= m) throw DimensionError("cs_decomposition: partition size p must satisfy 0 < p < m"); std::size_t q = m - p; // ブロック抽出 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: SVD(Q11) (p×p) を起点に U2, V2 を構築 detail_cs::solve_smaller_block(Q11, Q12, Q21, Q22, p, q, U1, U2, V1, V2, theta); } else { // p > q: SVD(Q22) (q×q) を起点に U1, V1 を構築 // 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): 列 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): 列 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; } } // 残りの列を 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 ランク分解の結果 * * A = B * C (B: m×r, C: r×n, r = rank(A)) * SVD に基づく: B = U_r * diag(σ_r), C = V_r^T * (U_r: 最初の r 列, σ_r: 非零特異値, V_r^T: 最初の r 行) */ template struct RankDecompositionResult { Matrix B; ///< m×r 行列 (列空間の基底 × 特異値) Matrix C; ///< r×n 行列 (行空間の基底) std::size_t rank; ///< 数値ランク }; /** * @brief ランク分解 (Rank Decomposition) * * 行列 A (m×n) を A = B * C (B: m×r, C: r×n) に分解する。 * SVD を利用し、数値ランク r を閾値で決定する。 * * @param A 入力行列 (m×n) * @param rank 指定ランク (0 の場合は自動検出) * @param tol ランク判定閾値 (0 の場合は max(m,n) * σ_max * ε) * @return RankDecompositionResult * @throws DimensionError 空行列 * @throws MathError 指定ランクが実際のランクより大きい場合 */ template RankDecompositionResult rank_decomposition( const Matrix& 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) throw DimensionError("rank_decomposition: empty matrix"); auto [U, sigma, Vt] = svd_decomposition(A); // ランク決定 std::size_t k = sigma.size(); // min(m, n) if (tol <= T(0)) { // デフォルト閾値: 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; // σ は降順 } if (rank > 0) { if (rank > r) throw MathError("rank_decomposition: specified rank exceeds numerical rank"); r = rank; } // r == 0 (零行列) の場合 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 分解 (CUR Decomposition) //===================================================================== /** * @brief CUR 分解の結果 * * A ≈ C * U * R * C: m×c 行列 (A の選択された c 列) * U: c×r 連結行列 * R: r×n 行列 (A の選択された r 行) * col_indices: 選択された列のインデックス * row_indices: 選択された行のインデックス */ template struct CURDecompositionResult { Matrix C; ///< m×c 行列 (選択列) Matrix U; ///< c×r 連結行列 Matrix R; ///< r×n 行列 (選択行) std::vector col_indices; ///< 選択された列インデックス std::vector row_indices; ///< 選択された行インデックス }; /** * @brief CUR 分解 (CUR Decomposition) * * 行列 A (m×n) を A ≈ C * U * R に分解する。 * SVD のレバレッジスコアに基づく決定的列・行選択。 * C は A の列部分集合、R は A の行部分集合、 * U = C⁺ * A * R⁺ (擬似逆行列による連結)。 * * @param A 入力行列 (m×n) * @param k 選択する列数・行数 (0 の場合は数値ランクを使用) * @return CURDecompositionResult * @throws DimensionError 空行列 * @throws MathError k が min(m,n) を超える場合 */ template CURDecompositionResult cur_decomposition( const Matrix& A, std::size_t k = 0) { std::size_t m = A.rows(); std::size_t n = A.cols(); if (m == 0 || n == 0) throw DimensionError("cur_decomposition: empty matrix"); auto [U_svd, sigma, Vt] = svd_decomposition(A); std::size_t s = sigma.size(); // min(m, n) // ランク決定 (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; // 最低 1 列/行 } if (k > std::min(m, n)) throw MathError("cur_decomposition: k exceeds min(m, n)"); // レバレッジスコアによる列選択 (右特異ベクトルの上位 k 成分の二乗和) // 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()); // レバレッジスコアによる行選択 (左特異ベクトルの上位 k 成分の二乗和) // 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: 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: 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) と R⁺ (n×r) は SVD 経由の擬似逆行列 // W = A の選択行×選択列部分行列 (r×c) // U = W⁺ (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]); // W の SVD → 擬似逆行列 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 NMF の結果 * * A ≈ W * H (W: m×k, H: k×n, W,H ≥ 0) */ template struct NMFResult { Matrix W; ///< m×k 基底行列 (非負) Matrix H; ///< k×n 係数行列 (非負) T residual; ///< フロベニウスノルム残差 ‖A - WH‖_F std::size_t iterations; ///< 実行反復回数 }; /** * @brief NMF (Non-negative Matrix Factorization) * * 非負行列 A (m×n) を A ≈ W * H (W: m×k, H: k×n, W,H ≥ 0) に分解する。 * Lee-Seung 乗法更新則を使用。 * * @param A 入力行列 (m×n, 全要素 ≥ 0) * @param k 因子数 (1 ≤ k ≤ min(m, n)) * @param max_iter 最大反復回数 (デフォルト 200) * @param tol 収束閾値 (残差の相対変化, デフォルト 1e-4) * @return NMFResult * @throws DimensionError 空行列, k が不正 * @throws MathError 負の要素が含まれる場合 */ template NMFResult nmf( const Matrix& 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) throw DimensionError("nmf: empty matrix"); if (k == 0 || k > std::min(m, n)) throw DimensionError("nmf: k must satisfy 1 <= k <= min(m, n)"); // 非負チェック 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(); // W, H を SVD ベースの 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 因子: 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 (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 因子 (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 乗法更新則 T prev_residual = std::numeric_limits::max(); std::size_t iter = 0; for (; iter < max_iter; ++iter) { // 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); // 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); // 残差計算 ‖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); // 収束判定 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 分解 (UTV Decomposition) //===================================================================== /** * @brief UTV 分解の結果 * * A = U * T * V^T * U: m×m 直交行列 * T: m×n 上三角行列 (対角が特異値の近似、降順) * V: n×n 直交行列 * rank: 数値ランク */ template struct UTVDecompositionResult { Matrix U; ///< m×m 直交行列 Matrix T_mat; ///< m×n 上三角行列 Matrix V; ///< n×n 直交行列 std::size_t rank; ///< 数値ランク }; /** * @brief UTV 分解 (UTV Decomposition) * * 行列 A (m×n) を A = U * T * V^T に分解する。 * 列ピボッティング付き QR 分解を基に構築し、 * T の対角が特異値を近似する。SVD より低コストなランク決定。 * * アルゴリズム: * 1. 列ピボッティング付き QR: A*P = Q*R * 2. R の右特異ベクトルで V を改良: R = U_R * Σ * V_R^T * 3. U = Q * U_R, T = Σ (対角に特異値), V = P * V_R * * @param A 入力行列 (m×n) * @param tol ランク判定閾値 (0: 自動) * @return UTVDecompositionResult * @throws DimensionError 空行列 */ template UTVDecompositionResult utv_decomposition( const Matrix& A, T tol = T(0)) { std::size_t m = A.rows(); std::size_t n = A.cols(); if (m == 0 || n == 0) throw DimensionError("utv_decomposition: empty matrix"); std::size_t k = std::min(m, n); // 列ピボッティング付き QR: 列ノルムが大きい順に選択 // A の作業コピー Matrix W(A); std::vector piv(n); for (std::size_t i = 0; i < n; ++i) piv[i] = i; // 列ノルムの計算 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 係数 for (std::size_t step = 0; step < k; ++step) { // 最大ノルム列を探索 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; } } // 列交換 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 変換で step 列の下三角を消去 // 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; // β = 2 / (v^T v) と等価 // 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; // 残りの列に Householder 変換を適用 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; } // 列ノルムの更新 for (std::size_t j = step + 1; j < n; ++j) col_norms[j] -= W(step, j) * W(step, j); } // R を抽出 (k×n 上三角) 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); // Q を構築 (m×m): Householder 変換の蓄積 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; } } // R の SVD: R = U_R * Σ * V_R^T auto [U_R, sigma, V_Rt] = svd_decomposition(R); // ランク決定 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; } // T 行列の構築 (m×n): 上部 k×n に Σ (対角に特異値 + 上三角 0) 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 は SVD(R) の左特異ベクトル (k×k or k×min(k,n)) // U_R_ext: 左上に U_R, 右下に I_{m-k} 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: ピボット置換を適用 // V_Rt は SVD(R) の右特異ベクトル転置。R が k×n のとき // V_Rt は min(k,n)×n or n×n (SVD 実装依存) // 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 RRQR の結果 * * A * P = Q * R (Q: m×m 直交, R: m×n 上三角, P: n×n 置換) * R の対角が特異値を近似し、ランクを明示する。 * Strong RRQR (Gu-Eisenstat) の簡易版: 列ピボッティング QR + * 後処理でのランク判定。 */ template struct RRQRResult { Matrix Q; ///< m×m 直交行列 Matrix R; ///< m×n 上三角行列 std::vector perm; ///< 列置換 (perm[j] = 元の列インデックス) std::size_t rank; ///< 数値ランク }; /** * @brief RRQR (Rank-Revealing QR) * * 行列 A (m×n) を列ピボッティング付き QR 分解し、 * R(k,k) が特異値の良い近似となるようにする。 * * @param A 入力行列 (m×n) * @param tol ランク判定閾値 (0: 自動 = max(m,n) * ‖A‖ * ε) * @return RRQRResult * @throws DimensionError 空行列 */ template RRQRResult rrqr(const Matrix& A, T tol = T(0)) { std::size_t m = A.rows(); std::size_t n = A.cols(); if (m == 0 || n == 0) throw DimensionError("rrqr: empty matrix"); std::size_t k = std::min(m, n); // 作業コピー Matrix W(A); std::vector piv(n); for (std::size_t i = 0; i < n; ++i) piv[i] = i; // 列ノルム 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 係数 Vector tau(k, T(0)); for (std::size_t step = 0; step < k; ++step) { // 最大ノルム列を選択 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; } } // 列交換 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 変換 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; // 残りの列に適用 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; } // 列ノルム更新 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); } } // 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); // 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; } } // ランク判定 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 }; } //===================================================================== // ブロック LU 分解 (Block LU Decomposition) //===================================================================== /** * @brief ブロック LU 分解の結果 * * A = P * L * U (L: 下三角, U: 上三角, P: 行置換) * ブロック単位で計算し、BLAS Level 3 (gemm) 相当の * 行列-行列乗算を活用してキャッシュ効率を改善。 */ template struct BlockLUResult { Matrix L; ///< n×n 下三角行列 (対角 = 1) Matrix U; ///< n×n 上三角行列 std::vector pivots; ///< 行置換 std::size_t block_size; ///< 使用ブロックサイズ }; /** * @brief ブロック LU 分解 (Block LU Decomposition) * * 正方行列 A (n×n) をブロック単位で LU 分解する。 * 各ステップで nb 列のパネルを通常 LU で分解し、 * 残りの部分を行列-行列乗算 (gemm 相当) で更新する。 * * @param A 入力行列 (n×n, 正方) * @param nb ブロックサイズ (0: 自動 = max(1, n/4)) * @return BlockLUResult * @throws DimensionError 空行列、非正方行列 * @throws MathError 特異行列 */ template BlockLUResult block_lu_decomposition( const Matrix& A, std::size_t nb = 0) { std::size_t n = A.rows(); if (n == 0 || A.cols() != n) 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); // 作業コピー: LU を一体で保持 Matrix LU(A); std::vector piv(n); for (std::size_t i = 0; i < n; ++i) piv[i] = i; // 特異判定の相対閾値: ||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; // パネル LU: 列 jb..jend-1 を部分ピボッティングで分解 for (std::size_t j = jb; j < jend; ++j) { // ピボット選択 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; // 行交換 (全列にわたって) if (pivot_row != j) { for (std::size_t c = 0; c < n; ++c) std::swap(LU(j, c), LU(pivot_row, c)); } // 特異チェック if (std::abs(LU(j, j)) <= sing_tol) throw MathError("block_lu_decomposition: singular matrix"); // L の列 j を計算 (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; // パネル内の残り列を更新 (ランク1更新) 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) { // U12 の前進代入: L11^{-1} * A12 → U12 // パネル LU はピボット行交換を全列に適用済みだが、 // ランク1更新はパネル内列のみ。右側列に L11 の効果を反映する。 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); // 残りのブロック更新 (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; } } } // L と 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 ブロック LU を使って連立方程式を解く: Ax = b */ template Vector block_lu_solve( const BlockLUResult& lu, const Vector& b) { std::size_t n = lu.L.rows(); if (b.size() != n) throw DimensionError("block_lu_solve: dimension mismatch"); // ピボット置換を適用 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]]); } // 前進代入: 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]; // 後退代入: 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 分解 (Hierarchically Semi-Separable) //===================================================================== /** * @brief HSS ノード: 階層的半分離構造の各ノードを表す * * 葉ノード: 対角ブロック D を保持 * 内部ノード: 生成行列 U, V (オフ対角低ランク表現) と * 変換行列 B (兄弟間の結合), R, W (圧縮行列) を保持 * * オフ対角ブロック 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; ///< 行の開始インデックス std::size_t row_size = 0; ///< 行の数 std::size_t col_start = 0; ///< 列の開始インデックス std::size_t col_size = 0; ///< 列の数 // 葉ノード: 対角ブロック Matrix D; // 全ノード: 生成行列 (低ランク表現用) Matrix U; ///< 行方向の生成行列 Matrix V; ///< 列方向の生成行列 // 内部ノード: 兄弟ノード間の結合行列 Matrix B; ///< 兄弟結合行列 // 内部ノード: 子から親への変換行列 Matrix R; ///< U の圧縮: U_parent = [R_left * U_left; R_right * U_right] Matrix W; ///< V の圧縮: V_parent = [W_left * V_left; W_right * V_right] // 子ノード std::unique_ptr> left; std::unique_ptr> right; }; /** * @brief HSS 分解の結果 */ template struct HSSResult { HSSNode root; ///< HSS 木のルート std::size_t leaf_size; ///< 葉ノードの最大サイズ std::size_t rank; ///< オフ対角低ランク近似のランク }; namespace detail_hss { /** * @brief HSS 木を構築 (分割のみ、行列値は未設定) */ 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 行列のサブブロックを抽出 */ template Matrix extract_block(const Matrix& 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 低ランク近似: 打ち切り SVD で A ≈ U * S * V^T を rank 階まで * @return {U_r (m×rank), V_r (n×rank)} — U_r には特異値を含む */ template std::pair, Matrix> low_rank_approx( const Matrix& 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()) }); // 有効ランクをさらに制限: 微小特異値を切り捨て 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); // 最低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 は V^T なので 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 HSS 木に行列の値を設定 (葉→根のボトムアップ) * * 葉ノード: D = A の対角ブロック, U/V はオフ対角行の低ランク近似 * 内部ノード: 子の U/V を圧縮して R/W を計算、B は兄弟間結合 */ template void compress_node( HSSNode& node, const Matrix& A, std::size_t rank) { std::size_t n = A.rows(); if (node.is_leaf) { // 対角ブロック node.D = extract_block(A, node.row_start, node.row_size, node.col_start, node.col_size); // オフ対角行: このノードの行のうち、対角ブロック以外の列 // 行 row_start..row_start+row_size-1 の全列から対角ブロック列を除く std::size_t off_cols = n - node.col_size; if (off_cols == 0) { // 行列全体が 1 ブロック → オフ対角なし node.U = Matrix(node.row_size, 1, T(0)); node.V = Matrix(1, 1, T(1)); // ダミー return; } // オフ対角行ブロックを構成 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); // オフ対角列ブロック: 対角ブロック以外の行、このノードの列 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 は V^T なので 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; } // 内部ノード: 子ノードを先に圧縮 compress_node(*node.left, A, rank); compress_node(*node.right, A, rank); auto& lc = *node.left; auto& rc = *node.right; // 兄弟間結合行列 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} // ただし U_left^+ = (U^T U)^{-1} U^T (擬似逆) 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); // 親の U, V 生成行列を子から合成 // オフ対角行ブロックを構成 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); // オフ対角列ブロック 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 HSS 表現から密行列を再構成 (検証用) */ template void reconstruct_node( const HSSNode& node, Matrix& A) { if (node.is_leaf) { // 対角ブロックを書き込み 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; } // 子ノードの対角部分を再帰的に再構成 reconstruct_node(*node.left, A); reconstruct_node(*node.right, A); auto& lc = *node.left; auto& rc = *node.right; // オフ対角ブロック: 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 // B^T の兄弟結合: A_rl = U_right * B_rl * V_left^T // B_rl は A(right, left) の結合行列 Matrix A_rl = extract_block( A, // 注意: A はまだ元の値を保持していない (再構成中) rc.row_start, rc.row_size, lc.col_start, lc.col_size); // 直接計算: A(right, left) の低ランク近似 // B_rl = pinv(U_right) * A_orig(right, left) * pinv(V_left)^T // しかし、再構成時には元行列がないため、別のアプローチが必要 // 対称的な 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 分解 (Hierarchically Semi-Separable) * * 正方行列 A を階層的半分離構造に分解する。 * オフ対角ブロックを低ランク近似し、階層的な木構造で表現する。 * * BEM、積分方程式、カーネル行列など低ランク構造を持つ行列に有効。 * * @param A 入力行列 (n×n, 正方) * @param leaf_size 葉ノードの最大サイズ (0: 自動 = max(2, n/4)) * @param rank オフ対角低ランク近似の最大ランク (0: 自動 = max(1, leaf_size/2)) * @return HSSResult * @throws DimensionError 空行列、非正方行列 */ template HSSResult hss_decomposition( const Matrix& A, std::size_t leaf_size = 0, std::size_t rank = 0) { std::size_t n = A.rows(); if (n == 0 || A.cols() != n) throw DimensionError("hss_decomposition: requires a non-empty square matrix"); if (n == 1) { // 1×1 行列: 単一葉 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 = A; 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); // HSS 木を構築 auto root = detail_hss::build_tree(0, n, 0, n, leaf_size); // ボトムアップ圧縮 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 HSS 表現から密行列を再構成 (検証用) * * @param hss HSS 分解の結果 * @return 再構成された密行列 */ 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 分解 (疎行列構造分解) //===================================================================== /** * @brief Dulmage-Mendelsohn 分解の結果 * * 行列 A(m×n) を行置換 P, 列置換 Q で * P * A * Q^T = ブロック上三角 (BTF) * に並べ替える。 * * 粗分解 (coarse decomposition): * HR (horizontal rough): マッチングされない行を含む部分 * SR (square rough): マッチングが完全な正方部分 * HC (horizontal coarse): マッチングされない列を含む部分 * * 正方部分 SR は強連結成分 (SCC) でさらにブロック上三角に分解される。 */ template struct DulmageMendelsohnResult { std::vector row_perm; ///< 行置換 (P) std::vector col_perm; ///< 列置換 (Q) std::vector block_starts; ///< ブロック境界 [0, b1, b2, ..., n_blocks] std::size_t n_blocks; ///< ブロック数 // 粗分解の境界 std::size_t hr_end; ///< HR 部分の終了行 (0 = HR なし) std::size_t sr_end; ///< SR 部分の終了行 std::size_t hc_start; ///< HC 部分の開始列 (n = HC なし) }; namespace detail_dm { /** * @brief 二部グラフの最大マッチングを Hopcroft-Karp で求める * * @param adj adj[row] = {col1, col2, ...} (非ゼロ列) * @param m 行数 * @param n 列数 * @param match_r match_r[row] = マッチした列 (SIZE_MAX = 未マッチ) * @param match_c match_c[col] = マッチした行 (SIZE_MAX = 未マッチ) * @return マッチングサイズ */ 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: 未マッチ行から交互路の距離ラベルを付ける 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; // NIL ノードの距離 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: 交互路を辿って増加パスを見つける std::function dfs = [&](std::size_t r) -> bool { if (r == m) return true; // 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 マッチングから到達可能な行/列を 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; // 未マッチ行から出発 for (std::size_t r = 0; r < m; ++r) { if (match_r[r] == NIL) { row_reached[r] = true; Q.push(r); } } // 交互路で到達可能な行/列を探索 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; // マッチ辺を逆にたどる if (match_c[c] != NIL && !row_reached[match_c[c]]) { row_reached[match_c[c]] = true; Q.push(match_c[c]); } } } } } /** * @brief 未マッチ列から列→行方向に到達可能な行/列を 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 マッチ行のみで構成されるグラフの強連結成分 (Tarjan) * * ノード = マッチされた行 (= マッチされた列と 1:1 対応) * 辺: 行 r → 列 match_r[r] → (非ゼロの) 行 r' (r' ≠ r, r' はマッチ行) */ 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 {}; // 行番号 → SQ 内インデックスのマッピング std::unordered_map row_to_idx; for (std::size_t i = 0; i < ns; ++i) row_to_idx[sq_rows[i]] = i; // SQ グラフの隣接リスト 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 の 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 は逆トポロジカル順で SCC を出力するので反転 std::reverse(order.begin(), order.end()); // block_starts も反転して再計算 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 は [size1, size1+size2, ...] のような累積値 // 正しい形式に修正 std::vector bs_new; bs_new.push_back(0); // 各ブロックのサイズを計算 std::vector sizes(nb); for (std::size_t i = 0; i < nb; ++i) { std::size_t end_pos = (i + 1 < nb) ? block_starts[i + 1] : ns; sizes[i] = block_starts[i] - (i > 0 ? block_starts[i - 1] : 0); } // 簡易化: order を SCC 順に再構成 // Tarjan の出力を逆転した結果、order[0..] がトポロジカル順 // block_starts を order から再構成する bs_new.clear(); bs_new.push_back(0); // Tarjan を再実装: iterative で正しいブロック区切りを出す // 現在の order は逆転済み。ブロック区切りを再計算 std::vector comp(ns, -1); { // もう一度 SCC を求め直す (簡潔に Kosaraju で) // 1st pass: DFS で finish order を求める 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); // 転置グラフ 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: 逆 finish order で転置グラフ DFS 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] がトポロジカル順の SCC ID // 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 分解 * * 行列 A (m×n) の非ゼロ構造から二部グラフを構成し、 * 最大マッチング + 強連結成分分解でブロック上三角形に並べ替える。 * * @param A 入力行列 * @param tol 非ゼロ判定の閾値 (|a_{ij}| > tol で非ゼロ) * @return DulmageMendelsohnResult * @throws DimensionError 空行列 */ template DulmageMendelsohnResult dulmage_mendelsohn( const Matrix& A, T tol = std::numeric_limits::epsilon()) { std::size_t m = A.rows(), n = A.cols(); if (m == 0 || n == 0) throw DimensionError("dulmage_mendelsohn: requires a non-empty matrix"); // 隣接リスト (行 → 非ゼロ列) std::vector> adj(m); std::vector> adj_t(n); // 転置 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); } // 最大マッチング std::vector match_r, match_c; detail_dm::hopcroft_karp(adj, m, n, match_r, match_c); // 粗分解: HR (未マッチ行から到達可能), HC (未マッチ列から到達可能) 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); // 分類: 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); } // SR 部分を 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); } // 行/列置換を構成 std::vector row_perm, col_perm; std::vector block_starts; // HR 部分 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 部分 (SCC 順) 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]); } // ブロック境界を追加 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 部分 for (std::size_t r : hc_r) row_perm.push_back(r); for (std::size_t c : hc_c) col_perm.push_back(c); // HC ブロックがある場合のみ末尾境界を追加 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 開始列 return result; } //===================================================================== // スミス標準形 (Smith Normal Form) //===================================================================== /** * @brief スミス標準形の結果 * * 整数行列 A (m×n) を A = U * D * V に分解する。 * - U: m×m ユニモジュラ行列 (det = ±1) * - D: m×n 対角行列 (不変因子: d_1 | d_2 | ... | d_r, 残りは 0) * - V: n×n ユニモジュラ行列 (det = ±1) * * 整数型 (int, long long, etc.) での厳密計算。 */ template struct SmithNormalFormResult { Matrix U; ///< 左ユニモジュラ行列 Matrix D; ///< 対角行列 (不変因子) Matrix V; ///< 右ユニモジュラ行列 std::vector invariant_factors; ///< 不変因子 d_1, d_2, ..., d_r std::size_t rank; ///< ランク (非ゼロ不変因子の数) }; /** * @brief スミス標準形 (Smith Normal Form) * * 整数行列 A (m×n) を行/列の基本変換で対角形に変換する。 * A = U * D * V (U, V: ユニモジュラ, D: 対角) * * アルゴリズム: 拡張ユークリッド互除法による逐次対角化 * * @param A 入力行列 (整数要素) * @return SmithNormalFormResult * @throws DimensionError 空行列 */ template SmithNormalFormResult smith_normal_form(const Matrix& A) { std::size_t m = A.rows(), n = A.cols(); if (m == 0 || n == 0) throw DimensionError("smith_normal_form: requires a non-empty matrix"); // 作業コピー 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) { // ピボット選択: D(k:m, k:n) の中で絶対値最小の非ゼロ要素を探す bool found_nonzero = true; while (found_nonzero) { // 最小非ゼロ要素をピボットに持ってくる 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; // 残りは全てゼロ // ピボットを (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)); // D の列交換 → V_new = C^{-1}*V, C は列置換 → V の行交換 for (std::size_t j = 0; j < n; ++j) std::swap(V(k, j), V(pj, j)); } // D(k,k) が負なら行を符号反転 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); // 列 k の消去: D(i, k) を 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; // 行 i -= q * 行 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; } // 行 k の消去: D(k, j) を 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; // 列 j -= q * 列 k → V の行 j += q * 行 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; // 余りが出た → 再度ループ } else { // 列と行の k+1 以降がゼロか確認 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; } } } // 整除条件の保証: d_i | d_{i+1} // 拡張 GCD + 2×2 行/列変換で 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; // D の対角要素を直接更新 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 (行 i, i+1 に対する 2×2 変換) // 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; } } // 対角要素を正に正規化 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); } } // 不変因子を抽出 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 ジョルダン標準形の結果 * * A = P * J * P^{-1} * J はジョルダンブロック対角行列 (実数近似) * * 注意: 数値計算では不安定。教育用・小行列向け。 */ template struct JordanResult { Matrix J; ///< ジョルダン行列 Matrix P; ///< 変換行列 (一般化固有ベクトル) std::vector eigenvalues; ///< 固有値 (実部のみ、重複あり) std::vector block_sizes; ///< 各ジョルダンブロックのサイズ }; namespace detail_jordan { /** * @brief Schur 形式から固有値を抽出 (1×1 実ブロック, 2×2 複素ブロック) * @return 固有値リスト (実部のみ), 対応する代数的重複度 */ template std::vector> extract_eigenvalues( const Matrix& 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 ブロック: 複素固有値 → 実部を 2 回追加 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; } } // グループ化 (近い固有値をまとめる) 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 行列の核空間の次元 (ランク落ち) を計算 */ template std::size_t null_space_dim(const Matrix& 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 (A - λI)^k の核空間の基底を SVD で計算 * @return 核空間の基底ベクトル (列ベクトル) */ template Matrix null_space_basis(const Matrix& 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)); // V の最後の null_dim 列 = Vt の最後の null_dim 行の転置 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 行列のべき乗 M^k を計算 */ template Matrix mat_power(const Matrix& M, std::size_t k) { 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) * * 正方行列 A を A = P * J * P^{-1} に分解する。 * J はジョルダンブロック対角行列。 * * 数値的には不安定なため、教育用・小行列 (n ≤ ~20) 向け。 * 複素固有値は実部のみで近似する。 * * @param A 入力行列 (n×n, 正方) * @return JordanResult * @throws DimensionError 空行列、非正方行列 */ template JordanResult jordan_normal_form(const Matrix& A) { std::size_t n = A.rows(); if (n == 0 || A.cols() != n) 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); // 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} }; } // Schur 分解で固有値を取得 auto [Sch, Q] = schur_decomposition(A); auto eig_groups = detail_jordan::extract_eigenvalues(Sch, tol); // 各固有値に対してジョルダンチェーンを構成 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; // (A - λI)^k の核空間次元を計算して // ジョルダンブロックの構造を決定 // null_dim(k) - null_dim(k-1) = k 以上のサイズのブロック数 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); // 代数的重複度を超えない null_dims.push_back(nd); if (nd >= alg_mult) break; } // ジョルダンブロック構造を導出 // Δ_k = null_dim(k) - null_dim(k-1) // ブロックサイズ 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}); } // ブロックサイズが決まらない場合のフォールバック std::size_t total_in_blocks = 0; for (auto& [sz, cnt] : blocks) total_in_blocks += sz * cnt; if (total_in_blocks < alg_mult) { // 残りを 1×1 ブロックで埋める 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}); } // ジョルダンチェーンの基底ベクトルを構成 // 各ブロックサイズ s に対して: // (A-λI)^s の核空間からチェーンの先頭ベクトルを選び、 // v, (A-λI)v, (A-λI)²v, ... で基底を構成 std::vector> chain_vectors; // 使用済み方向を管理するための直交射影 std::vector> used_vectors; // ブロックサイズ大→小の順で処理 (blocks はすでに大→小) 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); // 前の段階で使用済みの方向を除外するため、 // null_basis から used_vectors への射影を引く for (std::size_t b = 0; b < cnt; ++b) { // null_basis の列から、used_vectors と直交する方向を探す Vector seed(n, T(0)); bool found = false; for (std::size_t ci = 0; ci < null_basis.cols(); ++ci) { // 候補ベクトル Vector v(n, T(0)); for (std::size_t i = 0; i < n; ++i) v[i] = null_basis(i, ci); // 使用済みベクトルへの射影を引く (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]; } } // ノルムチェック T norm = T(0); for (std::size_t i = 0; i < n; ++i) norm += v[i] * v[i]; if (norm > tol * tol) { // 正規化 norm = std::sqrt(norm); for (std::size_t i = 0; i < n; ++i) v[i] /= norm; seed = v; found = true; break; } } if (!found) { // フォールバック: 単位ベクトルから選ぶ 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; } } } // ジョルダンチェーン: v_s = seed, v_{s-1} = (A-λI)*v_s, ... // 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; } // P に格納し、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); // 使用済みベクトルを記録 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 分解 (一般化実 Schur 分解) //===================================================================== namespace detail_qz { // Givens 回転パラメータ: 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}; } // 左 Givens 回転: 行 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; } } // 右 Givens 回転: 列 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 分解 (一般化実 Schur 分解) * * A = Q·S·Zᵀ, B = Q·T·Zᵀ * S: 準上三角 (実 Schur 形式) * T: 上三角 * Q, Z: 直交行列 * * 一般化固有値 λᵢ = S(i,i)/T(i,i) (1×1 ブロック) * 複素共役対は S の 2×2 ブロックに対応 * * アルゴリズム: * Phase 1: Hessenberg-Triangular 化 (Moler-Stewart) * Phase 2: Francis ダブルシフト QZ 反復 * * @tparam T 要素の型 * @param A 入力行列 (n×n) * @param B 入力行列 (n×n) * @return {S, T, Q, Z} */ template std::tuple, Matrix, Matrix, Matrix> qz_decomposition(const Matrix& A, const Matrix& B) { const int n = static_cast(A.rows()); if (A.rows() != A.cols() || B.rows() != B.cols() || A.rows() != B.rows()) { 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 化 // ============================================================ // Step 1a: B の QR 分解 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; } } // Q, Z を初期化 // Q = Q0 (左直交変換の蓄積) Matrix Q = Q0; Matrix Z(n, n, T(0)); for (int i = 0; i < n; ++i) Z(i, i) = T(1); // Step 1b: H を上 Hessenberg に縮約 (R の上三角性を維持) for (int k = 0; k < n - 2; ++k) { for (int i = n - 1; i >= k + 2; --i) { // 左 Givens: H(i, k) をゼロ化 (行 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); // Q 蓄積: Q ← Q · G (右から) detail_qz::apply_givens_right(Q, i - 1, i, c, s, 0, n); // R(i, i-1) に fill-in → 右 Givens でゼロ化 (列 i-1, i) auto [c2, s2] = detail_qz::givens_rotation(R(i, i), R(i, i - 1)); // 列 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); // Z 蓄積: Z ← Z · G (右から) detail_qz::apply_givens_right(Z, i, i - 1, c2, s2, 0, n); } } // 微小値クリーンアップ 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: 単一シフト QZ 反復 (Givens ベース) // ============================================================ 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) { // デフレーション: H のサブ対角要素が十分小さい箇所を探す 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 ブロック: デフレート --ihi; iter_since_deflation = 0; continue; } if (ilo == ihi - 2) { // 2×2 ブロック 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)) { // 実固有値: Wilkinson シフトで 1 回 QZ ステップ 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: 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; } // 単一シフト QZ ステップ // Wilkinson シフト: 右下 2×2 の M = R⁻¹H の固有値のうち // 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); // 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 { // 複素固有値: 実部をシフトに使用 sigma = tr / T(2); } // 例外シフト 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; } } // 初期 Givens: (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) { // 左 Givens: 行 (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); // 右 Givens: 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); // 次のステップ用: バルジ (H(k+2, k) が非ゼロ) if (k + 2 < ihi) { x = H(k + 1, k); y = H(k + 2, k); } } ++total_iter; ++iter_since_deflation; } // 微小値クリーンアップ 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 岩澤分解の結果 * * GL(n,R) の正則行列 G を G = K * A * N に分解する。 * - K: n×n 直交行列 (最大コンパクト部分群 O(n)) * - A: n×n 正の対角行列 (アーベル部分群) * - N: n×n 上三角冪単行列 (対角 = 1, 冪零部分群) * * アルゴリズム: QR 分解 G = Q*R の R から対角符号を分離 * K = Q*D, A = |diag(R)|, N = A^{-1}*D*R (D = diag(sign(R_ii))) */ template struct IwasawaResult { Matrix K; ///< 直交行列 Matrix A; ///< 正の対角行列 Matrix N; ///< 上三角冪単行列 (対角 = 1) }; /** * @brief 岩澤分解 (Iwasawa Decomposition) * * 正則行列 G (n×n) を G = K * A * N に分解する。 * * @param G 入力行列 (正則な正方行列) * @return IwasawaResult * @throws DimensionError 空行列または非正方行列 * @throws MathError 特異行列 */ template IwasawaResult iwasawa_decomposition(const Matrix& G) { std::size_t n = G.rows(); if (n == 0) throw DimensionError("iwasawa_decomposition: requires a non-empty matrix"); if (n != G.cols()) throw DimensionError("iwasawa_decomposition: requires a square matrix"); // QR 分解: G = Q * R auto [Q, R] = qr_decomposition(G); // 特異チェック: R の対角要素がゼロなら特異 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 (上三角冪単) Matrix K(Q); Matrix A(n, n, T(0)); Matrix N(n, n, T(0)); // 対角符号を Q に吸収 → K、対角絶対値 → 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 (対角), 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 分解 // ==================================================================== /** * @brief 列ピボット付き QR 分解の結果 * * A * P = Q * R (P は列置換行列) * rank() でランクを取得可能。 */ template struct ColPivQRResult { Matrix Q; ///< 直交行列 (m×m) Matrix R; ///< 上三角行列 (m×n) std::vector perm; ///< 列置換 (perm[j] = 元の列番号) /// 数値ランク (|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; } /// 置換行列 P を密行列で取得 [[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 列ピボット付き Householder QR 分解 * * 列ノルム最大の列をピボットとして選択し、ランク顕在化 QR を計算する。 * A * P = Q * R where P は列置換。 */ template ColPivQRResult col_piv_qr(const Matrix& a) { 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)); // 列ノルム (キャッシュ) 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) { // 残余列ノルムが最大の列を選択 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(R(i, step), R(i, best)); } // Householder 反射子 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) // 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; } // Q の更新: Q -= tau * Q * v * v^T (右から乗じる) 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; } // R の下三角をクリア (反射子ベクトルの保存場所を解放) for (std::size_t i = step + 1; i < m; ++i) R(i, step) = T(0); // 列ノルム更新 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) }; } /// 列ピボット付き QR で最小二乗解を求める (ランク落ちに対応) /// Q を明示的に構築せず、Householder ベクトルを保持して Q^T*b を直接計算 /// ブロック Householder (WY 表現) + AVX2 FMA template Vector col_piv_qr_solve(const Matrix& A, const Vector& b) { if (A.rows() != b.size()) 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)); // 列ノルム 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; } // サイズに応じてアンブロック/パネル分解を選択 constexpr std::size_t NB = 16; constexpr std::size_t PANEL_THRESHOLD = 384; if (n < PANEL_THRESHOLD) { // --- アンブロック版: 全後続列に直接適用 (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 { // --- パネル分解 (F 行列) + 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; } // 後続列をパネルに swap する場合: 先に lazy update を適用 if (best != step) { if (best >= panel_end && l > 0) { // best 列に累積 F を適用してから swap 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; // パネル内残り列に直接適用 { 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; } } // F 構築: 後続列 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; } } // ノルム更新: パネル列 for (std::size_t j = step + 1; j < panel_end; ++j) col_norms[j] -= Rp[step*n + j] * Rp[step*n + j]; // ノルム更新: 後続列 (仮想 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 (分岐なし最適化) if (panel_end < n) { // 三角部分: 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; } } // 矩形部分: rows [panel_end, m) — 全 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; } } } } } // ランク判定 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; } // Q^T * b を Householder 反射で直接計算 (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... 非連続 PT::load(qp + i), acc); // これは連続だが Rp は非連続 // Rp[i*n+step] は stride-n → AVX2 gather が必要 // → スカラーフォールバック 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; } // 後退代入 (AVX2 内積) 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]; } } // 置換の逆適用 Vector x(n, T(0)); for (std::size_t j = 0; j < n; ++j) x[perm[j]] = yp[j]; return x; } // ==================================================================== // FullPivLU — 完全ピボット LU 分解 // ==================================================================== /** * @brief 完全ピボット LU 分解の結果 * * P * A * Q = L * U (P は行置換, Q は列置換) */ template struct FullPivLUResult { Matrix LU; ///< L と U を合成 (対角は U) std::vector row_perm; ///< 行置換 std::vector col_perm; ///< 列置換 std::size_t pivots; ///< ピボット数 (符号計算用) /// 数値ランク [[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; } /// 行列式 (正方行列のみ) [[nodiscard]] T determinant() const { const auto n = LU.rows(); if (n != LU.cols()) 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; } /// L 行列を取得 [[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; } /// U 行列を取得 [[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 完全ピボット LU 分解 * * 行と列の両方でピボットを選択し、数値安定性を最大化する。 * P * A * Q = L * U */ template FullPivLUResult full_piv_lu(const Matrix& a) { 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) { // 全要素から最大値を探索 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; // 行交換 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; } // 列交換 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; } // 消去 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; // 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 }; } /// 完全ピボット LU で連立方程式を解く template Vector full_piv_lu_solve(const Matrix& A, const Vector& b) { const auto m = A.rows(); const auto n = A.cols(); if (m != n) throw DimensionError("full_piv_lu_solve: matrix must be square"); if (b.size() != m) throw DimensionError("full_piv_lu_solve: dimension mismatch"); auto result = full_piv_lu(A); const T* Lp = result.LU.data(); // P * b (行置換を適用) Vector pb(m); for (std::size_t i = 0; i < m; ++i) pb[i] = b[result.row_perm[i]]; T* pp = pb.data(); // 前方代入: 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; } // 後退代入: 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 (列置換の逆を適用) Vector x(n); for (std::size_t j = 0; j < n; ++j) x[result.col_perm[j]] = pb[j]; return x; } // ==================================================================== // CompleteOrthogonalDecomposition — 完全直交分解 // ==================================================================== /** * @brief 完全直交分解の結果 * * A * P = Q * [T 0; 0 0] * Z * ここで T は r×r 上三角, Q は m×m 直交, Z は n×n 直交, P は列置換。 * ランク落ち行列の最小ノルム最小二乗解に使用。 */ template struct CODResult { Matrix Q; ///< 直交行列 (m×m) Matrix T_mat; ///< 上三角 (r×r) Matrix Z; ///< 直交行列 (n×n) std::vector perm; ///< 列置換 std::size_t r; ///< ランク [[nodiscard]] std::size_t rank() const { return r; } }; /** * @brief 完全直交分解 * * ColPivQR の後、R の先頭 r 行に右 Householder を適用して * R[:r,:] = T * Z^T (T は r×r 上三角) にする。 */ template CODResult complete_orthogonal_decomposition(const Matrix& A) { 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) { // ランク 0 またはフルランク → 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 分解: R[:r,:] の右側 R[:r, r:] をゼロにする // 行 k (k = r-1 → 0) ごとに、ベクトル [R(k,k), R(k,r:n)] に // Householder を適用して 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; // Householder で [R(k,k), R(k,r), R(k,r+1), ...] → [sigma, 0, 0, ...] // 反射子ベクトル v は位置 k と 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] は位置 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); // Rwork の他の行に右から反射子を適用 (行 0..k-1) // row[k] += tau * (row[k] + sum(vtail[j]*row[r+j])) → 修正 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; } // Z の更新: Z の列 k と列 r:n に反射子を適用 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 }; } /// 完全直交分解で最小ノルム最小二乗解を求める /// Q, Z を明示構築せず Householder ベクトルで直接計算 template Vector cod_solve(const Matrix& A, const Vector& b) { if (A.rows() != b.size()) 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 (Householder ベクトルを R の下三角に保持) --- 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; } // サイズに応じてアンブロック/パネル分解を選択 constexpr std::size_t NB_COD = 16; constexpr std::size_t COD_PANEL_THRESHOLD = 384; if (n < COD_PANEL_THRESHOLD) { // --- アンブロック版 --- 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 { // --- パネル分解 (F 行列) + 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; } } } } } // ランク判定 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 (Householder 反射で直接計算) --- 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) { // フルランク: RZ 不要、T^{-1} * qtb + 置換逆 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 分解 + T^{-1} + Z 適用 (Householder ベクトル保持) --- // R[:r, :] の上三角部分を Rwork にコピー // (Rp の下三角には QR の Householder ベクトルがあるので別バッファ) 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]; // RZ Householder ベクトルと 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); // Rwork の他の行に右から反射子を適用 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]; } // Z * w を Householder 反射で計算 (逆順に適用) // Z = H_0 * H_1 * ... * H_{r-1} // Z * w = H_0(H_1(...(H_{r-1} * w)...)) // 各 H_k は位置 k と 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; } // 列置換の逆 Vector x(n, T(0)); for (std::size_t j = 0; j < n; ++j) x[perm[j]] = w[j]; return x; } // ==================================================================== // RealQZ — 一般化 Schur (QZ) 分解 // ==================================================================== /** * @brief QZ 分解の結果 * * Q^T * A * Z = S (quasi-upper triangular) * Q^T * B * Z = T (upper triangular) * 一般化固有値 = diag(S) / diag(T) */ template struct QZResult { Matrix S; ///< quasi-上三角 (実 Schur 形) Matrix TT; ///< 上三角 Matrix Q; ///< 左直交行列 Matrix Z; ///< 右直交行列 /// 一般化固有値 (alpha/beta ペア) [[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 ブロック → 複素固有値ペア 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 分解 (一般化 Schur 分解) * * 実装: Hessenberg-三角化 + QZ 反復 (単一シフト) * Q^T * A * Z = S, Q^T * B * Z = T */ template QZResult real_qz(const Matrix& A, const Matrix& B) { const auto n = A.rows(); if (n != A.cols() || n != B.rows() || n != B.cols()) 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 回転ヘルパー: 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}; }; // 左 Givens: 行 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; } }; // 右 Givens: 列 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: B を上三角化 (左 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: H を上 Hessenberg 化 (右 Givens + 左 Givens で R を再三角化) for (std::size_t k = 0; k + 2 < n; ++k) { for (std::size_t i = n - 1; i > k + 1; --i) { // H(i, k) をゼロに: 右 Givens (列 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); // R(i, i-1) をゼロに: 左 Givens (行 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 反復 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 チェック 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 シフト: 一般化固有値 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 ステップ: (H - shift * R) の先頭要素で Givens を開始 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); // 左 Givens (行 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); // R(k+1, k) をゼロに: 右 Givens (列 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 calx #endif // CALX_DECOMPOSITION_HPP