CQuaternion.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2010  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */

#ifndef CQuaternion_H
#define CQuaternion_H

#include <mrpt/math/CMatrixTemplateNumeric.h>
#include <mrpt/math/CVectorTemplate.h>
#include <mrpt/math/CArray.h>

namespace mrpt
{
        namespace math
        {
                /** General functions for quaternion. This quaternion class represents a 3D rotation as a complex vector
                 * with 3 imaginary parts x, y, z and 1 real part r. Where q = r + ix + jy + kz.
                 *
                 * For more information about quaternions, see:
                 *  - http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
                 */
                template <class T>
                class CQuaternion : public CArrayNumeric<T,4>
                {
                        typedef CArrayNumeric<T,4> BASE;
                public:
        /* @{ Constructors
         */

                /**     Default constructor: construct a (0,0,0,0) quaternion representing no rotation. */
                CQuaternion() { }

                /**     Construct a quaternion from its parameters 'r', 'x', 'y', 'z', with q = r + ix + jy + kz. */
                inline CQuaternion(const T r,const T x,const T y,const T z)
                {
                        (*this)[0] = r;
                        (*this)[1] = x;
                        (*this)[2] = y;
                        (*this)[3] = z;
                }

                /** Construct a quaternion from a 3D input vector (Rodrigues rotation 3-vector)
                */
                template <class ARRAYLIKE>
                inline CQuaternion(const ARRAYLIKE &in)
                {
                        fromRodriguesVector(in);
                }

                /* @}
                 */


                inline T  r()const {return (*this)[0];} //!< Return r coordinate of the quaternion
                inline T  x()const {return (*this)[1];} //!< Return x coordinate of the quaternion
                inline T  y()const {return (*this)[2];} //!< Return y coordinate of the quaternion
                inline T  z()const {return (*this)[3];} //!< Return z coordinate of the quaternion
                inline void  r(const T r) {(*this)[0]=r;}       //!< Set r coordinate of the quaternion
                inline void  x(const T x) {(*this)[1]=x;}       //!< Set x coordinate of the quaternion
                inline void  y(const T y) {(*this)[2]=y;}       //!< Set y coordinate of the quaternion
                inline void  z(const T z) {(*this)[3]=z;}       //!< Set z coordinate of the quaternion

                /**     Set this quaternion to the rotation described by a 3D Rodrigues rotation vector.
                  */
                template <class ARRAYLIKE>
                void  fromRodriguesVector(const ARRAYLIKE &in)
                {
                        if (in.size()!=3) THROW_EXCEPTION("Wrong Dimension in input vector for quaternion Constructor");

                        const T x = in[0];
                        const T y = in[1];
                        const T z = in[2];
                        if ((x==0)&&(y==0)&&(z==0))
                        {
                                (*this)[0] = 1;
                                (*this)[1] = 0;
                                (*this)[2] = 0;
                                (*this)[3] = 0;
                        }
                        else
                        {
                                const T angle = sqrt(x*x+y*y+z*z);
                                const T s = (sin(angle/2))/angle;
                                const T c = cos(angle/2);
                                (*this)[0] = c;
                                (*this)[1] = x * s;
                                (*this)[2] = y * s;
                                (*this)[3] = z * s;
                        }
                }

                /**     Calculate the cross product of two quaternion: this = qin1 x qin2
                */
                inline void  crossProduct(const CQuaternion &qin1, const CQuaternion &qin2)
                {
                        T q1r = qin1.r();
                        T q1x = qin1.x();
                        T q1y = qin1.y();
                        T q1z = qin1.z();

                        T q2r = qin2.r();
                        T q2x = qin2.x();
                        T q2y = qin2.y();
                        T q2z = qin2.z();

                        r(  q1r*q2r - q1x*q2x - q1y*q2y - q1z*q2z );
                        x(  q1r*q2x + q2r*q1x + q1y*q2z - q2y*q1z );
            y(  q1r*q2y + q2r*q1y + q1z*q2x - q2z*q1x );
                        z(  q1r*q2z + q2r*q1z + q1x*q2y - q2x*q1y );

                        normalize();
                }

