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

Last change on this file since 16 was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

File size: 3.0 KB
RevLine 
[2]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C 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]25namespace flair {
26namespace core {
[2]27
[15]28Euler::Euler(float inRoll, float inPitch, float inYaw)
29 : roll(inRoll), pitch(inPitch), yaw(inYaw) {}
[2]30
[15]31Euler::~Euler() {}
[2]32
[15]33Euler &Euler::operator=(const Euler &euler) {
34 roll = euler.roll;
35 pitch = euler.pitch;
36 yaw = euler.yaw;
37 return (*this);
[2]38}
39/*
40void 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
50void Euler::RotateXDeg(float value) {
51 RotateX(ToRadian(value));
52}
53
54void 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
64void Euler::RotateYDeg(float value) {
65 RotateY(ToRadian(value));
66}
67
68void 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
78void Euler::RotateZDeg(float value) {
79 RotateZ(ToRadian(value));
80}
81*/
82void 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
96Quaternion Euler::ToQuaternion(void) const {
[15]97 Quaternion quaternion;
98 ToQuaternion(quaternion);
99 return quaternion;
[2]100}
101
[15]102float Euler::ToDegree(float radianValue) { return radianValue * 180.0f / PI; }
[2]103
[15]104float Euler::ToRadian(float degreeValue) { return degreeValue / 180.0f * PI; }
[2]105
106float 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=yaw-angle
[2]118
[15]119 return rot1;
[2]120}
121
122} // end namespace core
123} // end namespace flair
Note: See TracBrowser for help on using the repository browser.