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

@@ -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
      */

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