[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: 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 


[15]  25  namespace flair {


 26  namespace core {


[2]  27 


[15]  28  Euler::Euler(float inRoll, float inPitch, float inYaw)


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


[2]  30 


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


[2]  32 


[15]  33  Euler &Euler::operator=(const Euler &euler) {


 34  roll = euler.roll;


 35  pitch = euler.pitch;


 36  yaw = euler.yaw;


 37  return (*this);


[2]  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;


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


[2]  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;


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


[2]  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;


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


[2]  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 {


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


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


[2]  85 


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


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


[2]  88 


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


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


[2]  91 


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


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


[2]  94  }


 95 


 96  Quaternion Euler::ToQuaternion(void) const {


[15]  97  Quaternion quaternion;


 98  ToQuaternion(quaternion);


 99  return quaternion;


[2]  100  }


 101 


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


[2]  103 


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


[2]  105 


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


[15]  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  if (rot2 < rot1)


 116  rot1 = rot2;


 117  rot1 = rot1; // pour avoir rot1=yawangle


[2]  118 


[15]  119  return rot1;


[2]  120  }


 121 


 122  } // end namespace core


 123  } // end namespace flair

