Function for computing trace of square matrix.

src/Math/Matrix.h

@@ -81,6 +81,22 @@ template<size_t s, class T> class Matrix: public RectangularMatrix<s, s, T> {
             return (*this = *this*other);
         }
 
+        /**
+         * @brief Trace of the matrix
+         *
+         * @f[
+         * tr(A) = \sum_{i=1}^n a_{i,i}
+         * @f]
+         */
+        T trace() const {
+            T out(0);
+
+            for(size_t i = 0; i != size; ++i)
+                out += (*this)(i, i);
+
+            return out;
+        }
+
         /** @brief %Matrix without given column and row */
         Matrix<size-1, T> ij(size_t skipCol, size_t skipRow) const {
             Matrix<size-1, T> out(Matrix<size-1, T>::Zero);

src/Math/Test/MatrixTest.cpp

@@ -34,6 +34,7 @@ MatrixTest::MatrixTest() {
     addTests(&MatrixTest::construct,
              &MatrixTest::constructIdentity,
              &MatrixTest::constructZero,
+             &MatrixTest::trace,
              &MatrixTest::ij,
              &MatrixTest::determinant,
              &MatrixTest::inverted,
@@ -96,6 +97,18 @@ void MatrixTest::constructZero() {
     CORRADE_COMPARE(zero, zeroExpected);
 }
 
+void MatrixTest::trace() {
+    Matrix<5, int> m(
+        1, 2, 3, 0, 0,
+        2, 3, 2, 1, -2,
+        1, 1, -20, 1, 0,
+        2, 0, 0, 10, 2,
+        3, 1, 0, 1, -2
+    );
+
+    CORRADE_COMPARE(m.trace(), -8);
+}
+
 void MatrixTest::ij() {
     Matrix4 original(
         0.0f,   1.0f,   2.0f,   3.0f,

src/Math/Test/MatrixTest.h

@@ -26,6 +26,7 @@ class MatrixTest: public Corrade::TestSuite::Tester<MatrixTest> {
         void construct();
         void constructIdentity();
         void constructZero();
+        void trace();
         void ij();
         void determinant();
         void inverted();