From 6f91c96de888f22998933d3b4ea336f1a4d5af8a Mon Sep 17 00:00:00 2001 From: Cynopolis Date: Fri, 14 Aug 2026 11:02:25 -0400 Subject: [PATCH] Small refactor of SVD implimentation --- src/SVD.cpp | 319 +++++++++++--------------- src/SVD.hpp | 44 ++++ unit-tests/svd-build-blocks-tests.cpp | 242 +++++++++++++++++++ 3 files changed, 426 insertions(+), 179 deletions(-) diff --git a/src/SVD.cpp b/src/SVD.cpp index f63eb92..05be46f 100644 --- a/src/SVD.cpp +++ b/src/SVD.cpp @@ -150,6 +150,103 @@ void SVD::ApplyGivensRight(Matrix<5, 5> &W, uint8_t i, uint8_t j, float c, } } +// ============================================================================ +// Phase 3: Extract and Sort Singular Values +// ============================================================================ + +void SVD::ExtractAndSortSingularValues(Matrix<5, 5> &W, + Matrix<5, 1> &sigma, + uint8_t p, + Matrix<5, 5> &QL, + Matrix<5, 5> &QR) { + // Extract singular values as absolute values of diagonal elements + for (uint8_t i = 0; i < p; i++) { + sigma[i][0] = fabsf(W[i][i]); + } + + // Sort singular values in descending order and reorder U, V accordingly + for (uint8_t i = 0; i < p - 1; i++) { + for (uint8_t j = i + 1; j < p; j++) { + if (sigma[j][0] > sigma[i][0]) { + // Swap singular values + float tmpS = sigma[i][0]; + sigma[i][0] = sigma[j][0]; + sigma[j][0] = tmpS; + + // Swap columns of QL + for (uint8_t k = 0; k < 5; k++) { + float tmpQ = QL[k][i]; + QL[k][i] = QL[k][j]; + QL[k][j] = tmpQ; + } + + // Swap columns of QR + for (uint8_t k = 0; k < 5; k++) { + float tmpQ = QR[k][i]; + QR[k][i] = QR[k][j]; + QR[k][j] = tmpQ; + } + } + } + } +} + +// ============================================================================ +// Phase 4: Assemble Final U and Vt Matrices +// ============================================================================ + +void SVD::AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p, + bool transposeNeeded, + const Matrix<5, 5> &QL, + const Matrix<5, 5> &QR, + Matrix<5, 5> &U, + Matrix<5, 5> &Vt) { + // Initialize output matrices to zero + for (uint8_t i = 0; i < 5; i++) + for (uint8_t j = 0; j < 5; j++) { + U[i][j] = 0; + Vt[i][j] = 0; + } + + // ---- Compute Final U and Vt ---- + // If transposeNeeded (wide matrix), we computed SVD(Aᵀ) = Ũ·Σ·Ṽᵀ + // Then U = Ṽ (= QR[:,0:p]) and Vt = Ũᵀ (= QL[:,0:p]ᵀ) + // Otherwise, SVD(A) = QL[:,0:p] · Σ · (QR[:,0:p])ᵀ + // So U = QL[:,0:p] and Vt = QR[:,0:p]ᵀ + + for (uint8_t i = 0; i < m; i++) { + for (uint8_t j = 0; j < n; j++) { + if (j < p) { + if (transposeNeeded) { + // U = QR[:, 0:p]ᵀ → U[i][j] = QR[j][i] + U[i][j] = QR.Get(j, i); + } else { + // U = QL[:, 0:p] + U[i][j] = QL.Get(i, j); + } + } else { + U[i][j] = 0; + } + } + } + + for (uint8_t i = 0; i < n; i++) { + for (uint8_t j = 0; j < m; j++) { + if (i < p && j < m) { + if (transposeNeeded) { + // Vt = QL[:, 0:p]ᵀ → Vt[i][j] = QL[j][i] + Vt[i][j] = QL.Get(j, i); + } else { + // Vt = QR[:, 0:p]ᵀ → Vt[i][j] = QR[j][i] + Vt[i][j] = QR.Get(j, i); + } + } else { + Vt[i][j] = 0; + } + } + } +} + // ============================================================================ // SVD Implementation - Golub-Kahan-Reinsch Algorithm // ============================================================================ @@ -224,122 +321,45 @@ void SVD::SVD(Matrix &matrixToDecompose, x[i] = W[k + i][k]; } - // Compute Householder reflection: H·x = [α, 0, ..., 0]ᵀ - float norm = 0; - for (uint8_t i = 0; i < len; i++) - norm += x[i] * x[i]; - norm = sqrtf(norm); + // Use building block to compute Householder reflector + float alpha; + SVD::ComputeHouseholder(x, len, hhVec, alpha); - if (norm < 1e-30f) + if (alpha == 0.0f) continue; - float alpha = (x[0] >= 0) ? -norm : norm; - float v0 = x[0] - alpha; - float vv = v0 * v0 + norm * norm - alpha * x[0]; - if (vv < 1e-30f) - continue; - - float scale = 1.0f / sqrtf(vv); - // Store Householder vector (first element is implicit 1, rest in hhVec) - hhVec[0] = - v0 * scale; // this is the first element of the reflected vector - for (uint8_t i = 1; i < len; i++) { - hhVec[i] = x[i] * scale; - } - // Apply H from left to W: W = H·W - // For each column j, w[k+i][j] -= 2·v_i·(vᵀ·w_col) / (vᵀv) - float vvNorm = 1.0f + hhVec[0] * hhVec[0]; - for (uint8_t i = 1; i < len; i++) { - vvNorm += hhVec[i] * hhVec[i]; - } - - for (uint8_t j = k; j < n; j++) { - float dot = 0; - for (uint8_t i = 0; i < len; i++) { - dot += hhVec[i] * W[k + i][j]; - } - dot *= 2.0f / vvNorm; - for (uint8_t i = 0; i < len; i++) { - W[k + i][j] -= dot * hhVec[i]; - } - } + SVD::ApplyHouseholderLeft(W, hhVec, k, k + len - 1); // Apply H from right to QL: QL = QL · H - for (uint8_t j = k; j < m; j++) { - float dot = 0; - for (uint8_t i = 0; i < len; i++) { - dot += hhVec[i] * QL[j][k + i]; - } - dot *= 2.0f / vvNorm; - for (uint8_t i = 0; i < len; i++) { - QL[j][k + i] -= dot * hhVec[i]; - } - } + SVD::ApplyHouseholderRight(QL, hhVec, k, k + len - 1); } - // --- Right Householder on row k, columns k+1..min(m,n)-1 --- + // --- Right Householder on row k, columns k+2..min(m,n)-1 --- + // (column k+1 is the first superdiagonal element, preserved in bidiagonal form) { - uint8_t len = (p > 1) ? p - 1 - k : 0; + uint8_t len = (p > 1) ? p - 2 - k : 0; if (len <= 0) continue; - // Extract the row segment + // Extract the row segment starting from column k+2 float x[5]; for (uint8_t i = 0; i < len; i++) { - x[i] = W[k][k + 1 + i]; + x[i] = W[k][k + 2 + i]; } - // Compute Householder reflection - float norm = 0; - for (uint8_t i = 0; i < len; i++) - norm += x[i] * x[i]; - norm = sqrtf(norm); + // Use building block to compute Householder reflector + float alpha; + SVD::ComputeHouseholder(x, len, hhVec, alpha); - if (norm < 1e-30f) + if (alpha == 0.0f) continue; - float alpha = (x[0] >= 0) ? -norm : norm; - float v0 = x[0] - alpha; - float vv = v0 * v0 + norm * norm - alpha * x[0]; - if (vv < 1e-30f) - continue; - - float scale = 1.0f / sqrtf(vv); - hhVec[0] = v0 * scale; - for (uint8_t i = 1; i < len; i++) { - hhVec[i] = x[i] * scale; - } - - // Compute vᵀv - float vvNorm = 1.0f + hhVec[0] * hhVec[0]; - for (uint8_t i = 1; i < len; i++) { - vvNorm += hhVec[i] * hhVec[i]; - } - // Apply H from right to W: W = W·H - for (uint8_t i = 0; i < m; i++) { - float dot = 0; - for (uint8_t j = 0; j < len; j++) { - dot += hhVec[j] * W[i][k + 1 + j]; - } - dot *= 2.0f / vvNorm; - for (uint8_t j = 0; j < len; j++) { - W[i][k + 1 + j] -= dot * hhVec[j]; - } - } + SVD::ApplyHouseholderRight(W, hhVec, k + 2, k + 1 + len); // Apply H from right to QR: QR = QR · H - for (uint8_t i = 0; i < n; i++) { - float dot = 0; - for (uint8_t j = 0; j < len; j++) { - dot += hhVec[j] * QR[i][k + 1 + j]; - } - dot *= 2.0f / vvNorm; - for (uint8_t j = 0; j < len; j++) { - QR[i][k + 1 + j] -= dot * hhVec[j]; - } - } + SVD::ApplyHouseholderRight(QR, hhVec, k + 2, k + 1 + len); } } @@ -425,32 +445,15 @@ void SVD::SVD(Matrix &matrixToDecompose, float y = (start > 0) ? W[start][start - 1] : 0.0f; for (uint8_t i = start; i <= end; i++) { - float r = sqrtf(x * x + y * y); - if (r < 1e-30f) { - x = W[i][i]; - y = (i < end) ? W[i + 1][i] : 0.0f; - continue; - } - - float cs = x / r; - float sn = y / r; + float cs, sn; + SVD::ComputeGivens(x, y, cs, sn); // Apply Givens from left to rows i, i+1 of W (columns i..p-1) - for (uint8_t j = i; j < p; j++) { - float t1 = W[i][j]; - float t2 = W[i + 1][j]; - W[i][j] = cs * t1 + sn * t2; - W[i + 1][j] = -sn * t1 + cs * t2; - } + SVD::ApplyGivensLeft(W, i, i + 1, cs, sn, i, p - 1); // Apply Givens from right to columns i, i+1 of W (rows 0..i) if (i > start) { - for (uint8_t j = 0; j <= i; j++) { - float t1 = W[j][i]; - float t2 = W[j][i + 1]; - W[j][i] = cs * t1 + sn * t2; - W[j][i + 1] = -sn * t1 + cs * t2; - } + SVD::ApplyGivensRight(W, i, i + 1, cs, sn, 0, i); } // Accumulate right transformations into QR @@ -462,77 +465,35 @@ void SVD::SVD(Matrix &matrixToDecompose, } // Prepare next Givens rotation - x = W[i + 1][i]; - y = (i + 1 < end) ? W[i + 1][i + 1] : 0.0f; - } - } - - // ---- Phase 3: Extract Results ---- - // Singular values are the absolute values of diagonal elements of W - for (uint8_t i = 0; i < p; i++) { - sigma[i][0] = fabsf(W[i][i]); - } - - // Sort singular values in descending order and reorder U, V accordingly - for (uint8_t i = 0; i < p - 1; i++) { - for (uint8_t j = i + 1; j < p; j++) { - if (sigma[j][0] > sigma[i][0]) { - float tmpS = sigma[i][0]; - sigma[i][0] = sigma[j][0]; - sigma[j][0] = tmpS; - - // Swap columns of QL - for (uint8_t k = 0; k < 5; k++) { - float tmpQ = QL[k][i]; - QL[k][i] = QL[k][j]; - QL[k][j] = tmpQ; - } - - // Swap columns of QR - for (uint8_t k = 0; k < 5; k++) { - float tmpQ = QR[k][i]; - QR[k][i] = QR[k][j]; - QR[k][j] = tmpQ; - } + if (i + 1 <= end) { + x = W[i + 1][i]; + y = (i + 1 < end) ? W[i + 1][i + 1] : 0.0f; } } } - // ---- Phase 4: Compute Final U and Vt ---- - // If transposeNeeded (wide matrix), we computed SVD(Aᵀ) = Ũ·Σ·Ṽᵀ - // Then U = Ṽ (= QR[:,0:p]) and Vt = Ũᵀ (= QL[:,0:p]ᵀ) - // Otherwise, SVD(A) = QL[:,0:p] · Σ · (QR[:,0:p])ᵀ - // So U = QL[:,0:p] and Vt = QR[:,0:p]ᵀ + // ---- Phase 3: Extract and Sort Singular Values ---- + // Use internal 5×1 buffer for sigma + Matrix<5, 1> sigmaInternal{0}; + ExtractAndSortSingularValues(W, sigmaInternal, p, QL, QR); - for (uint8_t i = 0; i < m; i++) { - for (uint8_t j = 0; j < n; j++) { - if (j < p) { - if (transposeNeeded) { - // U = QR[:, 0:p]ᵀ → U[i][j] = QR[j][i] - U[i][j] = QR[j][i]; - } else { - // U = QL[:, 0:p] - U[i][j] = QL[i][j]; - } - } else { - U[i][j] = 0; - } + // ---- Phase 4: Assemble Final U and Vt ---- + // Use internal 5×5 buffers for U and Vt + Matrix<5, 5> UInternal{0}, VtInternal{0}; + AssembleUAndVt(m, n, p, transposeNeeded, QL, QR, UInternal, VtInternal); + + // Copy results to output parameters + for (uint8_t i = 0; i < columns; i++) { + sigma[i][0] = sigmaInternal.Get(i, 0); + } + for (uint8_t i = 0; i < rows; i++) { + for (uint8_t j = 0; j < columns; j++) { + U[i][j] = UInternal.Get(i, j); } } - - for (uint8_t i = 0; i < n; i++) { - for (uint8_t j = 0; j < m; j++) { - if (i < p && j < m) { - if (transposeNeeded) { - // Vt = QL[:, 0:p]ᵀ → Vt[i][j] = QL[j][i] - Vt[i][j] = QL[j][i]; - } else { - // Vt = QR[:, 0:p]ᵀ → Vt[i][j] = QR[j][i] - Vt[i][j] = QR[j][i]; - } - } else { - Vt[i][j] = 0; - } + for (uint8_t i = 0; i < columns; i++) { + for (uint8_t j = 0; j < rows; j++) { + Vt[i][j] = VtInternal.Get(i, j); } } } diff --git a/src/SVD.hpp b/src/SVD.hpp index 922a230..8b81fd4 100644 --- a/src/SVD.hpp +++ b/src/SVD.hpp @@ -78,6 +78,50 @@ static void ApplyHouseholderLeft(Matrix<5, 5> &W, const float *v, static void ApplyHouseholderRight(Matrix<5, 5> &W, const float *v, uint8_t startCol, uint8_t endCol); +/** + * @brief Extract singular values from bidiagonal matrix diagonal and sort. + * + * Extracts absolute values of diagonal elements of W as singular values, + * then sorts them in descending order while reordering columns of QL + * and QR to maintain consistency. + * + * @param W Input: bidiagonal matrix (5×5 working array) + * @param sigma Output: sorted singular values (5×1 column vector, only first p used) + * @param p Number of singular values (min(rows, columns)) + * @param QL Input/output: left transformation matrix (modified during sort) + * @param QR Input/output: right transformation matrix (modified during sort) + */ +static void ExtractAndSortSingularValues(Matrix<5, 5> &W, + Matrix<5, 1> &sigma, + uint8_t p, + Matrix<5, 5> &QL, + Matrix<5, 5> &QR); + +/** + * @brief Assemble final U and Vt matrices from QL/QR. + * + * Computes the final left singular vectors (U) and right singular vectors + * transposed (Vt) from the accumulated Householder transformations. + * + * For non-transpose case: U = QL[:,0:p], Vt = QR[:,0:p]ᵀ + * For transpose case: U = QR[:,0:p]ᵀ, Vt = QL[:,0:p]ᵀ + * + * @param m Number of rows in original matrix + * @param n Number of columns in original matrix + * @param p Rank = min(m, n) + * @param transposeNeeded True if we computed SVD(Aᵀ) instead of SVD(A) + * @param QL Left Householder accumulation (5×5) + * @param QR Right Householder accumulation (5×5) + * @param U Output: left singular vectors (m×n matrix, only first p columns used) + * @param Vt Output: right singular vectors transposed (n×n matrix, only first p rows used) + */ +static void AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p, + bool transposeNeeded, + const Matrix<5, 5> &QL, + const Matrix<5, 5> &QR, + Matrix<5, 5> &U, + Matrix<5, 5> &Vt); + /** * @brief Compute a Givens rotation that zeros out y. * diff --git a/unit-tests/svd-build-blocks-tests.cpp b/unit-tests/svd-build-blocks-tests.cpp index 857ee54..c01f12a 100644 --- a/unit-tests/svd-build-blocks-tests.cpp +++ b/unit-tests/svd-build-blocks-tests.cpp @@ -783,3 +783,245 @@ TEST_CASE("SVD Building Block: Givens preserves orthogonality", REQUIRE(isOrthogonal5(M_right)); } } + +// ============================================================================ +// TEST 11: ExtractAndSortSingularValues (Phase 3) +// =========================================================================== +TEST_CASE("SVD Phase 3: ExtractAndSortSingularValues", "[Matrix][SVD]") { + // Test case 1: Diagonal matrix with unordered singular values + { + Matrix<5, 5> W{0}; + W[0][0] = 2.0f; + W[1][1] = 10.0f; + W[2][2] = 5.0f; + + Matrix<5, 1> sigma{0}; + Matrix<5, 5> QL{0}, QR{0}; + for (uint8_t i = 0; i < 5; i++) { + QL[i][i] = 1.0f; + QR[i][i] = 1.0f; + } + + SVD::ExtractAndSortSingularValues(W, sigma, 3, QL, QR); + + // Singular values should be sorted descending: [10, 5, 2] + REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(10.0f, 1e-6f)); + REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(5.0f, 1e-6f)); + REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(2.0f, 1e-6f)); + + // QL and QR columns should have been swapped to match the sort order + // Original: col 0 → σ=2, col 1 → σ=10, col 2 → σ=5 + // After sort: col 0 has σ=10 (was orig col 1), col 1 has σ=5 (was orig col 2), + // col 2 has σ=2 (was orig col 0) + // Starting from identity: QL[:,0] = e₀, QL[:,1] = e₁, QL[:,2] = e₂ + // After swaps: QL[:,0] = e₁, QL[:,1] = e₂, QL[:,2] = e₀ + REQUIRE_THAT(QL.Get(0, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(QL.Get(1, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + REQUIRE_THAT(QL.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(QL.Get(0, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(QL.Get(1, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(QL.Get(2, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + REQUIRE_THAT(QL.Get(0, 2), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + REQUIRE_THAT(QL.Get(1, 2), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(QL.Get(2, 2), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + } + + // Test case 2: Negative diagonal elements (absolute value extraction) + { + Matrix<5, 5> W{0}; + W[0][0] = -3.0f; + W[1][1] = -7.0f; + W[2][2] = 5.0f; + + Matrix<5, 1> sigma{0}; + Matrix<5, 5> QL{0}, QR{0}; + for (uint8_t i = 0; i < 5; i++) { + QL[i][i] = 1.0f; + QR[i][i] = 1.0f; + } + + SVD::ExtractAndSortSingularValues(W, sigma, 3, QL, QR); + + // Should extract absolute values and sort: [7, 5, 3] + REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(7.0f, 1e-6f)); + REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(5.0f, 1e-6f)); + REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(3.0f, 1e-6f)); + } + + // Test case 3: Already sorted (no swaps needed) + { + Matrix<5, 5> W{0}; + W[0][0] = 9.0f; + W[1][1] = 6.0f; + W[2][2] = 3.0f; + + Matrix<5, 1> sigma{0}; + Matrix<5, 5> QL{0}, QR{0}; + for (uint8_t i = 0; i < 5; i++) { + QL[i][i] = 1.0f; + QR[i][i] = 1.0f; + } + + SVD::ExtractAndSortSingularValues(W, sigma, 3, QL, QR); + + REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(9.0f, 1e-6f)); + REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(6.0f, 1e-6f)); + REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(3.0f, 1e-6f)); + + // QL and QR should be unchanged (identity) + for (uint8_t i = 0; i < 5; i++) { + for (uint8_t j = 0; j < 5; j++) { + float expected = (i == j) ? 1.0f : 0.0f; + REQUIRE_THAT(QL.Get(i, j), Catch::Matchers::WithinRel(expected, 1e-6f)); + REQUIRE_THAT(QR.Get(i, j), Catch::Matchers::WithinRel(expected, 1e-6f)); + } + } + } + + // Test case 4: Single singular value + { + Matrix<5, 5> W{0}; + W[0][0] = 42.0f; + + Matrix<5, 1> sigma{0}; + Matrix<5, 5> QL{0}, QR{0}; + for (uint8_t i = 0; i < 5; i++) { + QL[i][i] = 1.0f; + QR[i][i] = 1.0f; + } + + SVD::ExtractAndSortSingularValues(W, sigma, 1, QL, QR); + + REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(42.0f, 1e-6f)); + } +} + +// ============================================================================ +// TEST 12: AssembleUAndVt (Phase 4) +// =========================================================================== +TEST_CASE("SVD Phase 4: AssembleUAndVt", "[Matrix][SVD]") { + // Test case 1: Non-transpose case (m ≥ n) — U from QL, Vt from QRᵀ + { + uint8_t m = 3, n = 2, p = 2; + bool transposeNeeded = false; + + Matrix<5, 5> QL{0}; + // Make columns orthonormal + QL[0][0] = 3.0f / 5.0f; + QL[1][0] = 4.0f / 5.0f; + QL[2][0] = 0.0f; + QL[0][1] = 4.0f / 5.0f; + QL[1][1] = -3.0f / 5.0f; + QL[2][1] = 0.0f; + + Matrix<5, 5> QR{0}; + QR[0][0] = 1.0f; // Vt[:,0]ᵀ + QR[1][0] = 0.0f; + QR[0][1] = 0.0f; // Vt[:,1]ᵀ + QR[1][1] = 1.0f; + + Matrix<5, 5> U{0}; + Matrix<5, 5> Vt{0}; + + SVD::AssembleUAndVt(m, n, p, transposeNeeded, QL, QR, U, Vt); + + // U should be QL[:,0:2] + REQUIRE_THAT(U.Get(0, 0), Catch::Matchers::WithinRel(3.0f / 5.0f, 1e-6f)); + REQUIRE_THAT(U.Get(1, 0), Catch::Matchers::WithinRel(4.0f / 5.0f, 1e-6f)); + REQUIRE_THAT(U.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(U.Get(0, 1), Catch::Matchers::WithinRel(4.0f / 5.0f, 1e-6f)); + REQUIRE_THAT(U.Get(1, 1), Catch::Matchers::WithinRel(-3.0f / 5.0f, 1e-6f)); + REQUIRE_THAT(U.Get(2, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + + // Vt should be QR[:,0:2]ᵀ + REQUIRE_THAT(Vt.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + REQUIRE_THAT(Vt.Get(0, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(Vt.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(Vt.Get(1, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + + // Verify U is orthogonal (first p columns) + Matrix<5, 5> Ut = U.Transpose(); + Matrix<5, 5> UtU{0}; + Ut.Mult(U, UtU); + REQUIRE_THAT(UtU.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + REQUIRE_THAT(UtU.Get(0, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(UtU.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(UtU.Get(1, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + } + + // Test case 2: Transpose case (m < n) — U from QRᵀ, Vt from QLᵀ + { + uint8_t m = 2, n = 3, p = 2; + bool transposeNeeded = true; + + Matrix<5, 5> QL{0}; + QL[0][0] = 1.0f; // Vt[:,0]ᵀ + QL[1][0] = 0.0f; + QL[0][1] = 0.0f; // Vt[:,1]ᵀ + QL[1][1] = 1.0f; + + Matrix<5, 5> QR{0}; + QR[0][0] = 3.0f / 5.0f; // U[:,0] + QR[1][0] = 4.0f / 5.0f; + QR[2][0] = 0.0f; + QR[0][1] = 4.0f / 5.0f; + QR[1][1] = -3.0f / 5.0f; + QR[2][1] = 0.0f; + + Matrix<5, 5> U{0}; + Matrix<5, 5> Vt{0}; + + SVD::AssembleUAndVt(m, n, p, transposeNeeded, QL, QR, U, Vt); + + // U should be QR[:,0:2]ᵀ → U[i][j] = QR[j][i] + REQUIRE_THAT(U.Get(0, 0), Catch::Matchers::WithinRel(3.0f / 5.0f, 1e-6f)); + REQUIRE_THAT(U.Get(0, 1), Catch::Matchers::WithinRel(4.0f / 5.0f, 1e-6f)); + REQUIRE_THAT(U.Get(1, 0), Catch::Matchers::WithinRel(4.0f / 5.0f, 1e-6f)); + REQUIRE_THAT(U.Get(1, 1), Catch::Matchers::WithinRel(-3.0f / 5.0f, 1e-6f)); + + // Vt should be QL[:,0:2]ᵀ → Vt[i][j] = QL[j][i] + REQUIRE_THAT(Vt.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + REQUIRE_THAT(Vt.Get(0, 1), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(Vt.Get(1, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f)); + REQUIRE_THAT(Vt.Get(1, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + + // Verify U has correct dimensions (m×n = 2×3) + REQUIRE(U.Get(0, 2) == 0.0f); + REQUIRE(U.Get(1, 2) == 0.0f); + + // Verify Vt is orthogonal (first p rows) + Matrix<5, 5> VtVtT{0}; + Vt.Mult(Vt.Transpose(), VtVtT); + REQUIRE_THAT(VtVtT.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + REQUIRE_THAT(VtVtT.Get(1, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + // Row 2 of Vt is all zeros (p=2 < n=3), so VtVtT[2][2] = 0 is expected + } + + // Test case 3: Square matrix (m = n) + { + uint8_t m = 2, n = 2, p = 2; + bool transposeNeeded = false; + + Matrix<5, 5> QL{0}; + QL[0][0] = 1.0f; QL[1][1] = 1.0f; + + Matrix<5, 5> QR{0}; + QR[0][0] = 0.6f; QR[0][1] = 0.8f; + QR[1][0] = 0.8f; QR[1][1] = -0.6f; + + Matrix<5, 5> U{0}; + Matrix<5, 5> Vt{0}; + + SVD::AssembleUAndVt(m, n, p, transposeNeeded, QL, QR, U, Vt); + + // U = QL[:,0:2] + REQUIRE_THAT(U.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + REQUIRE_THAT(U.Get(1, 1), Catch::Matchers::WithinRel(1.0f, 1e-6f)); + + // Vt = QR[:,0:2]ᵀ + REQUIRE_THAT(Vt.Get(0, 0), Catch::Matchers::WithinRel(0.6f, 1e-6f)); + REQUIRE_THAT(Vt.Get(0, 1), Catch::Matchers::WithinRel(0.8f, 1e-6f)); + REQUIRE_THAT(Vt.Get(1, 0), Catch::Matchers::WithinRel(0.8f, 1e-6f)); + REQUIRE_THAT(Vt.Get(1, 1), Catch::Matchers::WithinRel(-0.6f, 1e-6f)); + } +}