From 37955c468fc3be8411bc557ec16ccde471c97769 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Vladim=C3=ADr=20Vondru=C5=A1?= <mosra@centrum.cz>
Date: Mon, 28 Jan 2013 17:58:59 +0100
Subject: [PATCH] Math: creating matrix from diagonal, retrieving values on
 diagonal.

---
 src/Math/RectangularMatrix.h            | 35 +++++++++++++++++++++++++
 src/Math/Test/RectangularMatrixTest.cpp | 34 ++++++++++++++++++++++++
 2 files changed, 69 insertions(+)

diff --git a/src/Math/RectangularMatrix.h b/src/Math/RectangularMatrix.h
index 84a7eca5e..7a0f45101 100644
--- a/src/Math/RectangularMatrix.h
+++ b/src/Math/RectangularMatrix.h
@@ -48,6 +48,13 @@ template<std::size_t cols, std::size_t rows, class T> class RectangularMatrix {
         const static std::size_t Cols = cols;   /**< @brief %Matrix column count */
         const static std::size_t Rows = rows;   /**< @brief %Matrix row count */
 
+        /**
+         * @brief Size of matrix diagonal
+         *
+         * @see fromDiagonal(), diagonal()
+         */
+        const static std::size_t DiagonalSize = (cols < rows ? cols : rows);
+
         /**
          * @brief %Matrix from array
          * @return Reference to the data as if it was Matrix, thus doesn't
@@ -64,6 +71,20 @@ template<std::size_t cols, std::size_t rows, class T> class RectangularMatrix {
             return *reinterpret_cast<const RectangularMatrix<cols, rows, T>*>(data);
         }
 
+        /**
+         * @brief Construct diagonal matrix
+         *
+         * @see diagonal()
+         */
+        inline static RectangularMatrix<cols, rows, T> fromDiagonal(const Vector<DiagonalSize, T>& diagonal) {
+            RectangularMatrix<cols, rows, T> out;
+
+            for(std::size_t i = 0; i != DiagonalSize; ++i)
+                out[i][i] = diagonal[i];
+
+            return out;
+        }
+
         /** @brief Construct zero-filled matrix */
         inline constexpr /*implicit*/ RectangularMatrix() {}
 
@@ -309,6 +330,20 @@ template<std::size_t cols, std::size_t rows, class T> class RectangularMatrix {
             return out;
         }
 
+        /**
+         * @brief Values on diagonal
+         *
+         * @see fromDiagonal()
+         */
+        Vector<DiagonalSize, T> diagonal() const {
+            Vector<DiagonalSize, T> out;
+
+            for(std::size_t i = 0; i != DiagonalSize; ++i)
+                out[i] = _data[i][i];
+
+            return out;
+        }
+
     private:
         /* Implementation for RectangularMatrix<cols, rows, T>::RectangularMatrix(const RectangularMatrix<cols, rows, U>&) */
         template<class U, std::size_t ...sequence> inline constexpr explicit RectangularMatrix(Implementation::Sequence<sequence...>, const RectangularMatrix<cols, rows, U>& matrix): _data{Vector<rows, T>(matrix[sequence])...} {}
diff --git a/src/Math/Test/RectangularMatrixTest.cpp b/src/Math/Test/RectangularMatrixTest.cpp
index 730b2c3e8..78d867199 100644
--- a/src/Math/Test/RectangularMatrixTest.cpp
+++ b/src/Math/Test/RectangularMatrixTest.cpp
@@ -31,6 +31,7 @@ class RectangularMatrixTest: public Corrade::TestSuite::Tester {
         void constructDefault();
         void constructConversion();
         void constructFromVectors();
+        void constructFromDiagonal();
         void data();
 
         void compare();
@@ -41,6 +42,7 @@ class RectangularMatrixTest: public Corrade::TestSuite::Tester {
         void multiply();
 
         void transposed();
+        void diagonal();
 
         void debug();
         void configuration();
@@ -60,6 +62,7 @@ RectangularMatrixTest::RectangularMatrixTest() {
              &RectangularMatrixTest::constructDefault,
              &RectangularMatrixTest::constructConversion,
              &RectangularMatrixTest::constructFromVectors,
+             &RectangularMatrixTest::constructFromDiagonal,
              &RectangularMatrixTest::data,
 
              &RectangularMatrixTest::compare,
@@ -70,6 +73,7 @@ RectangularMatrixTest::RectangularMatrixTest() {
              &RectangularMatrixTest::multiply,
 
              &RectangularMatrixTest::transposed,
+             &RectangularMatrixTest::diagonal,
 
              &RectangularMatrixTest::debug,
              &RectangularMatrixTest::configuration);
@@ -124,6 +128,21 @@ void RectangularMatrixTest::constructFromVectors() {
     CORRADE_COMPARE(actual, expected);
 }
 
+void RectangularMatrixTest::constructFromDiagonal() {
+    Vector3 diagonal(-1.0f, 5.0f, 11.0f);
+
+    Matrix3x4 expectedA(Vector4(-1.0f, 0.0f,  0.0f, 0.0f),
+                        Vector4( 0.0f, 5.0f,  0.0f, 0.0f),
+                        Vector4( 0.0f, 0.0f, 11.0f, 0.0f));
+    CORRADE_COMPARE(Matrix3x4::fromDiagonal(diagonal), expectedA);
+
+    Matrix4x3 expectedB(Vector3(-1.0f, 0.0f,  0.0f),
+                        Vector3( 0.0f, 5.0f,  0.0f),
+                        Vector3( 0.0f, 0.0f, 11.0f),
+                        Vector3( 0.0f, 0.0f,  0.0f));
+    CORRADE_COMPARE(Matrix4x3::fromDiagonal(diagonal), expectedB);
+}
+
 void RectangularMatrixTest::data() {
     Matrix3x4 m;
     Vector4 vector(4.0f, 5.0f, 6.0f, 7.0f);
@@ -252,6 +271,21 @@ void RectangularMatrixTest::transposed() {
     CORRADE_COMPARE(original.transposed(), transposed);
 }
 
+void RectangularMatrixTest::diagonal() {
+    Vector3 diagonal(-1.0f, 5.0f, 11.0f);
+
+    Matrix4x3 a(Vector3(-1.0f,  1.0f,  3.0f),
+                Vector3( 4.0f,  5.0f,  7.0f),
+                Vector3( 8.0f,  9.0f, 11.0f),
+                Vector3(12.0f, 13.0f, 15.0f));
+    CORRADE_COMPARE(a.diagonal(), diagonal);
+
+    Matrix3x4 b(Vector4(-1.0f, 4.0f,  8.0f, 12.0f),
+                Vector4( 1.0f, 5.0f,  9.0f, 13.0f),
+                Vector4( 3.0f, 7.0f, 11.0f, 15.0f));
+    CORRADE_COMPARE(b.diagonal(), diagonal);
+}
+
 void RectangularMatrixTest::debug() {
     Matrix3x4 m(Vector4(3.0f,  5.0f, 8.0f, 4.0f),
                 Vector4(4.0f,  4.0f, 7.0f, 3.0f),