[2] | 1 | // %flair:license{
|
---|
| 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C 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 | {
|
---|
| 27 | namespace core
|
---|
| 28 | {
|
---|
| 29 |
|
---|
| 30 | Euler::Euler(float inRoll,float inPitch,float inYaw): roll(inRoll), pitch(inPitch),yaw(inYaw) {
|
---|
| 31 | }
|
---|
| 32 |
|
---|
| 33 | Euler::~Euler() {
|
---|
| 34 |
|
---|
| 35 | }
|
---|
| 36 |
|
---|
| 37 | Euler& Euler::operator=(const Euler &euler) {
|
---|
| 38 | roll=euler.roll;
|
---|
| 39 | pitch=euler.pitch;
|
---|
| 40 | yaw=euler.yaw;
|
---|
| 41 | return (*this);
|
---|
| 42 | }
|
---|
| 43 | /*
|
---|
| 44 | void Euler::RotateX(float value) {
|
---|
| 45 | float pitch_tmp;
|
---|
| 46 | pitch_tmp=pitch*cosf(value)+yaw*sinf(value);
|
---|
| 47 | yaw=-pitch*sinf(value)+yaw*cosf(value);
|
---|
| 48 | pitch=pitch_tmp;
|
---|
| 49 | roll+=value;
|
---|
| 50 | if(roll<-PI) roll+=2*PI;
|
---|
| 51 | if(roll>PI) roll-=2*PI;
|
---|
| 52 | }
|
---|
| 53 |
|
---|
| 54 | void Euler::RotateXDeg(float value) {
|
---|
| 55 | RotateX(ToRadian(value));
|
---|
| 56 | }
|
---|
| 57 |
|
---|
| 58 | void Euler::RotateY(float value) {
|
---|
| 59 | float roll_tmp;
|
---|
| 60 | roll_tmp=roll*cosf(value)-yaw*sinf(value);
|
---|
| 61 | yaw=roll*sinf(value)+yaw*cosf(value);
|
---|
| 62 | roll=roll_tmp;
|
---|
| 63 | pitch+=value;
|
---|
| 64 | if(pitch<-PI) pitch+=2*PI;
|
---|
| 65 | if(pitch>PI) pitch-=2*PI;
|
---|
| 66 | }
|
---|
| 67 |
|
---|
| 68 | void Euler::RotateYDeg(float value) {
|
---|
| 69 | RotateY(ToRadian(value));
|
---|
| 70 | }
|
---|
| 71 |
|
---|
| 72 | void Euler::RotateZ(float value) {
|
---|
| 73 | float roll_tmp;
|
---|
| 74 | roll_tmp=roll*cosf(value)+pitch*sinf(value);
|
---|
| 75 | pitch=-roll*sinf(value)+pitch*cosf(value);
|
---|
| 76 | roll=roll_tmp;
|
---|
| 77 | yaw+=value;
|
---|
| 78 | if(yaw<-PI) yaw+=2*PI;
|
---|
| 79 | if(yaw>PI) yaw-=2*PI;
|
---|
| 80 | }
|
---|
| 81 |
|
---|
| 82 | void Euler::RotateZDeg(float value) {
|
---|
| 83 | RotateZ(ToRadian(value));
|
---|
| 84 | }
|
---|
| 85 | */
|
---|
| 86 | void Euler::ToQuaternion(Quaternion &quaternion) const {
|
---|
| 87 | quaternion.q0=cos(yaw/2)*cos(pitch/2)*cos(roll/2)
|
---|
| 88 | +sin(yaw/2)*sin(pitch/2)*sin(roll/2);
|
---|
| 89 |
|
---|
| 90 | quaternion.q1=cos(yaw/2)*cos(pitch/2)*sin(roll/2)
|
---|
| 91 | -sin(yaw/2)*sin(pitch/2)*cos(roll/2);
|
---|
| 92 |
|
---|
| 93 | quaternion.q2=cos(yaw/2)*sin(pitch/2)*cos(roll/2)
|
---|
| 94 | +sin(yaw/2)*cos(pitch/2)*sin(roll/2);
|
---|
| 95 |
|
---|
| 96 | quaternion.q3=sin(yaw/2)*cos(pitch/2)*cos(roll/2)
|
---|
| 97 | -cos(yaw/2)*sin(pitch/2)*sin(roll/2);
|
---|
| 98 | }
|
---|
| 99 |
|
---|
| 100 | Quaternion Euler::ToQuaternion(void) const {
|
---|
| 101 | Quaternion quaternion;
|
---|
| 102 | ToQuaternion(quaternion);
|
---|
| 103 | return quaternion;
|
---|
| 104 | }
|
---|
| 105 |
|
---|
| 106 | float Euler::ToDegree(float radianValue) {
|
---|
| 107 | return radianValue*180.0f/PI;
|
---|
| 108 | }
|
---|
| 109 |
|
---|
| 110 | float Euler::ToRadian(float degreeValue) {
|
---|
| 111 | return degreeValue/180.0f*PI;
|
---|
| 112 | }
|
---|
| 113 |
|
---|
| 114 | float Euler::YawDistanceFrom(float angle) const {
|
---|
| 115 | float rot1,rot2;
|
---|
| 116 | if(angle > yaw ) {
|
---|
| 117 | rot1=angle-yaw;
|
---|
| 118 | rot2=2*PI-angle+yaw;
|
---|
| 119 | } else {
|
---|
| 120 | rot1=2*PI+angle-yaw;
|
---|
| 121 | rot2=yaw-angle;
|
---|
| 122 | }
|
---|
| 123 | if(rot2<rot1) rot1=-rot2;
|
---|
| 124 | rot1=-rot1;//pour avoir rot1=yaw-angle
|
---|
| 125 |
|
---|
| 126 | return rot1;
|
---|
| 127 | }
|
---|
| 128 |
|
---|
| 129 | } // end namespace core
|
---|
| 130 | } // end namespace flair
|
---|