Merge pull request #6 from thomasgubler/pr1

scalar multiplication; std::cout support

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

matrix/Dcm.hpp

@@ -87,6 +87,6 @@ public:
 
 typedef Dcm<float> Dcmf;
 
-}; // namespace matrix
+} // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/Euler.hpp

@@ -97,6 +97,6 @@ public:
 
 typedef Euler<float> Eulerf;
 
-}; // namespace matrix
+} // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/Matrix.hpp

@@ -13,6 +13,10 @@
 #include <stdlib.h>
 #include <string.h>
 #include <math.h>
+#if defined(SUPPORT_STDIOSTREAM)
+#include <iostream>
+#include <iomanip>
+#endif // defined(SUPPORT_STDIOSTREAM)
 
 #include "math.hpp"
 
@@ -413,8 +417,31 @@ Matrix<Type, M, N> ones() {
     return m;
 }
 
+template<typename Type, size_t  M, size_t N>
+Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
+{
+    return other * scalar;
+}
+
+#if defined(SUPPORT_STDIOSTREAM)
+template<typename Type, size_t  M, size_t N>
+std::ostream& operator<<(std::ostream& os,
+        const matrix::Matrix<Type, M, N>& matrix)
+{
+        for (size_t i = 0; i < M; ++i) {
+            os << "[";
+            for (size_t j = 0; j < N; ++j) {
+                os << std::setw(10) << static_cast<double>(matrix(i, j));
+                os << "\t";
+            }
+            os << "]" << std::endl;
+        }
+    return os;
+}
+#endif // defined(SUPPORT_STDIOSTREAM)
+
 typedef Matrix<float, 3, 3> Matrix3f;
 
-}; // namespace matrix
+} // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/Quaternion.hpp

@@ -104,6 +104,18 @@ public:
         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[] = {
@@ -124,6 +136,6 @@ public:
 
 typedef Quaternion<float> Quatf;
 
-}; // namespace matrix
+} // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/Scalar.hpp

@@ -50,6 +50,6 @@ private:
 
 typedef Scalar<float> Scalarf;
 
-}; // namespace matrix
+} // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/SquareMatrix.hpp

@@ -217,6 +217,6 @@ SquareMatrix <Type, M> inv(const SquareMatrix<Type, M> & A)
 
 
 
-}; // namespace matrix
+} // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/Vector.hpp

@@ -69,6 +69,6 @@ public:
 
 };
 
-}; // namespace matrix
+} // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/Vector2.hpp

@@ -60,6 +60,6 @@ public:
 
 typedef Vector2<float> Vector2f;
 
-}; // namespace matrix
+} // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/Vector3.hpp

@@ -65,6 +65,6 @@ public:
 
 typedef Vector3<float> Vector3f;
 
-}; // namespace matrix
+} // namespace matrix
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/filter.hpp

@@ -23,4 +23,4 @@ int kalman_correct(
     return 0;
 }
 
-}; // namespace matrix
+} // namespace matrix

matrix/integration.hpp

@@ -24,4 +24,4 @@ int integrate_rk4(
     return 0;
 }
 
-}; // namespace matrix
+} // namespace matrix

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 : */