Quaternion add copyTo

matrix/Quaternion.hpp

@@ -253,6 +253,20 @@ 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_12 when
      * rotated with angular velocity expressed in frame 2