From 14c5fda1a7129b5a5f8f49e7b9443613641ed7a0 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Sat, 16 Jan 2021 22:43:45 +0300
Subject: [PATCH] Implement matrix trace

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

diff --git a/libs/geom/include/psemek/geom/matrix.hpp b/libs/geom/include/psemek/geom/matrix.hpp
index 235d9709..39c96eac 100644
--- a/libs/geom/include/psemek/geom/matrix.hpp
+++ b/libs/geom/include/psemek/geom/matrix.hpp
@@ -285,6 +285,15 @@ namespace psemek::geom
 		return std::sqrt(frobenius_norm_sqr(m));
 	}
 
+	template <typename T, std::size_t R, std::size_t C>
+	T trace(matrix<T, R, C> const & m)
+	{
+		T r{0};
+		for (std::size_t i = 0; i < std::min(R, C); ++i)
+			r += m[i][i];
+		return r;
+	}
+
 	template <typename Stream, typename T, std::size_t R, std::size_t C>
 	Stream & operator << (Stream & os, matrix<T, R, C> const & m)
 	{