From 5e61832a9ba8166d6edddfc11b9b95f63c1343ef Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Thu, 28 Dec 2023 13:25:17 +0300
Subject: [PATCH] Add QR decomposition implementation & tests

---
 libs/geom/include/psemek/geom/qr.hpp | 62 ++++++++++++++++++++++++++++
 libs/geom/tests/matrix.cpp           | 27 ++++++++++++
 2 files changed, 89 insertions(+)
 create mode 100644 libs/geom/include/psemek/geom/qr.hpp

diff --git a/libs/geom/include/psemek/geom/qr.hpp b/libs/geom/include/psemek/geom/qr.hpp
new file mode 100644
index 00000000..3ec82838
--- /dev/null
+++ b/libs/geom/include/psemek/geom/qr.hpp
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <psemek/geom/matrix.hpp>
+
+namespace psemek::geom
+{
+
+	template <typename T, std::size_t N>
+	struct qr_result
+	{
+		matrix<T, N, N> q; // orthogonal
+		matrix<T, N, N> r; // upper-triangular
+	};
+
+	template <typename T, std::size_t N>
+	qr_result<T, N> qr_decomposition(matrix<T, N, N> const & m)
+	{
+		matrix<T, N, N> q = matrix<T, N, N>::identity();
+		matrix<T, N, N> r = m;
+
+		// Iterate columns
+		for (std::size_t i = 0; i < N; ++i)
+		{
+			// Iterate rows
+			for (std::size_t j = i + 1; j < N; ++j)
+			{
+				T const max = std::max(std::abs(r[i][i]), std::abs(r[j][i]));
+
+				if (max == T{})
+					continue;
+
+				// Compute Givens rotation
+				T const l = max * std::sqrt(sqr(r[i][i] / max) + sqr(r[j][i] / max));
+				T const c =   r[i][i] / l;
+				T const s = - r[j][i] / l;
+
+				// Apply rotation to rows of r
+				for (std::size_t k = i; k < N; ++k)
+				{
+					T const x = r[i][k];
+					T const y = r[j][k];
+
+					r[i][k] = x * c - y * s;
+					r[j][k] = x * s + y * c;
+				}
+
+				// Apply inverse rotation to columns of q
+				for (std::size_t k = 0; k < N; ++k)
+				{
+					T const x = q[k][i];
+					T const y = q[k][j];
+
+					q[k][i] = x * c - y * s;
+					q[k][j] = x * s + y * c;
+				}
+			}
+		}
+
+		return {q, r};
+	}
+
+}
diff --git a/libs/geom/tests/matrix.cpp b/libs/geom/tests/matrix.cpp
index 7b499622..2debf5f1 100644
--- a/libs/geom/tests/matrix.cpp
+++ b/libs/geom/tests/matrix.cpp
@@ -2,7 +2,11 @@
 
 #include <psemek/geom/matrix.hpp>
 #include <psemek/geom/gauss.hpp>
+#include <psemek/geom/qr.hpp>
+#include <psemek/random/generator.hpp>
+#include <psemek/random/normal.hpp>
 
+using namespace psemek;
 using namespace psemek::geom;
 
 test_case(geom_matrix_empty)
@@ -190,3 +194,26 @@ test_case(geom_matrix_inverse)
 	expect(i);
 	expect_small(frobenius_norm(*i - by_rows(vector{-2.f, 1.f}, vector{1.5f, -0.5f})), 1e-6);
 }
+
+test_case(geom_matrix_qr)
+{
+	random::generator rng;
+	random::normal_distribution<float> normal;
+
+	for (int iteration = 0; iteration < 1024; ++iteration)
+	{
+		matrix<float, 8, 8> m;
+		for (std::size_t i = 0; i < m.rows(); ++i)
+			for (std::size_t j = 0; j < m.columns(); ++j)
+				m[i][j] = normal(rng);
+
+		auto result = qr_decomposition(m);
+
+		expect_small(frobenius_norm(m - result.q * result.r), 1e-5);
+		expect_small(frobenius_norm(matrix<float, 8, 8>::identity() - result.q * transpose(result.q)), 1e-5);
+
+		for (std::size_t i = 0; i < m.rows(); ++i)
+			for (std::size_t j = i + 1; j < m.columns(); ++j)
+				expect_small(result.r[j][i], 1e-6);
+	}
+}