Added axis angle attitude representation. (#25)

CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8)
 
-set(VERSION_MAJOR "0")
+set(VERSION_MAJOR "1")
-set(VERSION_MINOR "1")
+set(VERSION_MINOR "0")
 set(VERSION_PATCH "0")
 
 project(matrix CXX)

matrix/AxisAngle.hpp

@@ -0,0 +1,158 @@
+/**
+ * @file AxisAngle.hpp
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+#include "helper_functions.hpp"
+
+namespace matrix
+{
+
+template <typename Type>
+class Dcm;
+
+template <typename Type>
+class Euler;
+
+template <typename Type>
+class AxisAngle;
+
+/**
+ * AxisAngle class
+ *
+ * The rotation between two coordinate frames is
+ * described by this class.
+ */
+template<typename Type>
+class AxisAngle : public Vector<Type, 3>
+{
+public:
+    virtual ~AxisAngle() {};
+
+    typedef Matrix<Type, 3, 1> Matrix31;
+
+    /**
+     * Constructor from array
+     *
+     * @param data_ array
+     */
+    AxisAngle(const Type *data_) :
+        Vector<Type, 3>(data_)
+    {
+    }
+
+    /**
+     * Standard constructor
+     */
+    AxisAngle() :
+        Vector<Type, 3>()
+    {
+    }
+
+    /**
+     * Constructor from Matrix31
+     *
+     * @param other Matrix31 to copy
+     */
+    AxisAngle(const Matrix31 &other) :
+        Vector<Type, 3>(other)
+    {
+    }
+
+    /**
+     * Constructor from quaternion
+     *
+     * This sets the instance from a quaternion representing coordinate transformation from
+     * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
+     * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+     *
+     * @param q quaternion
+     */
+    AxisAngle(const Quaternion<Type> &q) :
+        Vector<Type, 3>()
+    {
+        AxisAngle &v = *this;
+        Type angle = (Type)2.0f*acosf(q(0));
+        Type mag = sinf(angle/2.0f);
+        v(0) = angle*q(1)/mag;
+        v(1) = angle*q(2)/mag;
+        v(2) = angle*q(3)/mag;
+    }
+
+    /**
+     * Constructor from dcm
+     *
+     * Instance is initialized from a dcm representing coordinate transformation
+     * from frame 2 to frame 1.
+     *
+     * @param dcm dcm to set quaternion to
+     */
+    AxisAngle(const Dcm<Type> &dcm) :
+        Vector<Type, 3>()
+    {
+        AxisAngle &v = *this;
+        v = Quaternion<float>(dcm);
+    }
+
+    /**
+     * Constructor from euler angles
+     *
+     * This sets the instance to a quaternion representing coordinate transformation from
+     * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
+     * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+     *
+     * @param euler euler angle instance
+     */
+    AxisAngle(const Euler<Type> &euler) :
+        Vector<Type, 3>()
+    {
+        AxisAngle &v = *this;
+        v = Quaternion<float>(euler);
+    }
+
+    /**
+     * Constructor from 3 axis angle values (unit vector * angle)
+     *
+     * @param x r_x*angle
+     * @param y r_y*angle
+     * @param z r_z*angle
+     */
+    AxisAngle(Type x, Type y, Type z) :
+        Vector<Type, 3>()
+    {
+        AxisAngle &v = *this;
+        v(0) = x;
+        v(1) = y;
+        v(2) = z;
+    }
+
+    /**
+     * Constructor from axis and angle
+     *
+     * @param x r_x*angle
+     * @param x r_x*angle
+     * @param y r_y*angle
+     * @param z r_z*angle
+     */
+    AxisAngle(const Matrix31 & axis, Type angle) :
+        Vector<Type, 3>()
+    {
+        AxisAngle &v = *this;
+        // make sure axis is a unit vector
+        Vector<Type, 3> a = axis;
+        a = a.unit();
+        v(0) = a(0)*angle;
+        v(1) = a(1)*angle;
+        v(2) = a(2)*angle;
+    }
+};
+
+typedef AxisAngle<float> AxisAnglef;
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

matrix/Dcm.hpp

@@ -27,6 +27,10 @@ class Quaternion;
 template<typename Type>
 class Euler;
 
