From d999923a354f8a337e2aa4d9349f67482ed7e394 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 18 Dec 2015 13:23:19 +0100 Subject: [PATCH 1/3] support for std::cout --- CMakeLists.txt | 7 +++++++ matrix/Matrix.hpp | 21 +++++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 639f9b19e269..19cb107e5ed9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,13 @@ endif() set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Profile") +option (SUPPORT_STDIOSTREAM + "If enabled provides support for << operator (as used with + std::cout)" OFF) +if((SUPPORT_STDIOSTREAM)) + add_definitions(-DSUPPORT_STDIOSTREAM) +endif() + set(CMAKE_CXX_FLAGS_PROFILE ${CMAKE_CXX_FLAGS_DEBUG} --coverage diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp index 2e32fc29b219..8d85946ed611 100644 --- a/matrix/Matrix.hpp +++ b/matrix/Matrix.hpp @@ -13,6 +13,10 @@ #include #include #include +#if defined(SUPPORT_STDIOSTREAM) +#include +#include +#endif // defined(SUPPORT_STDIOSTREAM) #include "math.hpp" @@ -413,6 +417,23 @@ Matrix ones() { return m; } +#if defined(SUPPORT_STDIOSTREAM) +template +std::ostream& operator<<(std::ostream& os, + const matrix::Matrix& matrix) +{ + for (size_t i = 0; i < M; ++i) { + os << "["; + for (size_t j = 0; j < N; ++j) { + os << std::setw(10) << static_cast(matrix(i, j)); + os << "\t"; + } + os << "]" << std::endl; + } + return os; +} +#endif // defined(SUPPORT_STDIOSTREAM) + typedef Matrix Matrix3f; }; // namespace matrix From 99ac5327465fddf5d6d2859941157d4c3fb04ee8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 26 Dec 2015 10:07:25 +0100 Subject: [PATCH 2/3] remove unnecessary ; --- matrix/Dcm.hpp | 2 +- matrix/Euler.hpp | 2 +- matrix/Matrix.hpp | 2 +- matrix/Quaternion.hpp | 2 +- matrix/Scalar.hpp | 2 +- matrix/SquareMatrix.hpp | 2 +- matrix/Vector.hpp | 2 +- matrix/Vector2.hpp | 2 +- matrix/Vector3.hpp | 2 +- matrix/filter.hpp | 2 +- matrix/integration.hpp | 2 +- 11 files changed, 11 insertions(+), 11 deletions(-) diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp index c94e890cfb1a..646fa7e2ec8a 100644 --- a/matrix/Dcm.hpp +++ b/matrix/Dcm.hpp @@ -87,6 +87,6 @@ class Dcm : public Matrix typedef Dcm Dcmf; -}; // namespace matrix +} // namespace matrix /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp index 74e364940f6f..53d23d02b1a0 100644 --- a/matrix/Euler.hpp +++ b/matrix/Euler.hpp @@ -97,6 +97,6 @@ class Euler : public Vector typedef Euler Eulerf; -}; // namespace matrix +} // namespace matrix /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp index 8d85946ed611..f54ad2d2e029 100644 --- a/matrix/Matrix.hpp +++ b/matrix/Matrix.hpp @@ -436,6 +436,6 @@ std::ostream& operator<<(std::ostream& os, typedef Matrix Matrix3f; -}; // namespace matrix +} // namespace matrix /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index e1067c6bacad..590cbac9bed8 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -124,6 +124,6 @@ class Quaternion : public Vector typedef Quaternion Quatf; -}; // namespace matrix +} // namespace matrix /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/Scalar.hpp b/matrix/Scalar.hpp index 022ab5f7de83..0e722eed8c45 100644 --- a/matrix/Scalar.hpp +++ b/matrix/Scalar.hpp @@ -50,6 +50,6 @@ class Scalar typedef Scalar Scalarf; -}; // namespace matrix +} // 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 36cc716beccc..c10d0a4b4d7e 100644 --- a/matrix/SquareMatrix.hpp +++ b/matrix/SquareMatrix.hpp @@ -217,6 +217,6 @@ SquareMatrix inv(const SquareMatrix & A) -}; // namespace matrix +} // namespace matrix /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp index 4c75ce56946e..8255d6e9c628 100644 --- a/matrix/Vector.hpp +++ b/matrix/Vector.hpp @@ -69,6 +69,6 @@ class Vector : public Matrix }; -}; // namespace matrix +} // namespace matrix /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/Vector2.hpp b/matrix/Vector2.hpp index 99565605949a..50e9855c9fc2 100644 --- a/matrix/Vector2.hpp +++ b/matrix/Vector2.hpp @@ -60,6 +60,6 @@ class Vector2 : public Vector typedef Vector2 Vector2f; -}; // namespace matrix +} // namespace matrix /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/Vector3.hpp b/matrix/Vector3.hpp index c33ff77e58c6..4bbfeb0745b4 100644 --- a/matrix/Vector3.hpp +++ b/matrix/Vector3.hpp @@ -65,6 +65,6 @@ class Vector3 : public Vector typedef Vector3 Vector3f; -}; // namespace matrix +} // namespace matrix /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/filter.hpp b/matrix/filter.hpp index a9d9982b1a5d..1841d1460ec7 100644 --- a/matrix/filter.hpp +++ b/matrix/filter.hpp @@ -23,4 +23,4 @@ int kalman_correct( return 0; } -}; // namespace matrix +} // namespace matrix diff --git a/matrix/integration.hpp b/matrix/integration.hpp index afc0cb7556be..a0ad55c68a66 100644 --- a/matrix/integration.hpp +++ b/matrix/integration.hpp @@ -24,4 +24,4 @@ int integrate_rk4( return 0; } -}; // namespace matrix +} // namespace matrix From 45e6012818419a1bdb57db39623bcaf1f7ca5c05 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 26 Dec 2015 13:04:07 +0100 Subject: [PATCH 3/3] matrix scalar pre multiplication and general scalar multiplication for quaternions --- matrix/Matrix.hpp | 6 ++++++ matrix/Quaternion.hpp | 12 ++++++++++++ test/attitude.cpp | 13 +++++++++++++ 3 files changed, 31 insertions(+) diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp index f54ad2d2e029..59498f6556fd 100644 --- a/matrix/Matrix.hpp +++ b/matrix/Matrix.hpp @@ -417,6 +417,12 @@ Matrix ones() { return m; } +template +Matrix operator*(Type scalar, const Matrix &other) +{ + return other * scalar; +} + #if defined(SUPPORT_STDIOSTREAM) template std::ostream& operator<<(std::ostream& os, diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index 590cbac9bed8..dc813ab2aef5 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -104,6 +104,18 @@ class Quaternion : public Vector self = self * other; } + Quaternion operator*(Type scalar) const + { + const Quaternion &q = *this; + return scalar * q; + } + + void operator*=(Type scalar) + { + Quaternion &q = *this; + q = q * scalar; + } + Matrix41 derivative(const Matrix31 & w) const { const Quaternion &q = *this; Type dataQ[] = { diff --git a/test/attitude.cpp b/test/attitude.cpp index 58f8f93fc886..b06409d2ba28 100644 --- a/test/attitude.cpp +++ b/test/attitude.cpp @@ -133,6 +133,19 @@ int main() q_check *= q_check; assert(q_prod_check == q_check); + // Quaternion scalar multiplication + float scalar = 0.5; + Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f); + Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar, + 3.0f * scalar, 4.0f * scalar); + Quatf q_scalar_mul_res = scalar * q_scalar_mul; + assert(q_scalar_mul_check == q_scalar_mul_res); + Quatf q_scalar_mul_res2 = q_scalar_mul * scalar; + assert(q_scalar_mul_check == q_scalar_mul_res2); + Quatf q_scalar_mul_res3(q_scalar_mul); + q_scalar_mul_res3 *= scalar; + assert(q_scalar_mul_check == q_scalar_mul_res3); + }; /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */