Features and fixes

* added the trace function for a SquareMatrix

* added Vector3.hat() and it's counterpart Dcm.vee()
for skewsymskew symmetric matrix operations in relation to the cross product
see https://en.wikipedia.org/wiki/Hat_operator

* Matrix::write_string produced runtime errors when I used it in PX4 posix simulation, i simplified it

* a Matrix3f is a SquareMatrix

* added tests for SquareMatrix.trace, Vector3.hat and Dcm.vee

* added a test for quaternion initialisation from array

* preventing buffer overflows in Matrix.write_string method

matrix/Dcm.hpp

@@ -83,6 +83,15 @@ public:
         dcm(2, 1) = sinPhi * cosThe;
         dcm(2, 2) = cosPhi * cosThe;
     }
+
+    Vector<Type, 3> vee() const {    // inverse to Vector.hat() operation
+        const Dcm &A(*this);
+        Vector<Type, 3> v;
+        v(0) = -A(1,2);
+        v(1) =  A(0,2);
+        v(2) = -A(0,1);
+        return v;
+    }
 };
 
 typedef Dcm<float> Dcmf;

matrix/Matrix.hpp

@@ -274,28 +274,14 @@ public:
 
     void write_string(char * buf, size_t n) const
     {
+        buf[0] = '\0'; // make an empty string to begin with (we need the '\0' for strlen to work)
         const Matrix<Type, M, N> &self = *this;
-        char data_buf[500] = {0};
         for (size_t i = 0; i < M; i++) {
-            char data_line[100] = {0};
-            char data_line_formatted[100] = {0};
             for (size_t j = 0; j < N; j++) {
-                char val_buf[15];
-                if (j == N-1) {
-                    snprintf(val_buf, 15, "\t%10g", double(self(i, j)));
-                } else {
-                    snprintf(val_buf, 15, "\t%10g,", double(self(i, j)));
-                }
-                strncat(data_line, val_buf, 300);
+                snprintf(buf + strlen(buf), n - strlen(buf), "\t%.2g", double(self(i, j))); // directly append to the string buffer
             }
-            if (i == M-1) {
-                snprintf(data_line_formatted, n, "[%s]", data_line);
-            } else {
-                snprintf(data_line_formatted, n, "[%s],\n", data_line);
-            }
-            strncat(data_buf, data_line_formatted, n);
+            snprintf(buf + strlen(buf), n - strlen(buf), "\n");
         }
-        snprintf(buf, n, "[%s]", data_buf);
     }
 
     void print() const
@@ -511,8 +497,6 @@ std::ostream& operator<<(std::ostream& os,
 }
 #endif // defined(SUPPORT_STDIOSTREAM)
 
-typedef Matrix<float, 3, 3> Matrix3f;
-
 } // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/SquareMatrix.hpp

@@ -61,6 +61,17 @@ public:
         return res;
     }
 
+    Type trace() const
+    {
+        Type res = 0;
+        const SquareMatrix<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            res += self(i, i);
+        }
+        return res;
+    }
+
 };
 
 typedef SquareMatrix<float, 3> SquareMatrix3f;
@@ -215,7 +226,7 @@ SquareMatrix <Type, M> inv(const SquareMatrix<Type, M> & A)
     return X;
 }
 
-
+typedef SquareMatrix<float, 3> Matrix3f;
 
 } // namespace matrix
 

matrix/Vector3.hpp

@@ -16,6 +16,9 @@ namespace matrix
 template <typename Type, size_t M>
 class Vector;
 
+template<typename Type>
+class Dcm;
+
 template<typename Type>
 class Vector3 : public Vector<Type, 3>
 {
@@ -61,6 +64,21 @@ public:
         return (*this).cross(b);
     }
 
+    Dcm<Type> hat() const {    // inverse to Dcm.vee() operation
+        const Vector3 &v(*this);
+        Dcm<Type> A;
+        A(0,0) = 0;
+        A(0,1) = -v(2);
+        A(0,2) = v(1);
+        A(1,0) = v(2);
+        A(1,1) = 0;
+        A(1,2) = -v(0);
+        A(2,0) = -v(1);
+        A(2,1) = v(0);
+        A(2,2) = 0;
+        return A;
+    }
+
 };
 
 typedef Vector3<float> Vector3f;

test/CMakeLists.txt

@@ -15,6 +15,7 @@ set(tests
 	integration
 	squareMatrix
 	helper
+	hatvee
 	)
 
 add_custom_target(test_build)

test/attitude.cpp

@@ -231,6 +231,12 @@ int main()
     TEST(fabsf(q(2) - q_true(2)) < eps);
     TEST(fabsf(q(3) - q_true(3)) < eps);
 
+    // Quaternion initialisation per array
+    float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
+    Quaternion<float>q_from_array(q_array);
+    for(int i = 0; i < 4; i++)
+        TEST(fabsf(q_from_array(i) - q_array[i]) < eps);
+
 };
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

test/hatvee.cpp

@@ -0,0 +1,23 @@
+#include <stdio.h>
+#include "test_macros.hpp"
+
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+template class SquareMatrix<float, 3>;
+
+int main()
+{
+    Euler<float> euler(0.1f, 0.2f, 0.3f);
+    Dcm<float> R(euler);
+    Dcm<float> skew = R - R.T();
+    Vector3<float> w = skew.vee();
+    Vector3<float> w_check(0.1348f, 0.4170f, 0.5647f);
+
+    TEST(isEqual(w, w_check));
+    TEST(isEqual(skew, w.hat()));
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

test/squareMatrix.cpp

@@ -17,6 +17,7 @@ int main()
     Vector3<float> diag_check(1, 5, 10);
 
     TEST(isEqual(A.diag(), diag_check));
+    TEST(A.trace() - 16 < 1e-3);
 
     float data_check[9] = {
         1.01158503f,  0.02190432f,  0.03238144f,