From 45e6012818419a1bdb57db39623bcaf1f7ca5c05 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 26 Dec 2015 13:04:07 +0100
Subject: [PATCH] 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 f54ad2d2e0..59498f6556 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -417,6 +417,12 @@ 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,
diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index 590cbac9be..dc813ab2ae 100644
--- a/matrix/Quaternion.hpp
+++ b/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[] = {
diff --git a/test/attitude.cpp b/test/attitude.cpp
index 58f8f93fc8..b06409d2ba 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 : */