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;