Merge branch 'quaternion'

Conflicts:
	test/attitude.cpp

matrix/Matrix.hpp

@@ -365,8 +365,8 @@ public:
     Matrix<Type, M, N> abs()
     {
         Matrix<Type, M, N> r;
-        for (int i=0; i<M; i++) {
-            for (int j=0; j<M; j++) {
+        for (size_t i=0; i<M; i++) {
+            for (size_t j=0; j<M; j++) {
                 r(i,j) = Type(fabs((*this)(i,j)));
             }
         }
@@ -376,8 +376,8 @@ public:
     Type max()
     {
         Type max_val = (*this)(0,0);
-        for (int i=0; i<M; i++) {
-            for (int j=0; j<M; j++) {
+        for (size_t i=0; i<M; i++) {
+            for (size_t j=0; j<M; j++) {
                 Type val = (*this)(i,j);
                 if (val > max_val) {
                     max_val = val;
@@ -390,8 +390,8 @@ public:
     Type min()
     {
         Type min_val = (*this)(0,0);
-        for (int i=0; i<M; i++) {
-            for (int j=0; j<M; j++) {
+        for (size_t i=0; i<M; i++) {
+            for (size_t j=0; j<M; j++) {
                 Type val = (*this)(i,j);
                 if (val < min_val) {
                     min_val = val;

matrix/Quaternion.hpp

@@ -9,6 +9,7 @@
 #pragma once
 
 #include "math.hpp"
+#include "helper_functions.hpp"
 
 namespace matrix
 {
@@ -61,12 +62,12 @@ public:
         Vector<Type, 4>()
     {
         Quaternion &q = *this;
-        Type cosPhi_2 = Type(cos(euler.phi() / 2.0));
-        Type cosTheta_2 = Type(cos(euler.theta() / 2.0));
-        Type cosPsi_2 = Type(cos(euler.psi() / 2.0));
-        Type sinPhi_2 = Type(sin(euler.phi() / 2.0));
-        Type sinTheta_2 = Type(sin(euler.theta() / 2.0));
-        Type sinPsi_2 = Type(sin(euler.psi() / 2.0));
+        Type cosPhi_2 = Type(cos(euler.phi() / (Type)2.0));
+        Type cosTheta_2 = Type(cos(euler.theta() / (Type)2.0));
+        Type cosPsi_2 = Type(cos(euler.psi() / (Type)2.0));
+        Type sinPhi_2 = Type(sin(euler.phi() / (Type)2.0));
+        Type sinTheta_2 = Type(sin(euler.theta() / (Type)2.0));
+        Type sinPsi_2 = Type(sin(euler.psi() / (Type)2.0));
         q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
                sinPhi_2 * sinTheta_2 * sinPsi_2;
         q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
@@ -132,6 +133,69 @@ public:
         v(3) = w(2,0);
         return Q * v * Type(0.5);
     }
+
+    void invert() {
+        Quaternion &q = *this;
+        q(1) *= -1;
+        q(2) *= -1;
+        q(3) *= -1;
+    }
+
+    Quaternion inversed() {
+        Quaternion &q = *this;
+        Quaternion ret;
+        ret(0) = q(0);
+        ret(1) = -q(1);
+        ret(2) = -q(2);
+        ret(3) = -q(3);
+        return ret;
+    }
+
+    void rotate(const Vector<Type, 3> &vec) {
+        Quaternion res;
+        res.from_axis_angle(vec);
+        (*this) = (*this) * res;
+    }
+
+    void from_axis_angle(Vector<Type, 3> vec) {
+        Quaternion &q = *this;
+        Type theta = vec.norm();
+        if(theta < (Type)1e-10) {
+            q(0) = (Type)1.0;
+            q(1)=q(2)=q(3)=0;
+            return;
+        }
+        vec /= theta;
+        from_axis_angle(vec,theta);
+    }
+
+    void from_axis_angle(const Vector<Type, 3> &axis, Type theta) {
+        Quaternion &q = *this;
+        if(theta < (Type)1e-10) {
+            q(0) = (Type)1.0;
+            q(1)=q(2)=q(3)=0;
+        }
+        Type magnitude = sinf(theta/2.0f);
+
+        q(0) = cosf(theta/2.0f);
+        q(1) = axis(0) * magnitude;
+        q(2) = axis(1) * magnitude;
+        q(3) = axis(2) * magnitude;
+    }
+
+    Vector<Type, 3> to_axis_angle() {
+        Quaternion &q = *this;
+        Type axis_magnitude = Type(sqrt(q(1) * q(1) + q(2) * q(2) + q(3) * q(3)));
+        Vector<Type, 3> vec;
+        vec(0) = q(1);
+        vec(1) = q(2);
+        vec(2) = q(3);
+        if(axis_magnitude >= (Type)1e-10) {
+            vec = vec / axis_magnitude;
+            vec = vec * wrap_pi((Type)2.0 * atan2f(axis_magnitude,q(0)));
+        }
+        return vec;
+    }
 };
 
 typedef Quaternion<float> Quatf;

matrix/Vector.hpp

@@ -52,7 +52,7 @@ public:
     Type dot(const MatrixM1 & b) const {
         const Vector &a(*this);
         Type r = 0;
-        for (int i = 0; i<M; i++) {
+        for (size_t i = 0; i<M; i++) {
             r += a(i)*b(i,0);
         }
         return r;

matrix/helper_functions.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "math.hpp"
+#include <math.h>
+
+
+namespace matrix
+{
+template<typename Type>
+float wrap_pi(Type x)
+{
+    if (!isfinite(x)) {
+        return x;
+    }
+
+    while (x >= (Type)M_PI) {
+        x -= (Type)(2.0 * M_PI);
+
+    }
+
+    while (x < (Type)(-M_PI)) {
+        x += (Type)(2.0 * M_PI);
+
+    }
+
+    return x;
+}
+
+
+};

test/attitude.cpp

@@ -146,6 +146,66 @@ int main()
     q_scalar_mul_res3 *= scalar;
     assert(q_scalar_mul_check == q_scalar_mul_res3);
 
+    // quaternion inverse
+    q = q_check.inversed();
+    assert(fabsf(q_check(0) - q(0)) < eps);
+    assert(fabsf(q_check(1) + q(1)) < eps);
+    assert(fabsf(q_check(2) + q(2)) < eps);
+    assert(fabsf(q_check(3) + q(3)) < eps);
+
+    q = q_check;
+    q.invert();
+    assert(fabsf(q_check(0) - q(0)) < eps);
+    assert(fabsf(q_check(1) + q(1)) < eps);
+    assert(fabsf(q_check(2) + q(2)) < eps);
+    assert(fabsf(q_check(3) + q(3)) < eps);
+
+    // rotate quaternion (nonzero rotation)
+    Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
+    Vector<float, 3> rot;
+    rot(0) = 1.0f;
+    rot(1) = rot(2) = 0.0f;
+    qI.rotate(rot);
+    Quatf q_true(cosf(1.0f / 2), sinf(1.0f / 2), 0.0f, 0.0f);
+    assert(fabsf(qI(0) - q_true(0)) < eps);
+    assert(fabsf(qI(1) - q_true(1)) < eps);
+    assert(fabsf(qI(2) - q_true(2)) < eps);
+    assert(fabsf(qI(3) - q_true(3)) < eps);
+
+    // rotate quaternion (zero rotation)
+    qI = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+    rot(0) = 0.0f;
+    rot(1) = rot(2) = 0.0f;
+    qI.rotate(rot);
+    q_true = Quatf(cosf(0.0f), sinf(0.0f), 0.0f, 0.0f);
+    assert(fabsf(qI(0) - q_true(0)) < eps);
+    assert(fabsf(qI(1) - q_true(1)) < eps);
+    assert(fabsf(qI(2) - q_true(2)) < eps);
+    assert(fabsf(qI(3) - q_true(3)) < eps);
+
+    // get rotation axis from quaternion (nonzero rotation)
+    q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f);
+    rot = q.to_axis_angle();
+    assert(fabsf(rot(0)) < eps);
+    assert(fabsf(rot(1) -1.0f) < eps);
+    assert(fabsf(rot(2)) < eps);
+
+    // get rotation axis from quaternion (zero rotation)
+    q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+    rot = q.to_axis_angle();
+    assert(fabsf(rot(0)) < eps);
+    assert(fabsf(rot(1)) < eps);
+    assert(fabsf(rot(2)) < eps);
+
+    // from axis angle (zero rotation)
+    rot(0) = rot(1) = rot(2) = 0.0f;
+    q.from_axis_angle(rot, 0.0f);
+    q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+    assert(fabsf(q(0) - q_true(0)) < eps);
+    assert(fabsf(q(1) - q_true(1)) < eps);
+    assert(fabsf(q(2) - q_true(2)) < eps);
+    assert(fabsf(q(3) - q_true(3)) < eps);
+
 };
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */