[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/09


 6  // filename: RotationMatrix.cpp


 7  //


 8  // author: Guillaume Sanahuja


 9  // Copyright Heudiasyc UMR UTC/CNRS 7253


 10  //


 11  // version: $Id: $


 12  //


 13  // purpose: Class defining a rotation matrix


 14  //


 15  //


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


 17 


 18  #include "RotationMatrix.h"


 19  #include "Object.h"


 20  #include "Euler.h"


[100]  21  #include "Quaternion.h"


[2]  22  #include "math.h"


 23 


[103]  24  using std::max;


 25 


[15]  26  namespace flair {


 27  namespace core {


[2]  28 


 29  RotationMatrix::RotationMatrix() {


[15]  30  for (int i = 0; i < 3; i++) {


 31  for (int j = 0; j < 3; j++) {


 32  if (i == j) {


 33  m[i][j] = 1;


 34  } else {


 35  m[i][j] = 0;


 36  }


[2]  37  }


[15]  38  }


[2]  39  }


 40 


[15]  41  RotationMatrix::~RotationMatrix() {}


[2]  42 


 43  void RotationMatrix::ToEuler(Euler &euler) const {


[15]  44  euler.roll = atanf(m[1][2] / m[2][2]);


 45  euler.pitch = asinf(m[0][2]);


 46  euler.yaw = atan2f(m[0][1], m[0][0]);


[2]  47  }


 48 


 49  Euler RotationMatrix::ToEuler(void) const {


[15]  50  Euler euler;


 51  ToEuler(euler);


 52  return euler;


[2]  53  }


 54 


[103]  55  //from 3dmgx325 manual


[100]  56  void RotationMatrix::ToQuaternion(Quaternion &quaternion) const {


[103]  57  float test1 = m[0][0] + m[1][1] + m[2][2];


 58  float test2 = m[0][0]  m[1][1]  m[2][2];


 59  float test3 = m[0][0] + m[1][1]  m[2][2];


 60  float test4 = m[0][0]  m[1][1] + m[2][2];


 61 


 62  if(test1==max(max(test1,test2),max(test3,test4))) {


 63  float s=2*sqrtf(1+test1);


 64  quaternion.q0 = s/4.;


 65  quaternion.q1 = ( m[1][2]  m[2][1] ) / s;


 66  quaternion.q2 = ( m[2][0]  m[0][2] ) / s;


 67  quaternion.q3 = ( m[0][1]  m[1][0] ) / s;


[101]  68  }


[103]  69 


 70  if(test2==max(max(test1,test2),max(test3,test4))) {


 71  float s=2*sqrtf(1+test2);


 72  quaternion.q0 = ( m[2][1]  m[1][2] ) / s;


 73  quaternion.q1 = s/4.;


 74  quaternion.q2 = ( m[1][0] + m[0][1] ) / s;


 75  quaternion.q3 = ( m[0][2] + m[2][0] ) / s;


 76  }


 77 


 78  if(test3==max(max(test1,test2),max(test3,test4))) {


 79  float s=2*sqrtf(1+test3);


 80  quaternion.q0 = (m[0][2]  m[2][0] ) / s;


 81  quaternion.q1 = ( m[1][0] + m[0][1] ) / s;


 82  quaternion.q2 = s/4.;


 83  quaternion.q3 = ( m[2][1] + m[1][2] ) / s;


 84  }


 85 


 86  if(test4==max(max(test1,test2),max(test3,test4))) {


 87  float s=2*sqrtf(1+test4);


 88  quaternion.q0 = (m[1][0]  m[0][1] ) / s;


 89  quaternion.q1 = ( m[0][2] + m[2][0] ) / s;


 90  quaternion.q2 = ( m[2][1] + m[1][2] ) / s;


 91  quaternion.q3 = s/4.;


 92  }


[101]  93  quaternion.Normalize();


 94  }


[103]  95 


[100]  96  Quaternion RotationMatrix::ToQuaternion(void) const {


 97  Quaternion quaternion;


 98  ToQuaternion(quaternion);


 99  return quaternion;


 100  }


 101 


[15]  102  float &RotationMatrix::operator()(size_t row, size_t col) {


 103  if (row < 3 && col < 3) {


 104  return m[row][col];


 105  } else {


 106  Printf("RotationMatrix: index (%i,%i) out of bound\n", row, col);


 107  return m[2][2];


 108  }


[2]  109  }


 110 


[15]  111  const float &RotationMatrix::operator()(size_t row, size_t col) const {


 112  if (row < 3 && col < 3) {


 113  return m[row][col];


 114  } else {


 115  Printf("RotationMatrix: index (%i,%i) out of bound\n", row, col);


 116  return m[2][2];


 117  }


[2]  118  }


 119 


 120  } // end namespace core


 121  } // end namespace flair

