From 92220ea5a483d6ece73bb6472af0773bc4106d73 Mon Sep 17 00:00:00 2001
From: y-jan137 <yousefjan24000@gmail.com>
Date: Sun, 3 May 2026 19:08:14 +0300
Subject: Add some QR algos

---
 src/qr.cpp | 134 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 134 insertions(+)

(limited to 'src/qr.cpp')

diff --git a/src/qr.cpp b/src/qr.cpp
index 7e32ce3..a907bab 100644
--- a/src/qr.cpp
+++ b/src/qr.cpp
@@ -37,6 +37,24 @@ QRResult qr_modified_gs(const Matrix& A, double zero_tolerance = 1e-14);
 // Throws DimensionMismatchError if rows < cols.
 QRResult qr_householder(const Matrix& A);
 
+struct QRColPivResult {
+    Matrix Q;
+    Matrix R;
+    std::vector<std::size_t> perm;
+    std::size_t rank;
+};
+
+// Householder QR with column pivoting (rank-revealing).
+// At each step, the column with largest remaining norm is selected as pivot.
+// The numerical rank is determined by comparing diagonal entries of R to
+// rank_tolerance * |R(0,0)|.
+//
+// Returns Q (m x n), R (n x n upper triangular), perm (column permutation),
+// and rank (numerical rank estimate).
+//
+// Throws DimensionMismatchError if rows < cols.
+QRColPivResult qr_colpiv(const Matrix& A, double rank_tolerance = 1e-12);
+
 }  // namespace linalgebra
 
 namespace {
@@ -194,4 +212,120 @@ QRResult qr_householder(const Matrix& A) {
     return QRResult{std::move(Q), std::move(R)};
 }
 
+QRColPivResult qr_colpiv(const Matrix& A, double rank_tolerance) {
+    require_tall(A, "qr_colpiv");
+    const std::size_t m = A.rows();
+    const std::size_t n = A.cols();
+
+    Matrix work = A;
+    Matrix Q_full = Matrix::identity(m);
+
+    std::vector<std::size_t> perm(n);
+    std::iota(perm.begin(), perm.end(), std::size_t{0});
+
+    // Precompute column norms squared.
+    std::vector<double> col_norms_sq(n);
+    for (std::size_t j = 0; j < n; ++j) {
+        double s = 0.0;
+        for (std::size_t i = 0; i < m; ++i) s += work(i, j) * work(i, j);
+        col_norms_sq[j] = s;
+    }
+
+    std::size_t rank = n;
+
+    for (std::size_t k = 0; k < n; ++k) {
+        // Find pivot: column with largest remaining norm.
+        std::size_t pivot = k;
+        double max_norm = col_norms_sq[k];
+        for (std::size_t j = k + 1; j < n; ++j) {
+            if (col_norms_sq[j] > max_norm) {
+                max_norm = col_norms_sq[j];
+                pivot = j;
+            }
+        }
+
+        // Swap columns k and pivot.
+        if (pivot != k) {
+            for (std::size_t i = 0; i < m; ++i) {
+                std::swap(work(i, k), work(i, pivot));
+            }
+            std::swap(col_norms_sq[k], col_norms_sq[pivot]);
+            std::swap(perm[k], perm[pivot]);
+        }
+
+        // Householder reflector for column k.
+        const std::size_t p = m - k;
+
+        std::vector<double> u(p);
+        for (std::size_t i = 0; i < p; ++i) u[i] = work(k + i, k);
+
+        const double x_norm = [&] {
+            double s = 0.0;
+            for (double v : u) s += v * v;
+            return std::sqrt(s);
+        }();
+
+        if (x_norm == 0.0) {
+            // Remaining columns are zero — rank determined.
+            rank = k;
+            break;
+        }
+
+        // Check rank: if this pivot norm is small relative to R(0,0).
+        if (k > 0) {
+            const double r00 = std::abs(work(0, 0));
+            if (x_norm <= rank_tolerance * r00) {
+                rank = k;
+                break;
+            }
+        }
+
+        const double sigma = (u[0] >= 0.0 ? 1.0 : -1.0) * x_norm;
+        u[0] += sigma;
+
+        const double utu = [&] {
+            double s = 0.0;
+            for (double v : u) s += v * v;
+            return s;
+        }();
+        const double tau = 2.0 / utu;
+
+        // Apply reflector to work columns k..n-1.
+        for (std::size_t j = k; j < n; ++j) {
+            double d = 0.0;
+            for (std::size_t i = 0; i < p; ++i) d += u[i] * work(k + i, j);
+            const double coeff = tau * d;
+            for (std::size_t i = 0; i < p; ++i) work(k + i, j) -= coeff * u[i];
+        }
+
+        // Apply reflector to Q_full.
+        for (std::size_t j = 0; j < m; ++j) {
+            double d = 0.0;
+            for (std::size_t i = 0; i < p; ++i) d += u[i] * Q_full(k + i, j);
+            const double coeff = tau * d;
+            for (std::size_t i = 0; i < p; ++i) Q_full(k + i, j) -= coeff * u[i];
+        }
+
+        // Update column norms (downdate).
+        for (std::size_t j = k + 1; j < n; ++j) {
+            const double val = work(k, j);
+            col_norms_sq[j] -= val * val;
+            if (col_norms_sq[j] < 0.0) col_norms_sq[j] = 0.0;
+        }
+    }
+
+    // Extract Q (m x n) and R (n x n).
+    Matrix Q(m, n);
+    for (std::size_t i = 0; i < m; ++i)
+        for (std::size_t j = 0; j < n; ++j)
+            Q(i, j) = Q_full(j, i);
+
+    Matrix R(n, n);
+    for (std::size_t i = 0; i < n; ++i)
+        for (std::size_t j = 0; j < n; ++j)
+            R(i, j) = work(i, j);
+
+    return QRColPivResult{std::move(Q), std::move(R), std::move(perm), rank};
+}
+
 }  // namespace linalgebra
-- 
cgit v1.2.3