[2]  1  // %flair:license{


[15]  2  // This file is part of the Flair framework distributed under the


 3  // CECILLC License, Version 1.0.


[2]  4  // %flair:license}


 5  // created: 2016/02/03


 6  // filename: Quaternion.cpp


 7  //


 8  // author: Guillaume Sanahuja


 9  // Copyright Heudiasyc UMR UTC/CNRS 7253


 10  //


 11  // version: $Id: $


 12  //


 13  // purpose: Class defining a quaternion


 14  //


 15  //


 16  /*********************************************************************/


 17 


 18  #include "Quaternion.h"


 19  #include "Vector3D.h"


 20  #include "Euler.h"


 21  #include "RotationMatrix.h"


 22  #include <math.h>


 23  #include <stdio.h>


 24 


 25  namespace flair {


 26  namespace core {


 27 


[15]  28  Quaternion::Quaternion(float inQ0, float inQ1, float inQ2, float inQ3)


 29  : q0(inQ0), q1(inQ1), q2(inQ2), q3(inQ3) {}


[2]  30 


[15]  31  Quaternion::~Quaternion() {}


[2]  32 


[15]  33  Quaternion &Quaternion::operator=(const Quaternion &quaternion) {


 34  q0 = quaternion.q0;


 35  q1 = quaternion.q1;


 36  q2 = quaternion.q2;


 37  q3 = quaternion.q3;


 38  return (*this);


[2]  39  }


 40 


 41  float Quaternion::GetNorm(void) const {


[15]  42  return sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);


[2]  43  }


 44 


 45  void Quaternion::Normalize(void) {


[15]  46  float n = GetNorm();


 47  if (n != 0) {


 48  q0 = q0 / n;


 49  q1 = q1 / n;


 50  q2 = q2 / n;


 51  q3 = q3 / n;


 52  }


[2]  53  }


 54 


 55  void Quaternion::Conjugate(void) {


[15]  56  q1 = q1;


 57  q2 = q2;


 58  q3 = q3;


[2]  59  }


 60 


 61  Quaternion Quaternion::GetConjugate(void) {


[15]  62  return Quaternion(q0, q1, q2, q3);


[2]  63  }


 64 


 65  void Quaternion::GetLogarithm(Vector3D &logarithm) {


[15]  66  Normalize();


 67  float v_norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3);


[2]  68 


[15]  69  if (v_norm != 0) {


 70  float v_arccos = acosf(q0);


 71  logarithm.x = (q1 * v_arccos) / v_norm;


 72  logarithm.y = (q2 * v_arccos) / v_norm;


 73  logarithm.z = (q3 * v_arccos) / v_norm;


 74  } else {


 75  logarithm.x = 0;


 76  logarithm.y = 0;


 77  logarithm.z = 0;


 78  }


[2]  79  }


 80 


 81  Vector3D Quaternion::GetLogarithm(void) {


[15]  82  Vector3D vector;


 83  GetLogarithm(vector);


 84  return vector;


[2]  85  }


 86 


 87  Quaternion Quaternion::GetDerivative(const Vector3D &angularSpeed) const {


[15]  88  const Quaternion Qw(0, angularSpeed.x, angularSpeed.y, angularSpeed.z);


 89  return 0.5 * (*this) * Qw;


[2]  90  }


 91 


 92  void Quaternion::Derivate(const Vector3D &angularSpeed) {


[15]  93  Quaternion Q = GetDerivative(angularSpeed);


 94  (*this) = Q;


[2]  95  }


 96 


 97  void Quaternion::ToEuler(Euler &euler) const {


[15]  98  euler.roll = atan2(2 * (q0 * q1 + q2 * q3), 1  2 * (q1 * q1 + q2 * q2));


 99  euler.pitch = asin(2 * (q0 * q2  q1 * q3));


 100  euler.yaw = atan2(2 * (q0 * q3 + q1 * q2), 1  2 * (q2 * q2 + q3 * q3));


[2]  101  }


 102 


 103  Euler Quaternion::ToEuler(void) const {


[15]  104  Euler euler;


 105  ToEuler(euler);


 106  return euler;


[2]  107  }


 108 


 109  void Quaternion::ToRotationMatrix(RotationMatrix &matrix) const {


[15]  110  float aSq = q0 * q0;


 111  float bSq = q1 * q1;


 112  float cSq = q2 * q2;


 113  float dSq = q3 * q3;


 114  matrix.m[0][0] = aSq + bSq  cSq  dSq;


 115  matrix.m[0][1] = 2.0f * (q1 * q2  q0 * q3);


 116  matrix.m[0][2] = 2.0f * (q0 * q2 + q1 * q3);


 117  matrix.m[1][0] = 2.0f * (q1 * q2 + q0 * q3);


 118  matrix.m[1][1] = aSq  bSq + cSq  dSq;


 119  matrix.m[1][2] = 2.0f * (q2 * q3  q0 * q1);


 120  matrix.m[2][0] = 2.0f * (q1 * q3  q0 * q2);


 121  matrix.m[2][1] = 2.0f * (q0 * q1 + q2 * q3);


 122  matrix.m[2][2] = aSq  bSq  cSq + dSq;


[2]  123  }


 124 


 125  Quaternion &Quaternion::operator+=(const Quaternion &quaternion) {


[15]  126  q0 += quaternion.q0;


 127  q1 += quaternion.q1;


 128  q2 += quaternion.q2;


 129  q3 += quaternion.q3;


 130  return (*this);


[2]  131  }


 132 


 133  Quaternion &Quaternion::operator=(const Quaternion &quaternion) {


[15]  134  q0 = quaternion.q0;


 135  q1 = quaternion.q1;


 136  q2 = quaternion.q2;


 137  q3 = quaternion.q3;


 138  return (*this);


[2]  139  }


 140 


