From 1dffd5930bfec53879e467294eb323b63efc500c Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Wed, 15 Nov 2017 22:15:13 +0100
Subject: [PATCH] Matrix: add copyTo copying data to an array and
 copyToColumnMajor which does the same but with column-major order same
 functionality explicitly for quaternions can be deleted

---
 matrix/Matrix.hpp     | 16 ++++++++++++++++
 matrix/Quaternion.hpp | 14 --------------
 2 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index 2428f9d246..559a8d804d 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -82,6 +82,22 @@ public:
         return (*this);
     }
 
+    void copyTo(Type (&dst)[M*N]) const
+    {
+        memcpy(dst, _data, sizeof(dst));
+    }
+
+    void copyToColumnMajor(Type (&dst)[M*N]) const
+    {
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                dst[i+(j*M)] = self(i, j);
+            }
+        }
+    }
+
     /**
      * Matrix Operations
      */
diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index bd8cd9955e..ea24e1fc6e 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -260,20 +260,6 @@ public:
         q = q * scalar;
     }
 
-    /**
-     * Copy quaternion to a float array
-     *
-     * @param dst array of 4 floats
-     */
-    void copyTo(float (&dst)[4])
-    {
-        const Quaternion &q = *this;
-        dst[0] = q(0);
-        dst[1] = q(1);
-        dst[2] = q(2);
-        dst[3] = q(3);
-    }
-
     /**
      * Computes the derivative of q_21 when
      * rotated with angular velocity expressed in frame 1