1  // %flair:license{


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


3  // CECILLC License, Version 1.0.


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"


21  #include "math.h"


22 


23  namespace flair {


24  namespace core {


25 


26  RotationMatrix::RotationMatrix() {


27  for (int i = 0; i < 3; i++) {


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


29  if (i == j) {


30  m[i][j] = 1;


31  } else {


32  m[i][j] = 0;


33  }


34  }


35  }


36  }


37 


38  RotationMatrix::~RotationMatrix() {}


39 


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


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


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


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


44  }


45 


46  Euler RotationMatrix::ToEuler(void) const {


47  Euler euler;


48  ToEuler(euler);


49  return euler;


50  }


51 


52  float &RotationMatrix::operator()(size_t row, size_t col) {


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


54  return m[row][col];


55  } else {


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


57  return m[2][2];


58  }


59  }


60 


61  const float &RotationMatrix::operator()(size_t row, size_t col) const {


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


63  return m[row][col];


64  } else {


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


66  return m[2][2];


67  }


68  }


69 


70  } // end namespace core


71  } // end namespace flair

