source: flair-src/trunk/lib/FlairCore/src/Quaternion.cpp

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

use float

File size: 5.4 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:    2016/02/03
6//  filename:   Quaternion.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Class defining a quaternion
14//
15//
16/*********************************************************************/
17
18#include "Quaternion.h"
19#include "Euler.h"
20#include "RotationMatrix.h"
21#include <math.h>
22#include <stdio.h>
23
24namespace flair {
25namespace core {
26
27Quaternion::Quaternion(float inQ0, float inQ1, float inQ2, float inQ3)
28    : q0(inQ0), q1(inQ1), q2(inQ2), q3(inQ3) {}
29
30Quaternion::~Quaternion() {}
31
32Quaternion &Quaternion::operator=(const Quaternion &quaternion) {
33  q0 = quaternion.q0;
34  q1 = quaternion.q1;
35  q2 = quaternion.q2;
36  q3 = quaternion.q3;
37  return (*this);
38}
39
40float Quaternion::GetNorm(void) const {
41  return sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
42}
43
44void Quaternion::Normalize(void) {
45  float n = GetNorm();
46  if (n != 0) {
47    q0 = q0 / n;
48    q1 = q1 / n;
49    q2 = q2 / n;
50    q3 = q3 / n;
51  }
52}
53
54void Quaternion::Conjugate(void) {
55  q1 = -q1;
56  q2 = -q2;
57  q3 = -q3;
58}
59
60Quaternion Quaternion::GetConjugate(void) {
61  return Quaternion(q0, -q1, -q2, -q3);
62}
63
64void Quaternion::GetLogarithm(Vector3Df &logarithm) {
65  Normalize();
66  float v_norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3);
67
68  if (v_norm != 0) {
69    float v_arccos = acosf(q0);
70    logarithm.x = (q1 * v_arccos) / v_norm;
71    logarithm.y = (q2 * v_arccos) / v_norm;
72    logarithm.z = (q3 * v_arccos) / v_norm;
73  } else {
74    logarithm.x = 0;
75    logarithm.y = 0;
76    logarithm.z = 0;
77  }
78}
79
80Vector3Df Quaternion::GetLogarithm(void) {
81  Vector3Df vector;
82  GetLogarithm(vector);
83  return vector;
84}
85
86Quaternion Quaternion::GetDerivative(const Vector3Df &angularSpeed) const {
87  const Quaternion Qw(0, angularSpeed.x, angularSpeed.y, angularSpeed.z);
88  return 0.5 * (*this) * Qw;
89}
90
91void Quaternion::Derivate(const Vector3Df &angularSpeed) {
92  Quaternion Q = GetDerivative(angularSpeed);
93  (*this) = Q;
94}
95
96float Quaternion::GetEulerRoll(void) const {
97    return atan2f(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
98}
99 
100
101float Quaternion::GetEulerPitch(void) const {
102    return asinf(2 * (q0 * q2 - q1 * q3));
103}
104 
105
106float Quaternion::GetEulerYaw(void) const {
107    return atan2f(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
108}
109 
110void Quaternion::ToEuler(Euler &euler) const {
111  euler.roll = GetEulerRoll();
112  euler.pitch = GetEulerPitch();
113  euler.yaw = GetEulerYaw();
114}
115
116Euler Quaternion::ToEuler(void) const {
117  Euler euler;
118  ToEuler(euler);
119  return euler;
120}
121
122void Quaternion::ToRotationMatrix(RotationMatrix &matrix) const {
123  float aSq = q0 * q0;
124  float bSq = q1 * q1;
125  float cSq = q2 * q2;
126  float dSq = q3 * q3;
127  matrix.m[0][0] = aSq + bSq - cSq - dSq;
128  matrix.m[0][1] = 2.0f * (q1 * q2 - q0 * q3);
129  matrix.m[0][2] = 2.0f * (q0 * q2 + q1 * q3);
130  matrix.m[1][0] = 2.0f * (q1 * q2 + q0 * q3);
131  matrix.m[1][1] = aSq - bSq + cSq - dSq;
132  matrix.m[1][2] = 2.0f * (q2 * q3 - q0 * q1);
133  matrix.m[2][0] = 2.0f * (q1 * q3 - q0 * q2);
134  matrix.m[2][1] = 2.0f * (q0 * q1 + q2 * q3);
135  matrix.m[2][2] = aSq - bSq - cSq + dSq;
136}
137
138Quaternion &Quaternion::operator+=(const Quaternion &quaternion) {
139  q0 += quaternion.q0;
140  q1 += quaternion.q1;
141  q2 += quaternion.q2;
142  q3 += quaternion.q3;
143  return (*this);
144}
145
146Quaternion &Quaternion::operator-=(const Quaternion &quaternion) {
147  q0 -= quaternion.q0;
148  q1 -= quaternion.q1;
149  q2 -= quaternion.q2;
150  q3 -= quaternion.q3;
151  return (*this);
152}
153
154Quaternion operator+(const Quaternion &quaternionA,
155                     const Quaternion &quaterniontB) {
156  return Quaternion(
157      quaternionA.q0 + quaterniontB.q0, quaternionA.q1 + quaterniontB.q1,
158      quaternionA.q2 + quaterniontB.q2, quaternionA.q3 + quaterniontB.q3);
159}
160
161Quaternion operator-(const Quaternion &quaterniontA,
162                     const Quaternion &quaterniontB) {
163  return Quaternion(
164      quaterniontA.q0 - quaterniontB.q0, quaterniontA.q1 - quaterniontB.q1,
165      quaterniontA.q2 - quaterniontB.q2, quaterniontA.q3 - quaterniontB.q3);
166}
167
168Quaternion operator-(const Quaternion &quaternion) {
169  return Quaternion(-quaternion.q0, -quaternion.q1, -quaternion.q2,
170                    -quaternion.q3);
171}
172
173Quaternion operator*(const Quaternion &quaterniontA,
174                     const Quaternion &quaterniontB) {
175  return Quaternion(
176      quaterniontA.q0 * quaterniontB.q0 - quaterniontA.q1 * quaterniontB.q1 -
177          quaterniontA.q2 * quaterniontB.q2 - quaterniontA.q3 * quaterniontB.q3,
178      quaterniontA.q0 * quaterniontB.q1 + quaterniontA.q1 * quaterniontB.q0 +
179          quaterniontA.q2 * quaterniontB.q3 - quaterniontA.q3 * quaterniontB.q2,
180      quaterniontA.q0 * quaterniontB.q2 - quaterniontA.q1 * quaterniontB.q3 +
181          quaterniontA.q2 * quaterniontB.q0 + quaterniontA.q3 * quaterniontB.q1,
182      quaterniontA.q0 * quaterniontB.q3 + quaterniontA.q1 * quaterniontB.q2 -
183          quaterniontA.q2 * quaterniontB.q1 + quaterniontA.q3 * quaterniontB.q0);
184}
185
186Quaternion operator*(float coeff, const Quaternion &quaternion) {
187  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,
188                    coeff * quaternion.q2, coeff * quaternion.q3);
189}
190
191Quaternion operator*(const Quaternion &quaternion, float coeff) {
192  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,
193                    coeff * quaternion.q2, coeff * quaternion.q3);
194}
195
196} // end namespace core
197} // end namespace flair
Note: See TracBrowser for help on using the repository browser.