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)
 	{