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: 2013/05/02


6  // filename: Euler.cpp


7  //


8  // author: Guillaume Sanahuja


9  // Copyright Heudiasyc UMR UTC/CNRS 7253


10  //


11  // version: $Id: $


12  //


13  // purpose: Class defining euler angles


14  //


15  //


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


17 


18  #include "Euler.h"


19  #include "Quaternion.h"


20  #include <math.h>


21 


22  #define PI ((float)3.14159265358979323846)


23  #define PI_D ((double)3.14159265358979323846)


24 


25  namespace flair {


26  namespace core {


27 


28  Euler::Euler(float inRoll, float inPitch, float inYaw)


29  : roll(inRoll), pitch(inPitch), yaw(inYaw) {}


30 


31  Euler::~Euler() {}


32 


33  Euler &Euler::operator=(const Euler &euler) {


34  roll = euler.roll;


35  pitch = euler.pitch;


36  yaw = euler.yaw;


37  return (*this);


38  }


39  /*


40  void Euler::RotateX(float value) {


41  float pitch_tmp;


42  pitch_tmp=pitch*cosf(value)+yaw*sinf(value);


43  yaw=pitch*sinf(value)+yaw*cosf(value);


44  pitch=pitch_tmp;


45  roll+=value;


46  if(roll<PI) roll+=2*PI;


47  if(roll>PI) roll=2*PI;


48  }


49 


50  void Euler::RotateXDeg(float value) {


51  RotateX(ToRadian(value));


52  }


53 


54  void Euler::RotateY(float value) {


55  float roll_tmp;


56  roll_tmp=roll*cosf(value)yaw*sinf(value);


57  yaw=roll*sinf(value)+yaw*cosf(value);


58  roll=roll_tmp;


59  pitch+=value;


60  if(pitch<PI) pitch+=2*PI;


61  if(pitch>PI) pitch=2*PI;


62  }


63 


64  void Euler::RotateYDeg(float value) {


65  RotateY(ToRadian(value));


66  }


67 


68  void Euler::RotateZ(float value) {


69  float roll_tmp;


70  roll_tmp=roll*cosf(value)+pitch*sinf(value);


71  pitch=roll*sinf(value)+pitch*cosf(value);


72  roll=roll_tmp;


73  yaw+=value;


74  if(yaw<PI) yaw+=2*PI;


75  if(yaw>PI) yaw=2*PI;


76  }


77 


78  void Euler::RotateZDeg(float value) {


79  RotateZ(ToRadian(value));


80  }


81  */


82  void Euler::ToQuaternion(Quaternion &quaternion) const {


83  quaternion.q0 = cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2) +


84  sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2);


85 


86  quaternion.q1 = cos(yaw / 2) * cos(pitch / 2) * sin(roll / 2) 


87  sin(yaw / 2) * sin(pitch / 2) * cos(roll / 2);


88 


89  quaternion.q2 = cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2) +


90  sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2);


91 


92  quaternion.q3 = sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2) 


93  cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2);


94  }


95 


96  Quaternion Euler::ToQuaternion(void) const {


97  Quaternion quaternion;


98  ToQuaternion(quaternion);


99  return quaternion;


100  }


101 


102  float Euler::ToDegree(float radianValue) { return radianValue * 180.0f / PI; }


103 


104  float Euler::ToRadian(float degreeValue) { return degreeValue / 180.0f * PI; }


105 


106  float Euler::YawDistanceFrom(float angle) const {


107  float rot1, rot2;


108  if (angle > yaw) {


109  rot1 = angle  yaw;


110  rot2 = 2 * PI  angle + yaw;


111  } else {


112  rot1 = 2 * PI + angle  yaw;


113  rot2 = yaw  angle;


114  }


115 


116  if (rot2 < rot1) rot1 = rot2;


117 


118  rot1 = rot1; // pour avoir rot1=yawangle


119 


120  return rot1;


121  }


122 


123  } // end namespace core


124  } // end namespace flair

