Quaternion.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 Open Source Robotics Foundation
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 
18 #ifndef _GAZEBO_MATH_QUATERNION_HH_
19 #define _GAZEBO_MATH_QUATERNION_HH_
20 
21 #include <math.h>
22 #include <iostream>
23 #include <cmath>
24 #include <ignition/math/Quaternion.hh>
25 
26 #include "gazebo/math/Helpers.hh"
27 #include "gazebo/math/Angle.hh"
28 #include "gazebo/math/Vector3.hh"
29 #include "gazebo/math/Matrix3.hh"
30 #include "gazebo/math/Matrix4.hh"
31 #include "gazebo/util/system.hh"
32 
33 namespace gazebo
34 {
35  namespace math
36  {
39 
42  class GZ_MATH_VISIBLE Quaternion
43  {
45  public: Quaternion();
46 
52  public: Quaternion(const double &_w, const double &_x, const double &_y,
53  const double &_z);
54 
59  public: Quaternion(const double &_roll, const double &_pitch,
60  const double &_yaw);
61 
65  public: Quaternion(const Vector3 &_axis, const double &_angle);
66 
69  public: Quaternion(const Vector3 &_rpy);
70 
73  public: Quaternion(const Quaternion &_qt);
74 
77  public: Quaternion(const ignition::math::Quaterniond &_qt);
78 
80  public: ~Quaternion();
81 
84  public: Quaternion &operator =(const Quaternion &_qt);
85 
88  public: ignition::math::Quaterniond Ign() const;
89 
93  public: Quaternion &operator =(const ignition::math::Quaterniond &_v);
94 
96  public: void Invert();
97 
100  public: inline Quaternion GetInverse() const
101  {
102  double s = 0;
103  Quaternion q(this->w, this->x, this->y, this->z);
104 
105  // use s to test if quaternion is valid
106  s = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
107 
108  if (math::equal(s, 0.0))
109  {
110  q.w = 1.0;
111  q.x = 0.0;
112  q.y = 0.0;
113  q.z = 0.0;
114  }
115  else
116  {
117  // deal with non-normalized quaternion
118  // div by s so q * qinv = identity
119  q.w = q.w / s;
120  q.x = -q.x / s;
121  q.y = -q.y / s;
122  q.z = -q.z / s;
123  }
124  return q;
125  }
126 
128  public: void SetToIdentity();
129 
132  public: Quaternion GetLog() const;
133 
136  public: Quaternion GetExp() const;
137 
139  public: void Normalize();
140 
146  public: void SetFromAxis(double _x, double _y, double _z, double _a);
147 
151  public: void SetFromAxis(const Vector3 &_axis, double _a);
152 
158  public: void Set(double _u, double _x, double _y, double _z);
159 
165  public: void SetFromEuler(const Vector3 &_vec);
166 
171  public: void SetFromEuler(double _roll, double _pitch, double _yaw);
172 
175  public: Vector3 GetAsEuler() const;
176 
180  public: static Quaternion EulerToQuaternion(const Vector3 &_vec);
181 
187  public: static Quaternion EulerToQuaternion(double _x,
188  double _y,
189  double _z);
190 
193  public: double GetRoll();
194 
197  public: double GetPitch();
198 
201  public: double GetYaw();
202 
206  public: void GetAsAxis(Vector3 &_axis, double &_angle) const;
207 
210  public: void Scale(double _scale);
211 
215  public: Quaternion operator+(const Quaternion &_qt) const;
216 
220  public: Quaternion operator+=(const Quaternion &_qt);
221 
225  public: Quaternion operator-(const Quaternion &_qt) const;
226 
230  public: Quaternion operator-=(const Quaternion &_qt);
231 
235  public: inline Quaternion operator*(const Quaternion &_q) const
236  {
237  return Quaternion(
238  this->w*_q.w - this->x*_q.x - this->y*_q.y - this->z*_q.z,
239  this->w*_q.x + this->x*_q.w + this->y*_q.z - this->z*_q.y,
240  this->w*_q.y - this->x*_q.z + this->y*_q.w + this->z*_q.x,
241  this->w*_q.z + this->x*_q.y - this->y*_q.x + this->z*_q.w);
242  }
243 
247  public: Quaternion operator*(const double &_f) const;
248 
252  public: Quaternion operator*=(const Quaternion &qt);
253 
257  public: Vector3 operator*(const Vector3 &_v) const;
258 
262  public: bool operator ==(const Quaternion &_qt) const;
263 
267  public: bool operator!=(const Quaternion &_qt) const;
268 
271  public: Quaternion operator-() const;
272 
276  public: inline Vector3 RotateVector(const Vector3 &_vec) const
277  {
278  Quaternion tmp(0.0, _vec.x, _vec.y, _vec.z);
279  tmp = (*this) * (tmp * this->GetInverse());
280  return Vector3(tmp.x, tmp.y, tmp.z);
281  }
282 
286  public: Vector3 RotateVectorReverse(Vector3 _vec) const;
287 
290  public: bool IsFinite() const;
291 
293  public: inline void Correct()
294  {
295  if (!std::isfinite(this->x))
296  this->x = 0;
297  if (!std::isfinite(this->y))
298  this->y = 0;
299  if (!std::isfinite(this->z))
300  this->z = 0;
301  if (!std::isfinite(this->w))
302  this->w = 1;
303 
304  if (math::equal(this->w, 0.0) &&
305  math::equal(this->x, 0.0) &&
306  math::equal(this->y, 0.0) &&
307  math::equal(this->z, 0.0))
308  {
309  this->w = 1;
310  }
311  }
312 
315  public: Matrix3 GetAsMatrix3() const;
316 
319  public: Matrix4 GetAsMatrix4() const;
320 
323  public: Vector3 GetXAxis() const;
324 
327  public: Vector3 GetYAxis() const;
328 
331  public: Vector3 GetZAxis() const;
332 
335  public: void Round(int _precision);
336 
340  public: double Dot(const Quaternion &_q) const;
341 
352  public: static Quaternion Squad(double _fT, const Quaternion &_rkP,
353  const Quaternion &_rkA, const Quaternion &_rkB,
354  const Quaternion &_rkQ, bool _shortestPath = false);
355 
364  public: static Quaternion Slerp(double _fT, const Quaternion &_rkP,
365  const Quaternion &_rkQ, bool _shortestPath = false);
366 
373  public: Quaternion Integrate(const Vector3 &_angularVelocity,
374  const double _deltaT) const;
375 
377  public: double w;
378 
380  public: double x;
381 
383  public: double y;
384 
386  public: double z;
387 
392  public: friend std::ostream &operator<<(std::ostream &_out,
393  const gazebo::math::Quaternion &_q)
394  {
395  Vector3 v(_q.GetAsEuler());
396  _out << precision(v.x, 6) << " " << precision(v.y, 6) << " "
397  << precision(v.z, 6);
398  return _out;
399  }
400 
405  public: friend std::istream &operator>>(std::istream &_in,
407  {
408  Angle roll, pitch, yaw;
409 
410  // Skip white spaces
411  _in.setf(std::ios_base::skipws);
412  _in >> roll >> pitch >> yaw;
413 
414  _q.SetFromEuler(Vector3(*roll, *pitch, *yaw));
415 
416  return _in;
417  }
418  };
420  }
421 }
422 #endif
double x
X location.
Definition: Vector3.hh:311
Quaternion GetInverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:100
void SetFromEuler(const Vector3 &_vec)
Set the quaternion from Euler angles.
double y
Y location.
Definition: Vector3.hh:314
Quaternion operator*(const Quaternion &_q) const
Multiplication operator.
Definition: Quaternion.hh:235
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
A 3x3 matrix class.
Definition: Matrix4.hh:40
double y
y value of the quaternion
Definition: Quaternion.hh:383
bool equal(const T &_a, const T &_b, const T &_epsilon=1e-6)
check if two values are equal, within a tolerance
Definition: Helpers.hh:171
double z
Z location.
Definition: Vector3.hh:317
double w
w value of the quaternion
Definition: Quaternion.hh:377
A 3x3 matrix class.
Definition: Matrix3.hh:34
Vector3 RotateVector(const Vector3 &_vec) const
Rotate a vector using the quaternion.
Definition: Quaternion.hh:276
Vector3 GetAsEuler() const
Return the rotation in Euler angles.
void Correct()
Correct any nan values in this quaternion.
Definition: Quaternion.hh:293
A quaternion class.
Definition: Quaternion.hh:42
friend std::istream & operator>>(std::istream &_in, gazebo::math::Quaternion &_q)
Stream extraction operator.
Definition: Quaternion.hh:405
GAZEBO_VISIBLE void Set(common::Image &_img, const msgs::Image &_msg)
Convert a msgs::Image to a common::Image.
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:182
double x
x value of the quaternion
Definition: Quaternion.hh:380
An angle and related functions.
Definition: Angle.hh:53
double z
z value of the quaternion
Definition: Quaternion.hh:386
friend std::ostream & operator<<(std::ostream &_out, const gazebo::math::Quaternion &_q)
Stream insertion operator.
Definition: Quaternion.hh:392