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

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

flaircore

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