                /**     Normalize this quaternion.
                  */
                inline void normalize()
                {
                        const T qq = 1.0/sqrt( square(r())+square(x())+square(y())+square(z()));
                        for (unsigned int i=0;i<4;i++)
                                (*this)[i] *= qq;
                }

                /** Calculate the normalized 4x4 Jacobian of a quaternion
                  *  The output matrix can be a dynamic or fixed size (4x4) matrix.
                  * JL: TODO: Document, the jacobian of what wrt what!?!?
                  */
                template <class MATRIXLIKE>
                void  normalizedJacobian(MATRIXLIKE &J) const
                {
                        const T n = square(r()) + square(x()) + square(y()) + square(z());
                        n = 1.0 / (n*sqrt(n));

                        J.setSize(4,4);
                        J.get_unsafe(0,0)=x()*x()+y()*y()+z()*z();
                        J.get_unsafe(0,1)=-r()*x();
                        J.get_unsafe(0,2)=-r()*y();
                        J.get_unsafe(0,3)=-r()*z();

                        J.get_unsafe(1,0)=-x()*r();
                        J.get_unsafe(1,1)=r()*r()+y()*y()+z()*z();
                        J.get_unsafe(1,2)=-x()*y();
                        J.get_unsafe(1,3)=-x()*z();

                        J.get_unsafe(2,0)=-y()*r();
                        J.get_unsafe(2,1)=-y()*x();
                        J.get_unsafe(2,2)=r()*r()+x()*x()+z()*z();
                        J.get_unsafe(2,3)=-y()*z();

                        J.get_unsafe(3,0)=-z()*r();
                        J.get_unsafe(3,1)=-z()*x();
                        J.get_unsafe(3,2)=-z()*y();
                        J.get_unsafe(3,3)=r()*r()+x()*x()+y()*y();
                        J *=n;
                }

                /** Calculate the 3x3 rotation matrix associated to this quaternion */
                template <class MATRIXLIKE>
        inline void  rotationMatrix(MATRIXLIKE &M) const
                {
                        M.setSize(3,3);
                        M.get_unsafe(0,0)=r()*r()+x()*x()-y()*y()-z()*z();              M.get_unsafe(0,1)=2*(x()*y() -r()*z());                 M.get_unsafe(0,2)=2*(z()*x()+r()*y());
                        M.get_unsafe(1,0)=2*(x()*y()+r()*z());                          M.get_unsafe(1,1)=r()*r()-x()*x()+y()*y()-z()*z();              M.get_unsafe(1,2)=2*(y()*z()-r()*x());
                        M.get_unsafe(2,0)=2*(z()*x()-r()*y());                          M.get_unsafe(2,1)=2*(y()*z()+r()*x());                          M.get_unsafe(2,2)=r()*r()-x()*x()-y()*y()+z()*z();
                }

                /**     Return the conjugate quaternion  */
                inline void conj(CQuaternion &q_out) const
                {
                        q_out.r( r() );
                        q_out.x(-x() );
                        q_out.y(-y() );
                        q_out.z(-z() );
                }

                /**     Return the conjugate quaternion  */
                inline CQuaternion conj() const
                {
                        CQuaternion q_aux;
                        conj(q_aux);
                        return q_aux;
                }

                /**     Return the yaw, pitch & roll angles associated to quaternion
                */
                inline void rpy(T &roll, T &pitch, T &yaw) const
                {
                        // TODO: Handle special cases as explained in:
                        // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
                        roll=atan2(2*(y()*z()+r()*x()),r()*r()-x()*x()-y()*y()+z()*z());
                        pitch=asin(-2*(x()*z()-r()*y()));
                        yaw=atan2(2*(x()*y()+r()*z()),r()*r()+x()*x()-y()*y()-z()*z());
                }

                inline CQuaternion  operator * (const T &factor)
                {
                        CQuaternion q = *this;
                        q*=factor;
                        return q;
                }

                };      // end class

                typedef CQuaternion<double> CQuaternionDouble;  //!< A quaternion of data type "double"
                typedef CQuaternion<float>  CQuaternionFloat;   //!< A quaternion of data type "float"

        }       // end namespace

} // end namespace mrpt

#endif