From 9a50990c5c4cc758a8aff2a200cadcb960947c85 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Sat, 25 Dec 2021 14:19:22 +0300
Subject: [PATCH] Add geom::submatrix

---
 libs/geom/include/psemek/geom/matrix.hpp | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/libs/geom/include/psemek/geom/matrix.hpp b/libs/geom/include/psemek/geom/matrix.hpp
index 57f4eef2..55dad440 100644
--- a/libs/geom/include/psemek/geom/matrix.hpp
+++ b/libs/geom/include/psemek/geom/matrix.hpp
@@ -303,6 +303,25 @@ namespace psemek::geom
 		return r;
 	}
 
+	template <std::size_t R1, std::size_t C1, std::size_t R2, std::size_t C2, typename T, std::size_t R, std::size_t C>
+	matrix<T, R2 - R1, C2 - C1> submatrix(matrix<T, R, C> const & m)
+	{
+		static_assert(R1 < R2);
+		static_assert(C1 < C2);
+		static_assert(R2 <= R);
+		static_assert(C2 <= C);
+
+		matrix<T, R2 - R1, C2 - C1> result;
+		for (std::size_t i = 0; i < R2 - R1; ++i)
+		{
+			for (std::size_t j = 0; j < C2 - C1; ++j)
+			{
+				result[i][j] = m[i + R1][j + C1];
+			}
+		}
+		return result;
+	}
+
 	template <typename T, std::size_t R, std::size_t C>
 	std::ostream & operator << (std::ostream & os, matrix<T, R, C> const & m)
 	{