From 19554c4470cd6322ffd18cb20a96485940f226b6 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 16 Aug 2016 03:59:06 -0400
Subject: [PATCH] Use Hamilton quaternion definition.

---
 matrix/Quaternion.hpp | 18 ++++++++++++++----
 test/attitude.cpp     | 20 +++++++++++++-------
 2 files changed, 27 insertions(+), 11 deletions(-)

diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index 6917357fbf..a96d8217bc 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -13,6 +13,16 @@
  * represents the real part, thus, a quaternion representing a zero-rotation
  * is defined as (1,0,0,0).
  *
+ * Using Hamilton Quaternion
+ *  - https://en.wikipedia.org/wiki/Quaternion
+ *  - http://www.iri.upc.edu/people/jsola/JoanSola/objectes/notes/kinematics.pdf
+ *  - Components (q_w, q_v) where q_w is the scalar and q_v is a vector
+ *  - algebrea: i*j = k
+ *  - handedness: right
+ *  - function: passive (rotation operator rotates frames)
+ *  - right-to-left product means local-to-global, q_GL
+ *    default operator v_G = q x v_L x q*
+ *
  * @author James Goppert <james.goppert@gmail.com>
  */
 
@@ -190,9 +200,9 @@ public:
         const Quaternion &p = *this;
         Quaternion r;
         r(0) = p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3);
-        r(1) = p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2);
-        r(2) = p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1);
-        r(3) = p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0);
+        r(1) = p(0) * q(1) + p(1) * q(0) + p(2) * q(3) - p(3) * q(2);
+        r(2) = p(0) * q(2) - p(1) * q(3) + p(2) * q(0) + p(3) * q(1);
+        r(3) = p(0) * q(3) + p(1) * q(2) - p(2) * q(1) + p(3) * q(0);
         return r;
     }
 
@@ -239,7 +249,7 @@ public:
     {
         const Quaternion &q = *this;
         Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
-        return v * q  * Type(0.5);
+        return q * v  * Type(0.5);
     }
 
     /**
diff --git a/test/attitude.cpp b/test/attitude.cpp
index 789b7aeb23..b13bf73c2c 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -156,6 +156,19 @@ int main()
     Quatf q_from_m(m4);
     TEST(isEqual(q_from_m, m4));
 
+    // quaternion product
+    Quatf q3(0, 0, 0, 1);
+    Quatf q4(AxisAnglef(0, 0, float(2*M_PI)));
+    q4 *= q4;
+    q4 *= q3;
+    TEST(isEqual(q3, q4));
+
+    q3 = Quatf(0, 0, 0, 1);
+    q4 = AxisAnglef(0, 0, float(M_PI/2));
+    q4 *= q4;
+    q4 *= q3;
+    TEST(isEqual(Quatf(-1, 0, 0, 0), q4));
+
     // quaternion derivate
     Quatf q1(0, 1, 0, 0);
     Vector<float, 4> q1_dot = q1.derivative(Vector3f(1, 2, 3));
@@ -163,13 +176,6 @@ int main()
     Vector<float, 4> q1_dot_check(data_q_dot_check);
     TEST(isEqual(q1_dot, q1_dot_check));
 
-    // quaternion product
-    Quatf q_prod_check(
-        0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
-    TEST(isEqual(q_prod_check, q_check*q_check));
-    q_check *= q_check;
-    TEST(isEqual(q_prod_check, q_check));
-
     // Quaternion scalar multiplication
     float scalar = 0.5;
     Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f);