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

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

sources reformatted with flair-format-dir script

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