From e21c9ca3a5f31edc5d9de766907ad6455c8080ea Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Vladim=C3=ADr=20Vondru=C5=A1?= <mosra@centrum.cz>
Date: Sun, 26 Aug 2012 03:19:39 +0200
Subject: [PATCH] Function for computing trace of square matrix.

---
 src/Math/Matrix.h            | 16 ++++++++++++++++
 src/Math/Test/MatrixTest.cpp | 13 +++++++++++++
 src/Math/Test/MatrixTest.h   |  1 +
 3 files changed, 30 insertions(+)

diff --git a/src/Math/Matrix.h b/src/Math/Matrix.h
index c524c877b..2273ce3b1 100644
--- a/src/Math/Matrix.h
+++ b/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);
diff --git a/src/Math/Test/MatrixTest.cpp b/src/Math/Test/MatrixTest.cpp
index 1cdd211f6..2d803ec16 100644
--- a/src/Math/Test/MatrixTest.cpp
+++ b/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,
diff --git a/src/Math/Test/MatrixTest.h b/src/Math/Test/MatrixTest.h
index 57f9377a5..ba1cb02ee 100644
--- a/src/Math/Test/MatrixTest.h
+++ b/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();