Class
List
Hierarchy
Modules
Common
Events
Math
Messages
Physics
Rendering
Sensors
Transport
Links
Gazebo Website
Wiki
Tutorials
Download
All
Classes
Namespaces
Files
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
Macros
Groups
Pages
gazebo
math
Quaternion.hh
Go to the documentation of this file.
1
/*
2
* Copyright 2012 Nate Koenig
3
*
4
* Licensed under the Apache License, Version 2.0 (the "License");
5
* you may not use this file except in compliance with the License.
6
* You may obtain a copy of the License at
7
*
8
* http://www.apache.org/licenses/LICENSE-2.0
9
*
10
* Unless required by applicable law or agreed to in writing, software
11
* distributed under the License is distributed on an "AS IS" BASIS,
12
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13
* See the License for the specific language governing permissions and
14
* limitations under the License.
15
*
16
*/
17
/* Desc: External interfaces for Gazebo
18
* Author: Nate Koenig
19
* Date: 03 Apr 2007
20
*/
21
22
#ifndef _QUATERNION_HH_
23
#define _QUATERNION_HH_
24
25
#include <math.h>
26
#include <iostream>
27
#include <cmath>
28
29
#include "
math/Helpers.hh
"
30
#include "
math/Angle.hh
"
31
#include "
math/Vector3.hh
"
32
#include "
math/Matrix3.hh
"
33
#include "
math/Matrix4.hh
"
34
35
namespace
gazebo
36
{
37
namespace
math
38
{
41
44
class
Quaternion
45
{
47
public
:
Quaternion
();
48
54
public
:
Quaternion
(
const
double
&_w,
const
double
&_x,
const
double
&_y,
55
const
double
&_z);
56
61
public
:
Quaternion
(
const
double
&_roll,
const
double
&_pitch,
62
const
double
&_yaw);
63
67
public
:
Quaternion
(
const
Vector3
&_axis,
const
double
&_angle);
68
71
public
:
Quaternion
(
const
Vector3
&_rpy);
72
75
public
:
Quaternion
(
const
Quaternion
&_qt);
76
78
public
:
~Quaternion
();
79
82
public
:
Quaternion
&
operator =
(
const
Quaternion
&_qt);
83
85
public
:
void
Invert
();
86
89
public
:
inline
Quaternion
GetInverse
()
const
90
{
91
double
s = 0;
92
Quaternion
q(this->
w
, this->
x
, this->
y
, this->
z
);
93
94
// use s to test if quaternion is valid
95
s = q.
w
* q.
w
+ q.
x
* q.
x
+ q.
y
* q.
y
+ q.
z
* q.
z
;
96
97
if
(
math::equal
(s, 0.0))
98
{
99
q.
w
= 1.0;
100
q.
x
= 0.0;
101
q.
y
= 0.0;
102
q.
z
= 0.0;
103
}
104
else
105
{
106
// deal with non-normalized quaternion
107
// div by s so q * qinv = identity
108
q.
w
= q.
w
/ s;
109
q.
x
= -q.
x
/ s;
110
q.
y
= -q.
y
/ s;
111
q.
z
= -q.
z
/ s;
112
}
113
return
q;
114
}
115
117
public
:
void
SetToIdentity
();
118
121
public
:
Quaternion
GetLog
()
const
;
122
125
public
:
Quaternion
GetExp
()
const
;
126
128
public
:
void
Normalize
();
129
135
public
:
void
SetFromAxis
(
double
_x,
double
_y,
double
_z,
double
_a);
136
140
public
:
void
SetFromAxis
(
const
Vector3
&_axis,
double
_a);
141
147
public
:
void
Set
(
double
_u,
double
_x,
double
_y,
double
_z);
148
151
public
:
void
SetFromEuler
(
const
Vector3
&_vec);
152
155
public
:
Vector3
GetAsEuler
()
const
;
156
159
public
:
static
Quaternion
EulerToQuaternion
(
const
Vector3
&_vec);
160
165
public
:
static
Quaternion
EulerToQuaternion
(
double
_x,
166
double
_y,
167
double
_z);
168
171
public
:
double
GetRoll
();
172
175
public
:
double
GetPitch
();
176
179
public
:
double
GetYaw
();
180
184
public
:
void
GetAsAxis
(
Vector3
&_axis,
double
&_angle)
const
;
185
188
public
:
void
Scale
(
double
_scale);
189
193
public
:
Quaternion
operator+
(
const
Quaternion
&_qt)
const
;
194
198
public
:
Quaternion
operator+=
(
const
Quaternion
&_qt);
199
203
public
:
Quaternion
operator-
(
const
Quaternion
&_qt)
const
;
204
208
public
:
Quaternion
operator-=
(
const
Quaternion
&_qt);
209
213
public
:
inline
Quaternion
operator*
(
const
Quaternion
&_q)
const
214
{
215
return
Quaternion
(
216
this->
w
*_q.
w
- this->x*_q.
x
- this->y*_q.
y
- this->z*_q.
z
,
217
this->w*_q.
x
+ this->x*_q.
w
+ this->y*_q.
z
- this->z*_q.
y
,
218
this->w*_q.
y
- this->x*_q.
z
+ this->y*_q.
w
+ this->z*_q.
x
,
219
this->w*_q.
z
+ this->x*_q.
y
- this->y*_q.
x
+ this->z*_q.
w
);
220
}
221
225
public
:
Quaternion
operator*
(
const
double
&_f)
const
;
226
230
public
:
Quaternion
operator*=
(
const
Quaternion
&qt);
231
234
public
:
Vector3
operator*
(
const
Vector3
&_v)
const
;
235
239
public
:
bool
operator ==
(
const
Quaternion
&_qt)
const
;
240
244
public
:
bool
operator!=
(
const
Quaternion
&_qt)
const
;
245
248
public
:
Quaternion
operator-
()
const
;
249
253
public
:
inline
Vector3
RotateVector
(
const
Vector3
&_vec)
const
254
{
255
Quaternion
tmp(0.0, _vec.
x
, _vec.
y
, _vec.
z
);
256
tmp = (*this) * (tmp * this->
GetInverse
());
257
return
Vector3
(tmp.
x
, tmp.
y
, tmp.
z
);
258
}
259
263
public
:
Vector3
RotateVectorReverse
(
Vector3
_vec)
const
;
264
267
public
:
bool
IsFinite
()
const
;
268
270
public
:
inline
void
Correct
()
271
{
272
if
(!finite(this->
x
))
273
this->
x
= 0;
274
if
(!finite(this->
y
))
275
this->
y
= 0;
276
if
(!finite(this->
z
))
277
this->
z
= 0;
278
if
(!finite(this->
w
))
279
this->
w
= 1;
280
281
if
(
math::equal
(this->
w
, 0.0) &&
282
math::equal
(this->
x
, 0.0) &&
283
math::equal
(this->
y
, 0.0) &&
284
math::equal
(this->
z
, 0.0))
285
{
286
this->
w
= 1;
287
}
288
}
289
291
public
:
Matrix3
GetAsMatrix3
()
const
;
292
295
public
:
Matrix4
GetAsMatrix4
()
const
;
296
299
public
:
Vector3
GetXAxis
()
const
;
300
303
public
:
Vector3
GetYAxis
()
const
;
304
307
public
:
Vector3
GetZAxis
()
const
;
308
311
public
:
void
Round
(
int
_precision);
312
316
public
:
double
Dot
(
const
Quaternion
&_q)
const
;
317
327
public
:
static
Quaternion
Squad
(
double
_fT,
const
Quaternion
&_rkP,
328
const
Quaternion
&_rkA,
const
Quaternion
&_rkB,
329
const
Quaternion
&_rkQ,
bool
_shortestPath =
false
);
330
338
public
:
static
Quaternion
Slerp
(
double
_fT,
const
Quaternion
&_rkP,
339
const
Quaternion
&_rkQ,
bool
_shortestPath =
false
);
340
341
343
public
:
double
w
;
344
346
public
:
double
x
;
347
349
public
:
double
y
;
350
352
public
:
double
z
;
353
358
public
:
friend
std::ostream &
operator<<
(std::ostream &_out,
359
const
gazebo::math::Quaternion
&_q)
360
{
361
Vector3
v(_q.
GetAsEuler
());
362
_out << v.
x
<<
" "
<< v.y <<
" "
<< v.z;
363
return
_out;
364
}
365
370
public
:
friend
std::istream &
operator>>
(std::istream &_in,
371
gazebo::math::Quaternion
&_q)
372
{
373
Angle
r, p,
y
;
374
375
// Skip white spaces
376
_in.setf(std::ios_base::skipws);
377
_in >> r >> p >>
y
;
378
379
_q.
SetFromEuler
(
Vector3
(*r, *p, *y));
380
381
return
_in;
382
}
383
};
385
}
386
}
387
#endif
388