From 65679fbcbb4b31a1a77f3dc5268ca78ca95b3bf3 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 28 Apr 2016 21:16:36 +0200 Subject: [PATCH] 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 | 9 +++++++++ matrix/Matrix.hpp | 22 +++------------------- matrix/SquareMatrix.hpp | 13 ++++++++++++- matrix/Vector3.hpp | 18 ++++++++++++++++++ test/CMakeLists.txt | 1 + test/attitude.cpp | 6 ++++++ test/hatvee.cpp | 23 +++++++++++++++++++++++ test/squareMatrix.cpp | 1 + 8 files changed, 73 insertions(+), 20 deletions(-) create mode 100644 test/hatvee.cpp diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp index 2118dab3c2..e7379d916b 100644 --- a/matrix/Dcm.hpp +++ b/matrix/Dcm.hpp @@ -83,6 +83,15 @@ public: dcm(2, 1) = sinPhi * cosThe; dcm(2, 2) = cosPhi * cosThe; } + + Vector vee() const { // inverse to Vector.hat() operation + const Dcm &A(*this); + Vector v; + v(0) = -A(1,2); + v(1) = A(0,2); + v(2) = -A(0,1); + return v; + } }; typedef Dcm Dcmf; diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp index 4cf2d431c8..2fcfce321a 100644 --- a/matrix/Matrix.hpp +++ b/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 &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 Matrix3f; - } // namespace matrix /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp index c10d0a4b4d..3e42b64e55 100644 --- a/matrix/SquareMatrix.hpp +++ b/matrix/SquareMatrix.hpp @@ -61,6 +61,17 @@ public: return res; } + Type trace() const + { + Type res = 0; + const SquareMatrix &self = *this; + + for (size_t i = 0; i < M; i++) { + res += self(i, i); + } + return res; + } + }; typedef SquareMatrix SquareMatrix3f; @@ -215,7 +226,7 @@ SquareMatrix inv(const SquareMatrix & A) return X; } - +typedef SquareMatrix Matrix3f; } // namespace matrix diff --git a/matrix/Vector3.hpp b/matrix/Vector3.hpp index 4bbfeb0745..9fed6ac3f6 100644 --- a/matrix/Vector3.hpp +++ b/matrix/Vector3.hpp @@ -16,6 +16,9 @@ namespace matrix template class Vector; +template +class Dcm; + template class Vector3 : public Vector { @@ -61,6 +64,21 @@ public: return (*this).cross(b); } + Dcm hat() const { // inverse to Dcm.vee() operation + const Vector3 &v(*this); + Dcm 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 Vector3f; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 2cff34c3ac..30f5c55f57 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -15,6 +15,7 @@ set(tests integration squareMatrix helper + hatvee ) add_custom_target(test_build) diff --git a/test/attitude.cpp b/test/attitude.cpp index f7857d3cd4..abdfeb67da 100644 --- a/test/attitude.cpp +++ b/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}; + Quaternionq_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 : */ diff --git a/test/hatvee.cpp b/test/hatvee.cpp new file mode 100644 index 0000000000..c79a2b4184 --- /dev/null +++ b/test/hatvee.cpp @@ -0,0 +1,23 @@ +#include +#include "test_macros.hpp" + +#include + +using namespace matrix; + +template class SquareMatrix; + +int main() +{ + Euler euler(0.1f, 0.2f, 0.3f); + Dcm R(euler); + Dcm skew = R - R.T(); + Vector3 w = skew.vee(); + Vector3 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 : */ diff --git a/test/squareMatrix.cpp b/test/squareMatrix.cpp index f8f1caac54..ff87a3b8be 100644 --- a/test/squareMatrix.cpp +++ b/test/squareMatrix.cpp @@ -17,6 +17,7 @@ int main() Vector3 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,