[15]  141  Quaternion operator+(const Quaternion &quaternionA,


 142  const Quaternion &quaterniontB) {


 143  return Quaternion(


 144  quaternionA.q0 + quaterniontB.q0, quaternionA.q1 + quaterniontB.q1,


 145  quaternionA.q2 + quaterniontB.q2, quaternionA.q3 + quaterniontB.q3);


[2]  146  }


 147 


[15]  148  Quaternion operator(const Quaternion &quaterniontA,


 149  const Quaternion &quaterniontB) {


 150  return Quaternion(


 151  quaterniontA.q0  quaterniontB.q0, quaterniontA.q1  quaterniontB.q1,


 152  quaterniontA.q2  quaterniontB.q2, quaterniontA.q3  quaterniontB.q3);


[2]  153  }


 154 


 155  Quaternion operator(const Quaternion &quaternion) {


[15]  156  return Quaternion(quaternion.q0, quaternion.q1, quaternion.q2,


 157  quaternion.q3);


[2]  158  }


 159 


[15]  160  Quaternion operator*(const Quaternion &quaterniontA,


 161  const Quaternion &quaterniontB) {


 162  return Quaternion(


 163  quaterniontA.q0 * quaterniontB.q0  quaterniontA.q1 * quaterniontB.q1 


 164  quaterniontA.q2 * quaterniontB.q2  quaterniontA.q3 * quaterniontB.q3,


 165  quaterniontA.q0 * quaterniontB.q1 + quaterniontA.q1 * quaterniontB.q0 +


 166  quaterniontA.q2 * quaterniontB.q3  quaterniontA.q3 * quaterniontB.q2,


 167  quaterniontA.q0 * quaterniontB.q2  quaterniontA.q1 * quaterniontB.q3 +


 168  quaterniontA.q2 * quaterniontB.q0 + quaterniontA.q3 * quaterniontB.q1,


 169  quaterniontA.q0 * quaterniontB.q3 + quaterniontA.q1 * quaterniontB.q2 


[161]  170  quaterniontA.q2 * quaterniontB.q1 + quaterniontA.q3 * quaterniontB.q0);


[2]  171  }


 172 


[15]  173  Quaternion operator*(float coeff, const Quaternion &quaternion) {


 174  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,


 175  coeff * quaternion.q2, coeff * quaternion.q3);


[2]  176  }


 177 


[15]  178  Quaternion operator*(const Quaternion &quaternion, float coeff) {


 179  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,


 180  coeff * quaternion.q2, coeff * quaternion.q3);


[2]  181  }


 182 


 183  } // end namespace core


 184  } // end namespace flair

