ARGoS  3
A parallel, multi-engine simulator for swarm robotics
core/utility/math/quaternion.h
Go to the documentation of this file.
00001 
00007 #ifndef CQUATERNION_H
00008 #define CQUATERNION_H
00009 
00010 #include <argos3/core/utility/math/vector3.h>
00011 
00012 namespace argos {
00013 
00014    class CQuaternion {
00015 
00016    public:
00017       CQuaternion() {
00018          m_fValues[0] = 1.0;
00019          m_fValues[1] = 0.0;
00020          m_fValues[2] = 0.0;
00021          m_fValues[3] = 0.0;
00022       }
00023 
00024       CQuaternion(const CQuaternion& c_quaternion) {
00025          *this = c_quaternion;
00026       }
00027 
00028       CQuaternion(Real f_real,
00029                   Real f_img1,
00030                   Real f_img2,
00031                   Real f_img3) {
00032          m_fValues[0] = f_real;
00033          m_fValues[1] = f_img1;
00034          m_fValues[2] = f_img2;
00035          m_fValues[3] = f_img3;
00036       }
00037 
00038       CQuaternion(const CRadians& c_radians,
00039                   const CVector3& c_vector3) {
00040          FromAngleAxis(c_radians, c_vector3);
00041       }
00042 
00043       inline CQuaternion(const CVector3& c_vector1,
00044                          const CVector3& c_vector2) {
00045          BetweenTwoVectors(c_vector1, c_vector2);
00046       }
00047 
00048       inline Real GetW() const {
00049          return m_fValues[0];
00050       }
00051 
00052       inline Real GetX() const {
00053          return m_fValues[1];
00054       }
00055 
00056       inline Real GetY() const {
00057          return m_fValues[2];
00058       }
00059 
00060       inline Real GetZ() const {
00061          return m_fValues[3];
00062       }
00063 
00064       inline CQuaternion Conjugate() const {
00065          return CQuaternion(m_fValues[0],
00066                             -m_fValues[1],
00067                             -m_fValues[2],
00068                             -m_fValues[3]);
00069       }
00070 
00071       inline CQuaternion Inverse() const {
00072          return CQuaternion(m_fValues[0],
00073                             -m_fValues[1],
00074                             -m_fValues[2],
00075                             -m_fValues[3]);
00076       }
00077 
00078       inline Real Length() const {
00079          return ::sqrt(SquareLength());
00080       }
00081 
00082       inline Real SquareLength() const {
00083          return
00084             Square(m_fValues[0]) +
00085             Square(m_fValues[1]) +
00086             Square(m_fValues[2]) +
00087             Square(m_fValues[3]);
00088       }
00089 
00090       inline CQuaternion& Normalize() {
00091          Real fInvLength = 1.0 / Length();
00092          m_fValues[0] *= fInvLength;
00093          m_fValues[1] *= fInvLength;
00094          m_fValues[2] *= fInvLength;
00095          m_fValues[3] *= fInvLength;
00096          return *this;
00097       }
00098 
00099       inline CQuaternion& FromAngleAxis(const CRadians& c_angle,
00100                                         const CVector3& c_vector) {
00101          CRadians cHalfAngle = c_angle * 0.5;
00102          Real fSin, fCos;
00103 #ifdef ARGOS_SINCOS
00104          SinCos(cHalfAngle, fSin, fCos);
00105 #else
00106          fSin = Sin(cHalfAngle);
00107          fCos = Cos(cHalfAngle);
00108 #endif
00109          m_fValues[0] = fCos;
00110          m_fValues[1] = c_vector.GetX() * fSin;
00111          m_fValues[2] = c_vector.GetY() * fSin;
00112          m_fValues[3] = c_vector.GetZ() * fSin;
00113          return *this;
00114       }
00115 
00116       inline void ToAngleAxis(CRadians& c_angle,
00117                               CVector3& c_vector) const {
00118          Real fSquareLength =
00119             Square(m_fValues[1]) +
00120             Square(m_fValues[2]) +
00121             Square(m_fValues[3]);
00122          if(fSquareLength > 0.0f) {
00123             c_angle = 2.0f * ACos(m_fValues[0]);
00124             Real fInvLength = 1.0f / ::sqrt(fSquareLength);
00125             c_vector.Set(m_fValues[1] * fInvLength,
00126                          m_fValues[2] * fInvLength,
00127                          m_fValues[3] * fInvLength);
00128          }
00129          else {
00130             /* By default, to ease the support of robot orientation, no rotation refers to the Z axis */
00131             c_angle = CRadians::ZERO;
00132             c_vector = CVector3::Z;
00133          }
00134       }
00135 
00136       inline CQuaternion& FromEulerAngles(const CRadians& c_z_angle,
00137                                           const CRadians& c_y_angle,
00138                                           const CRadians& c_x_angle) {
00139          (*this) = CQuaternion(c_x_angle, CVector3::X) *
00140             CQuaternion(c_y_angle, CVector3::Y) *
00141             CQuaternion(c_z_angle, CVector3::Z);
00142          return (*this);
00143       }
00144 
00145       inline void ToEulerAngles(CRadians& c_z_angle,
00146                                 CRadians& c_y_angle,
00147                                 CRadians& c_x_angle) const {
00148          /* With the ZYX convention, gimbal lock happens when
00149             cos(y_angle) = 0, that is when y_angle = +- pi/2
00150             In this condition, the Z and X axis overlap and we
00151             lose one degree of freedom. It's a problem of the
00152             Euler representation of rotations that is not
00153             present when we deal with quaternions.
00154             For reasons of speed, we consider gimbal lock
00155             happened when fTest > 0.499 and when fTest < -0.499.
00156          */
00157          /* Computed to understand if we have gimbal lock or not */
00158          Real fTest =
00159             m_fValues[1] * m_fValues[3] +
00160             m_fValues[0] * m_fValues[2];
00161 
00162          if(fTest > 0.499f) {
00163             /* Gimbal lock */
00164             c_x_angle = CRadians::ZERO;
00165             c_y_angle = CRadians::PI_OVER_TWO;
00166             c_z_angle = ATan2(2.0f * (m_fValues[1] * m_fValues[2] + m_fValues[0] * m_fValues[3]),
00167                               1.0f - 2.0f * (m_fValues[1] * m_fValues[1] + m_fValues[3] * m_fValues[3]));
00168          }
00169          else if(fTest < -0.499f) {
00170             /* Gimbal lock */
00171             c_x_angle = CRadians::ZERO;
00172             c_y_angle = -CRadians::PI_OVER_TWO;
00173             c_z_angle = ATan2(2.0f * (m_fValues[1] * m_fValues[2] + m_fValues[0] * m_fValues[3]),
00174                               1.0f - 2.0f * (m_fValues[1] * m_fValues[1] + m_fValues[3] * m_fValues[3]));
00175          }
00176          else {
00177             /* Normal case */
00178             Real fSqValues[4] = {
00179                Square(m_fValues[0]),
00180                Square(m_fValues[1]),
00181                Square(m_fValues[2]),
00182                Square(m_fValues[3])
00183             };
00184             
00185             c_x_angle = ATan2(2.0 * (m_fValues[0] * m_fValues[1] - m_fValues[2] * m_fValues[3]),
00186                               fSqValues[0] - fSqValues[1] - fSqValues[2] + fSqValues[3]);
00187             c_y_angle = ASin(2.0 * (m_fValues[1] * m_fValues[3] + m_fValues[0] * m_fValues[2]));
00188             c_z_angle = ATan2(2.0 * (m_fValues[0] * m_fValues[3] - m_fValues[1] * m_fValues[2]),
00189                               fSqValues[0] + fSqValues[1] - fSqValues[2] - fSqValues[3]);
00190          }
00191       }
00192 
00193       inline CQuaternion& BetweenTwoVectors(const CVector3& c_vector1,
00194                                             const CVector3& c_vector2) {
00195          m_fValues[0] = ::sqrt(c_vector1.SquareLength() * c_vector2.SquareLength()) + c_vector1.DotProduct(c_vector2);
00196          CVector3 cCrossProd(c_vector1);
00197          cCrossProd.CrossProduct(c_vector2);
00198          m_fValues[1] = cCrossProd.GetX();
00199          m_fValues[2] = cCrossProd.GetY();
00200          m_fValues[3] = cCrossProd.GetZ();
00201          Normalize();
00202          return *this;
00203       }
00204 
00205       inline bool operator==(const CQuaternion& c_quaternion) {
00206          return (m_fValues[0] == c_quaternion.m_fValues[0] &&
00207                  m_fValues[1] == c_quaternion.m_fValues[1] &&
00208                  m_fValues[2] == c_quaternion.m_fValues[2] &&
00209                  m_fValues[3] == c_quaternion.m_fValues[3]);
00210       }      
00211 
00212       inline CQuaternion& operator=(const CQuaternion& c_quaternion) {
00213          if (&c_quaternion != this) {
00214             m_fValues[0] = c_quaternion.m_fValues[0];
00215             m_fValues[1] = c_quaternion.m_fValues[1];
00216             m_fValues[2] = c_quaternion.m_fValues[2];
00217             m_fValues[3] = c_quaternion.m_fValues[3];
00218          }
00219          return *this;
00220       }
00221 
00222       inline CQuaternion& operator+=(const CQuaternion& c_quaternion) {
00223          m_fValues[0] += c_quaternion.m_fValues[0];
00224          m_fValues[1] += c_quaternion.m_fValues[1];
00225          m_fValues[2] += c_quaternion.m_fValues[2];
00226          m_fValues[3] += c_quaternion.m_fValues[3];
00227          return *this;
00228       }
00229 
00230       inline CQuaternion& operator-=(const CQuaternion& c_quaternion) {
00231          m_fValues[0] -= c_quaternion.m_fValues[0];
00232          m_fValues[1] -= c_quaternion.m_fValues[1];
00233          m_fValues[2] -= c_quaternion.m_fValues[2];
00234          m_fValues[3] -= c_quaternion.m_fValues[3];
00235          return *this;
00236       }
00237 
00238       inline CQuaternion& operator*=(const CQuaternion& c_quaternion) {
00239          Real newv[4];
00240          newv[0] = m_fValues[0] * c_quaternion.m_fValues[0] -
00241             m_fValues[1] * c_quaternion.m_fValues[1] -
00242             m_fValues[2] * c_quaternion.m_fValues[2] -
00243             m_fValues[3] * c_quaternion.m_fValues[3];
00244          newv[1] = m_fValues[0] * c_quaternion.m_fValues[1] +
00245             m_fValues[1] * c_quaternion.m_fValues[0] +
00246             m_fValues[2] * c_quaternion.m_fValues[3] -
00247             m_fValues[3] * c_quaternion.m_fValues[2];
00248          newv[2] = m_fValues[0] * c_quaternion.m_fValues[2] -
00249             m_fValues[1] * c_quaternion.m_fValues[3] +
00250             m_fValues[2] * c_quaternion.m_fValues[0] +
00251             m_fValues[3] * c_quaternion.m_fValues[1];
00252          newv[3] = m_fValues[0] * c_quaternion.m_fValues[3] +
00253             m_fValues[1] * c_quaternion.m_fValues[2] -
00254             m_fValues[2] * c_quaternion.m_fValues[1] +
00255             m_fValues[3] * c_quaternion.m_fValues[0];
00256          m_fValues[0] = newv[0];
00257          m_fValues[1] = newv[1];
00258          m_fValues[2] = newv[2];
00259          m_fValues[3] = newv[3];
00260          return *this;
00261       }
00262 
00263       inline CQuaternion operator+(const CQuaternion& c_quaternion) const {
00264          CQuaternion result(*this);
00265          result += c_quaternion;
00266          return result;
00267       }
00268 
00269       inline CQuaternion operator-(const CQuaternion& c_quaternion) const {
00270          CQuaternion result(*this);
00271          result -= c_quaternion;
00272          return result;
00273       }
00274 
00275       inline CQuaternion operator*(const CQuaternion& c_quaternion) const {
00276          CQuaternion result(*this);
00277          result *= c_quaternion;
00278          return result;
00279       }
00280 
00288       inline friend std::ostream& operator<<(std::ostream& c_os, const CQuaternion& c_quaternion) {
00289          CRadians cZAngle, cYAngle, cXAngle;
00290          c_quaternion.ToEulerAngles(cZAngle, cYAngle, cXAngle);        
00291          c_os << ToDegrees(cZAngle).GetValue() << ","
00292               << ToDegrees(cYAngle).GetValue() << ","
00293               << ToDegrees(cXAngle).GetValue();
00294          return c_os;
00295       }
00296       
00304       inline friend std::istream& operator>>(std::istream& c_is, CQuaternion& c_quaternion) {
00305          Real fValues[3];
00306          ParseValues<Real>(c_is, 3, fValues, ',');
00307          c_quaternion.FromEulerAngles(ToRadians(CDegrees(fValues[0])),
00308                                       ToRadians(CDegrees(fValues[1])), 
00309                                       ToRadians(CDegrees(fValues[2])));
00310          return c_is;
00311       }
00312 
00313    private:
00314 
00315       Real m_fValues[4];
00316    };
00317 
00318 }
00319 
00320 #endif