source: flair-src/trunk/lib/FlairCore/src/Euler.cpp @ 2

Last change on this file since 2 was 2, checked in by Sanahuja Guillaume, 5 years ago

flaircore

File size: 2.9 KB
Line 
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
25namespace flair
26{
27namespace core
28{
29
30Euler::Euler(float inRoll,float inPitch,float inYaw): roll(inRoll), pitch(inPitch),yaw(inYaw) {
31}
32
33Euler::~Euler() {
34
35}
36
37Euler& Euler::operator=(const Euler &euler) {
38    roll=euler.roll;
39    pitch=euler.pitch;
40    yaw=euler.yaw;
41    return (*this);
42}
43/*
44void 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
54void Euler::RotateXDeg(float value) {
55    RotateX(ToRadian(value));
56}
57
58void 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
68void Euler::RotateYDeg(float value) {
69    RotateY(ToRadian(value));
70}
71
72void 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
82void Euler::RotateZDeg(float value) {
83    RotateZ(ToRadian(value));
84}
85*/
86void 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
100Quaternion Euler::ToQuaternion(void) const {
101    Quaternion quaternion;
102    ToQuaternion(quaternion);
103    return quaternion;
104}
105
106float Euler::ToDegree(float radianValue) {
107    return radianValue*180.0f/PI;
108}
109
110float Euler::ToRadian(float degreeValue) {
111    return degreeValue/180.0f*PI;
112}
113
114float 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
Note: See TracBrowser for help on using the repository browser.