diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index 314a6daeaa..2e0a768c8a 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -13,16 +13,6 @@ * 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 */ @@ -200,9 +190,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; } @@ -249,7 +239,7 @@ public: { const Quaternion &q = *this; Quaternion v(0, w(0, 0), w(1, 0), w(2, 0)); - return q * v * Type(0.5); + return v * q * Type(0.5); } /** diff --git a/test/attitude.cpp b/test/attitude.cpp index 8d2fb28322..c7b1feadb9 100644 --- a/test/attitude.cpp +++ b/test/attitude.cpp @@ -157,19 +157,6 @@ 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 q1_dot = q1.derivative(Vector3f(1, 2, 3)); @@ -177,6 +164,13 @@ int main() Vector 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);