Merge pull request #6 from thomasgubler/pr1

scalar multiplication; std::cout support
PX4 · Jan 12, 2016 · 0e48d25 · 0e48d25
2 parents 9cd6ac3 + 45e6012
commit 0e48d25
Show file tree

Hide file tree

Showing 13 changed files with 70 additions and 11 deletions.
diff --git 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/Dcm.hpp b/matrix/Dcm.hpp
@@ -87,6 +87,6 @@ class Dcm : public Matrix<Type, 3, 3>
 
 typedef Dcm<float> 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
@@ -97,6 +97,6 @@ class Euler : public Vector<Type, 3>
 
 typedef Euler<float> 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
@@ -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 : */
diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
@@ -104,6 +104,18 @@ class Quaternion : public Vector<Type, 4>
         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 @@ class Quaternion : public Vector<Type, 4>
 
 typedef Quaternion<float> 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
@@ -50,6 +50,6 @@ class Scalar
 
 typedef Scalar<float> 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
@@ -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 : */
diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp
@@ -69,6 +69,6 @@ class Vector : public Matrix<Type, M, 1>
 
 };
 
-}; // 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
@@ -60,6 +60,6 @@ class Vector2 : public Vector<Type, 2>
 
 typedef Vector2<float> 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
@@ -65,6 +65,6 @@ class Vector3 : public Vector<Type, 3>
 
 typedef Vector3<float> 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
@@ -23,4 +23,4 @@ int kalman_correct(
     return 0;
 }
 
-}; // namespace matrix
+} // namespace matrix
diff --git a/matrix/integration.hpp b/matrix/integration.hpp
@@ -24,4 +24,4 @@ int integrate_rk4(
     return 0;
 }
 
-}; // namespace matrix
+} // namespace matrix
diff --git 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 : */
Original file line number	Diff line number	Diff line change
Expand Up		@@ -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 : */