From 34e6e2a941ce1f8dad6492c8d672a7d5ac5233c3 Mon Sep 17 00:00:00 2001
From: Andreas Bircher <bircher@gmx.ch>
Date: Tue, 3 May 2016 17:32:49 +0200
Subject: [PATCH] refactoring rotation classes and adding initial description

---
 matrix/Dcm.hpp        |  63 ++++++++++++++-
 matrix/Euler.hpp      |  91 +++++++++++++++++++++-
 matrix/Quaternion.hpp | 176 +++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 322 insertions(+), 8 deletions(-)

diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp
index e7379d916b..bd622731c9 100644
--- a/matrix/Dcm.hpp
+++ b/matrix/Dcm.hpp
@@ -20,6 +20,11 @@ class Quaternion;
 template<typename Type>
 class Euler;
 
+/**
+ * Direction cosine matrix class
+ *
+ * More elaborate class description
+ */
 template<typename Type>
 class Dcm : public Matrix<Type, 3, 3>
 {
@@ -28,20 +33,69 @@ public:
 
     typedef Matrix<Type, 3, 1> Vector3;
 
+    /**
+     * Standard constructor
+     *
+     * Initializes to identity
+     */
     Dcm() : Matrix<Type, 3, 3>()
     {
         (*this) = eye<Type, 3>();
     }
 
+    /**
+     * Constructor from array
+     *
+     * Copies the given scalar value to all matrix fields.
+     *
+     * @param other array
+     */
     Dcm(const Type *data_) : Matrix<Type, 3, 3>(data_)
     {
     }
 
+    /**
+     * Copy constructor
+     *
+     * More elaborate function description
+     *
+     * @param other Matrix33 to set dcm to
+     */
     Dcm(const Matrix<Type, 3, 3> &other) : Matrix<Type, 3, 3>(other)
     {
     }
 
+    /**
+     * Constructor from quaternion
+     *
+     * More elaborate function description
+     *
+     * @param q quaternion to set dcm to
+     */
     Dcm(const Quaternion<Type> & q) {
+        set_from_quaternion(q);
+    }
+
+    /**
+     * Constructor from euler angles
+     *
+     * More elaborate function description
+     *
+     * @param euler euler angles to set dcm to
+     */
+    Dcm(const Euler<Type> & euler) {
+        set_from_euler(euler);
+    }
+
+
+    /**
+     * Set from quaternion
+     *
+     * More elaborate function description
+     *
+     * @param q quaternion to set dcm to
+     */
+    void set_from_quaternion(const Quaternion<Type> & q) {
         Dcm &dcm = *this;
         Type a = q(0);
         Type b = q(1);
@@ -62,7 +116,14 @@ public:
         dcm(2, 2) = aSq - bSq - cSq + dSq;
     }
 
-    Dcm(const Euler<Type> & euler) {
+    /**
+     * Set from euler angles
+     *
+     * More elaborate function description
+     *
+     * @param euler euler angles to set dcm to
+     */
+    void set_from_euler(const Euler<Type> & euler) {
         Dcm &dcm = *this;
         Type cosPhi = Type(cos(euler.phi()));
         Type sinPhi = Type(sin(euler.phi()));
diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp
index 57dcbb48a2..24ffd326df 100644
--- a/matrix/Euler.hpp
+++ b/matrix/Euler.hpp
@@ -23,34 +23,113 @@ class Dcm;
 template <typename Type>
 class Quaternion;
 
+/**
+ * Euler angles class
+ *
+ * More elaborate class description
+ */
 template<typename Type>
 class Euler : public Vector<Type, 3>
 {
 public:
     virtual ~Euler() {};
 
+    /**
+     * Standard constructor
+     *
+     * More elaborate function description
+     */
     Euler() : Vector<Type, 3>()
     {
     }
 
+    /**
+     * Copy constructor
+     *
+     * More elaborate function description
+     *
+     * @param other vector to copy
+     */
     Euler(const Vector<Type, 3> & other) :
         Vector<Type, 3>(other)
     {
     }
 
+    /**
+     * Constructor from Matrix31
+     *
+     * More elaborate function description
+     *
+     * @param other Matrix31 to copy
+     */
     Euler(const Matrix<Type, 3, 1> & other) :
         Vector<Type, 3>(other)
     {
     }
 
+    /**
+     * Constructor from euler angles
+     *
+     * More elaborate function description
+     *
+     * @param phi_   roll
+     * @param theta_ pitch
+     * @param psi_   yaw
+     */
     Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
+    {
+        set_from_euler(phi_, theta_, psi_);
+    }
+
+    /**
+     * Constructor from dcm
+     *
+     * More elaborate function description
+     *
+     * @param dcm_ dcm to set angles to
+     */
+    Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
+    {
+        set_from_dcm(dcm);
+    }
+
+    /**
+     * Constructor from quaternion
+     *
+     * More elaborate function description
+     *
+     * @param q quaternion to set angles to
+     */
+    Euler(const Quaternion<Type> & q) :
+        Vector<Type, 3>()
+    {
+        set_from_quaternion(q);
+    }
+
+    /**
+     * Set from euler angles
+     *
+     * More elaborate function description
+     *
+     * @param phi_   roll
+     * @param theta_ pitch
+     * @param psi_   yaw
+     */
+    void set_from_euler(Type phi_, Type theta_, Type psi_)
     {
         phi() = phi_;
         theta() = theta_;
         psi() = psi_;
     }
 
-    Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
+    /**
+     * Set from dcm
+     *
+     * More elaborate function description
+     *
+     * @param dcm_ dcm to set angles to
+     */
+    void set_from_dcm(const Dcm<Type> & dcm)
     {
         Type phi_val = Type(atan2(dcm(2,1), dcm(2,2)));
         Type theta_val = Type(asin(-dcm(2,0)));
@@ -70,8 +149,14 @@ public:
         psi() = psi_val;
     }
 
-    Euler(const Quaternion<Type> & q) :
-        Vector<Type, 3>()
+    /**
+     * Set from dcm
+     *
+     * More elaborate function description
+     *
+     * @param q quaternion to set angles to
+     */
+    void set_from_quaternion(const Quaternion<Type> & q)
     {
         *this = Euler(Dcm<Type>(q));
     }
diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index 7b845b1475..e135464537 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -20,6 +20,11 @@ class Dcm;
 template <typename Type>
 class Euler;
 
+/**
+ * Quaternion class
+ *
+ * More elaborate class description
+ */
 template<typename Type>
 class Quaternion : public Vector<Type, 4>
 {
@@ -29,11 +34,23 @@ public:
     typedef Matrix<Type, 4, 1> Matrix41;
     typedef Matrix<Type, 3, 1> Matrix31;
 
+    /**
+     * Constructor from array
+     *
+     * Copies the given scalar value to all quaternion fields.
+     *
+     * @param data_ array
+     */
     Quaternion(const Type *data_) :
         Vector<Type, 4>(data_)
     {
     }
 
+    /**
+     * Standard constructor
+     *
+     * More elaborate function description
+     */
     Quaternion() :
         Vector<Type, 4>()
     {
@@ -44,13 +61,68 @@ public:
         q(3) = 0;
     }
 
+    /**
+     * Constructor from Matrix41
+     *
+     * More elaborate function description
+     *
+     * @param other Matrix41 to copy
+     */
     Quaternion(const Matrix41 & other) :
         Vector<Type, 4>(other)
     {
     }
 
+    /**
+     * Constructor from dcm
+     *
+     * More elaborate function description
+     *
+     * @param dcm dcm to set quaternion to
+     */
     Quaternion(const Dcm<Type> & dcm) :
         Vector<Type, 4>()
+    {
+        set_from_dcm(dcm);
+    }
+
+    /**
+     * Constructor from euler angles
+     *
+     * More elaborate function description
+     *
+     * @param euler euler angles to set quaternion to
+     */
+    Quaternion(const Euler<Type> & euler) :
+        Vector<Type, 4>()
+    {
+        set_from_euler(euler);
+    }
+
+    /**
+     * Constructor from quaternion values
+     *
+     * More elaborate function description
+     *
+     * @param a set quaternion value 0
+     * @param b set quaternion value 1
+     * @param c set quaternion value 2
+     * @param d set quaternion value 3
+     */
+    Quaternion(Type a, Type b, Type c, Type d) :
+        Vector<Type, 4>()
+    {
+        set_from_quaternion(a, b, c, d);
+    }
+
+    /**
+     * Set from dcm
+     *
+     * More elaborate function description
+     *
+     * @param dcm dcm to set quaternion to
+     */
+    void set_from_dcm(const Dcm<Type> & dcm)
     {
         Quaternion &q = *this;
         q(0) = Type(0.5 * sqrt(1 + dcm(0, 0) +
@@ -63,8 +135,14 @@ public:
                     (4 * q(0)));
     }
 
-    Quaternion(const Euler<Type> & euler) :
-        Vector<Type, 4>()
+    /**
+     * Set from euler angles
+     *
+     * More elaborate function description
+     *
+     * @param euler euler angles to set quaternion to
+     */
+    void set_from_euler(const Euler<Type> & euler)
     {
         Quaternion &q = *this;
         Type cosPhi_2 = Type(cos(euler.phi() / (Type)2.0));
@@ -83,8 +161,17 @@ public:
                sinPhi_2 * sinTheta_2 * cosPsi_2;
     }
 
-    Quaternion(Type a, Type b, Type c, Type d) :
-        Vector<Type, 4>()
+    /**
+     * Set from quaternion values
+     *
+     * More elaborate function description
+     *
+     * @param a set quaternion value 0
+     * @param b set quaternion value 1
+     * @param c set quaternion value 2
+     * @param d set quaternion value 3
+     */
+    void set_from_quaternion(Type a, Type b, Type c, Type d)
     {
         Quaternion &q = *this;
         q(0) = a;
@@ -93,6 +180,14 @@ public:
         q(3) = d;
     }
 
+    /**
+     * Quaternion multiplication operator
+     *
+     * More elaborate function description
+     *
+     * @param q quaternion to multiply with
+     * @return product
+     */
     Quaternion operator*(const Quaternion &q) const
     {
         const Quaternion &p = *this;
@@ -104,24 +199,53 @@ public:
         return r;
     }
 
+    /**
+     * Self-multiplication operator
+     *
+     * More elaborate function description
+     *
+     * @param other quaternion to multiply with
+     */
     void operator*=(const Quaternion & other)
     {
         Quaternion &self = *this;
         self = self * other;
     }
 
+    /**
+     * Scalar multiplication operator
+     *
+     * More elaborate function description
+     *
+     * @param scalar scalar to multiply with
+     * @return product
+     */
     Quaternion operator*(Type scalar) const
     {
         const Quaternion &q = *this;
         return scalar * q;
     }
 
+    /**
+     * Scalar self-multiplication operator
+     *
+     * More elaborate function description
+     *
+     * @param scalar scalar to multiply with
+     */
     void operator*=(Type scalar)
     {
         Quaternion &q = *this;
         q = q * scalar;
     }
 
+    /**
+     * Computes the derivative
+     *
+     * More elaborate function description
+     *
+     * @param w direction
+     */
     Matrix41 derivative(const Matrix31 & w) const {
         const Quaternion &q = *this;
         Type dataQ[] = {
@@ -139,6 +263,11 @@ public:
         return Q * v * Type(0.5);
     }
 
+    /**
+     * Invert quaternion
+     *
+     * More elaborate function description
+     */
     void invert() {
         Quaternion &q = *this;
         q(1) *= -1;
@@ -146,6 +275,13 @@ public:
         q(3) *= -1;
     }
 
+    /**
+     * Invert quaternion
+     *
+     * More elaborate function description
+     *
+     * @return inverted quaternion
+     */
     Quaternion inversed() {
         Quaternion &q = *this;
         Quaternion ret;
@@ -156,12 +292,27 @@ public:
         return ret;
     }
 
+    /**
+     * Rotate quaternion from rotation vector
+     *
+     * More elaborate function description
+     *
+     * @param vec rotation vector
+     */
     void rotate(const Vector<Type, 3> &vec) {
         Quaternion res;
         res.from_axis_angle(vec);
         (*this) = (*this) * res;
     }
 
+    /**
+     * Rotation quaternion from vector
+     *
+     * More elaborate function description
+     *
+     * @param vec rotation vector
+     * @return quaternion representing the rotation
+     */
     void from_axis_angle(Vector<Type, 3> vec) {
         Quaternion &q = *this;
         Type theta = vec.norm();
@@ -174,6 +325,15 @@ public:
         from_axis_angle(vec,theta);
     }
 
+    /**
+     * Rotation quaternion from axis and scalar
+     *
+     * More elaborate function description
+     *
+     * @param axis axis of rotation
+     * @param theta scalar describing angle of rotation
+     * @return quaternion representing the rotation
+     */
     void from_axis_angle(const Vector<Type, 3> &axis, Type theta) {
         Quaternion &q = *this;
         if(theta < (Type)1e-10) {
@@ -188,6 +348,14 @@ public:
         q(3) = axis(2) * magnitude;
     }
 
+
+    /**
+     * Rotation vector from quaternion
+     *
+     * More elaborate function description
+     *
+     * @return vector, direction representing rotation axis and norm representing angle
+     */
     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)));