+template<typename Type>
+class AxisAngle;
+
+
 /**
  * Direction cosine matrix class
  *
@@ -129,6 +133,21 @@ public:
         dcm(2, 2) = cosPhi * cosThe;
     }
 
+
+    /**
+     * Constructor from axis angle
+     *
+     * This sets the transformation matrix from frame 2 to frame 1 where the rotation
+     * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+     *
+     *
+     * @param euler euler angle instance
+     */
+    Dcm(const AxisAngle<Type> & aa) {
+        Dcm &dcm = *this;
+        dcm = Quaternion<Type>(aa);
+    }
+
     Vector<Type, 3> vee() const {    // inverse to Vector.hat() operation
         const Dcm &A(*this);
         Vector<Type, 3> v;

matrix/Quaternion.hpp

@@ -30,6 +30,10 @@ class Dcm;
 template <typename Type>
 class Euler;
 
+template <typename Type>
+class AxisAngle;
+
+
 /**
  * Quaternion class
  *
@@ -129,6 +133,30 @@ public:
                sinPhi_2 * sinTheta_2 * cosPsi_2;
     }
 
+    /**
+     * Quaternion from AxisAngle
+     *
+     * @param aa axis-angle vector
+     */
+    Quaternion(const AxisAngle<Type> &aa) :
+        Vector<Type, 4>()
+    {
+        Quaternion &q = *this;
+        Type angle = aa.norm();
+        Vector<Type, 3> axis = aa.unit();
+        if (angle < (Type)1e-10) {
+            q(0) = (Type)1.0;
+            q(1) = q(2) = q(3) = 0;
+        } else {
+            Type magnitude = sinf(angle / 2.0f);
+            q(0) = cosf(angle / 2.0f);
+            q(1) = axis(0) * magnitude;
+            q(2) = axis(1) * magnitude;
+            q(3) = axis(2) * magnitude;
+        }
+    }
+
+
     /**
      * Constructor from quaternion values
      *

matrix/math.hpp

@@ -12,3 +12,4 @@
 #include "Dcm.hpp"
 #include "Scalar.hpp"
 #include "Quaternion.hpp"
+#include "AxisAngle.hpp"

test/attitude.cpp

@@ -6,9 +6,16 @@
 
 using namespace matrix;
 
+// important to list all classes here for coverage
 template class Quaternion<float>;
 template class Euler<float>;
 template class Dcm<float>;
+template class AxisAngle<float>;
+template class Scalar<float>;
+template class SquareMatrix<float, 2>;
+template class Vector<float, 3>;
+template class Vector2<float>;
+template class Vector3<float>;
 
 int main()
 {
@@ -238,6 +245,30 @@ int main()
     for(int i = 0; i < 4; i++)
         TEST(fabsf(q_from_array(i) - q_array[i]) < eps);
 
+    // axis angle
+    AxisAnglef aa_true(Vector3f(1.0f, 2.0f, 3.0f));
+    TEST(isEqual(aa_true, Vector3f(1.0f, 2.0f, 3.0f)));
+    AxisAnglef aa_empty;
+    TEST(isEqual(aa_empty, AxisAnglef(0.0f, 0.0f, 0.0f)));
+    float aa_data[] =  {4.0f, 5.0f, 6.0f};
+    AxisAnglef aa_data_init(aa_data);
+    TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f)));
+
+    q = Quatf(-0.29555112749297824f, 0.25532186f,  0.51064372f,  0.76596558f);
+    AxisAnglef aa_q_init(q);
+    TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
+
+    AxisAnglef aa_euler_init(Eulerf(0.0f, 0.0f, 0.0f));
+    TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 1.0f)));
+
+    Dcmf dcm_aa_check = AxisAnglef(dcm_check);
+    TEST(isEqual(dcm_aa_check, dcm_check));
+
+    AxisAnglef aa_axis_angle_init(Vector3f(1.0f, 2.0f, 3.0f), 3.0f);
+    TEST(isEqual(aa_axis_angle_init, Vector3f(0.80178373f, 1.60356745f, 2.40535118f)));
+
+    TEST(isEqual(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))),
+                 Quatf(1.0f, 0.0f, 0.0f, 0.0f)));
 };
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

test/matrixAssignment.cpp

@@ -90,7 +90,11 @@ int main()
 
     Scalar<float> s;
     s = 1;
+    float const & s_float = (const Scalar<float>)s;
+    const Vector<float, 1> & s_vect = s;
     TEST(fabs(s - 1) < 1e-5);
+    TEST(fabs(s_float - 1.0f) < 1e-5);
+    TEST(fabs(s_vect(0) - 1.0f) < 1e-5);
 
     Matrix<float, 1, 1> m5 = s;
     TEST(fabs(m5(0,0) - s) < 1e-5);