Quaternion add copyTo

.gitignore

@@ -1,4 +1,33 @@
-build*/
 *.orig
 *.swp
+astyle/
+build*/
+cmake_install.cmake
+CMakeCache.txt
+CMakeFiles/
+CPackConfig.cmake
+CPackSourceConfig.cmake
+CTestTestfile.cmake
+Makefile
+test/attitude
+test/cmake_install.cmake
+test/CMakeFiles/
+test/CTestTestfile.cmake
+test/filter
+test/hatvee
+test/helper
+test/integration
+test/inverse
+test/Makefile
+test/matrixAssignment
+test/matrixMult
+test/matrixScalarMult
+test/setIdentity
+test/slice
+test/squareMatrix
+test/transpose
+test/vector
+test/vector2
+test/vector3
+test/vectorAssignment
 Testing/

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

test/attitude.cpp

@@ -343,6 +343,17 @@ int main()
     q = Quatf(0,0,0,1); // 180 degree rotation around the z axis
     R = Dcmf(q);
     TEST(isEqual(q, Quatf(R)));
+
+
+    // Quaternion copyTo
+    q = Quatf(1, 2, 3, 4);
+    float dst[4] = {};
+    q.copyTo(dst);
+    TEST(fabsf(q(0) - dst[0]) < eps);
+    TEST(fabsf(q(1) - dst[1]) < eps);
+    TEST(fabsf(q(2) - dst[2]) < eps);
+    TEST(fabsf(q(3) - dst[3]) < eps);
+
 }
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */