Quaternion.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 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/Helpers.hh>
25 #include <ignition/math/Quaternion.hh>
26 
27 #include "gazebo/math/Helpers.hh"
28 #include "gazebo/math/Angle.hh"
29 #include "gazebo/math/Vector3.hh"
30 #include "gazebo/math/Matrix3.hh"
31 #include "gazebo/math/Matrix4.hh"
32 #include "gazebo/util/system.hh"
33 
34 #ifndef _WIN32
35 #pragma GCC diagnostic push
36 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
37 #endif
38 
39 namespace gazebo
40 {
41  namespace math
42  {
45 
48  class GZ_MATH_VISIBLE Quaternion
49  {
51  public: Quaternion();
52 
58  public: Quaternion(const double &_w, const double &_x, const double &_y,
59  const double &_z) GAZEBO_DEPRECATED(8.0);
60 
65  public: Quaternion(const double &_roll, const double &_pitch,
66  const double &_yaw) GAZEBO_DEPRECATED(8.0);
67 
71  public: Quaternion(const Vector3 &_axis, const double &_angle)
72  GAZEBO_DEPRECATED(8.0);
73 
76  public: Quaternion(const Vector3 &_rpy) GAZEBO_DEPRECATED(8.0);
77 
80  public: Quaternion(const Quaternion &_qt) GAZEBO_DEPRECATED(8.0);
81 
84  public: Quaternion(const ignition::math::Quaterniond &_qt)
85  GAZEBO_DEPRECATED(8.0);
86 
88  public: ~Quaternion() GAZEBO_DEPRECATED(8.0);
89 
92  public: Quaternion &operator =(const Quaternion &_qt)
93  GAZEBO_DEPRECATED(8.0);
94 
97  public: ignition::math::Quaterniond Ign() const
98  GAZEBO_DEPRECATED(8.0);
99 
103  public: Quaternion &operator =(const ignition::math::Quaterniond &_v)
104  GAZEBO_DEPRECATED(8.0);
105 
107  public: void Invert() GAZEBO_DEPRECATED(8.0);
108 
111  public: inline Quaternion GetInverse() const GAZEBO_DEPRECATED(8.0)
112  {
113  double s = 0;
114  Quaternion q(this->w, this->x, this->y, this->z);
115 
116  // use s to test if quaternion is valid
117  s = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
118 
119  if (ignition::math::equal(s, 0.0))
120  {
121  q.w = 1.0;
122  q.x = 0.0;
123  q.y = 0.0;
124  q.z = 0.0;
125  }
126  else
127  {
128  // deal with non-normalized quaternion
129  // div by s so q * qinv = identity
130  q.w = q.w / s;
131  q.x = -q.x / s;
132  q.y = -q.y / s;
133  q.z = -q.z / s;
134  }
135  return q;
136  }
137 
139  public: void SetToIdentity() GAZEBO_DEPRECATED(8.0);
140 
143  public: Quaternion GetLog() const GAZEBO_DEPRECATED(8.0);
144 
147  public: Quaternion GetExp() const GAZEBO_DEPRECATED(8.0);
148 
150  public: void Normalize() GAZEBO_DEPRECATED(8.0);
151 
157  public: void SetFromAxis(double _x, double _y, double _z, double _a)
158  GAZEBO_DEPRECATED(8.0);
159 
163  public: void SetFromAxis(const Vector3 &_axis, double _a)
164  GAZEBO_DEPRECATED(8.0);
165 
171  public: void Set(double _u, double _x, double _y, double _z)
172  GAZEBO_DEPRECATED(8.0);
173 
179  public: void SetFromEuler(const Vector3 &_vec) GAZEBO_DEPRECATED(8.0);
180 
185  public: void SetFromEuler(double _roll, double _pitch, double _yaw)
186  GAZEBO_DEPRECATED(8.0);
187 
190  public: Vector3 GetAsEuler() const
191  GAZEBO_DEPRECATED(8.0);
192 
196  public: static Quaternion EulerToQuaternion(const Vector3 &_vec)
197  GAZEBO_DEPRECATED(8.0);
198 
204  public: static Quaternion EulerToQuaternion(double _x,
205  double _y,
206  double _z)
207  GAZEBO_DEPRECATED(8.0);
208 
211  public: double GetRoll() GAZEBO_DEPRECATED(8.0);
212 
215  public: double GetPitch() GAZEBO_DEPRECATED(8.0);
216 
219  public: double GetYaw() GAZEBO_DEPRECATED(8.0);
220 
224  public: void GetAsAxis(Vector3 &_axis, double &_angle) const
225  GAZEBO_DEPRECATED(8.0);
226 
229  public: void Scale(double _scale) GAZEBO_DEPRECATED(8.0);
230 
234  public: Quaternion operator+(const Quaternion &_qt) const
235  GAZEBO_DEPRECATED(8.0);
236 
240  public: Quaternion operator+=(const Quaternion &_qt)
241  GAZEBO_DEPRECATED(8.0);
242 
246  public: Quaternion operator-(const Quaternion &_qt) const
247  GAZEBO_DEPRECATED(8.0);
248 
252  public: Quaternion operator-=(const Quaternion &_qt)
253  GAZEBO_DEPRECATED(8.0);
254 
258  public: inline Quaternion operator*(const Quaternion &_q) const
259  GAZEBO_DEPRECATED(8.0)
260  {
261  return Quaternion(
262  this->w*_q.w - this->x*_q.x - this->y*_q.y - this->z*_q.z,
263  this->w*_q.x + this->x*_q.w + this->y*_q.z - this->z*_q.y,
264  this->w*_q.y - this->x*_q.z + this->y*_q.w + this->z*_q.x,
265  this->w*_q.z + this->x*_q.y - this->y*_q.x + this->z*_q.w);
266  }
267 
271  public: Quaternion operator*(const double &_f) const
272  GAZEBO_DEPRECATED(8.0);
273 
277  public: Quaternion operator*=(const Quaternion &qt)
278  GAZEBO_DEPRECATED(8.0);
279 
283  public: Vector3 operator*(const Vector3 &_v) const
284  GAZEBO_DEPRECATED(8.0);
285 
290  public: bool operator ==(const Quaternion &_qt) const;
291 
296  public: bool operator!=(const Quaternion &_qt) const;
297 
300  public: Quaternion operator-() const GAZEBO_DEPRECATED(8.0);
301 
305  public: inline Vector3 RotateVector(const Vector3 &_vec) const
306  GAZEBO_DEPRECATED(8.0)
307  {
308  Quaternion tmp(0.0, _vec.x, _vec.y, _vec.z);
309  tmp = (*this) * (tmp * this->GetInverse());
310  return Vector3(tmp.x, tmp.y, tmp.z);
311  }
312 
316  public: Vector3 RotateVectorReverse(Vector3 _vec) const
317  GAZEBO_DEPRECATED(8.0);
318 
321  public: bool IsFinite() const GAZEBO_DEPRECATED(8.0);
322 
324  public: inline void Correct() GAZEBO_DEPRECATED(8.0)
325  {
326  if (!std::isfinite(this->x))
327  this->x = 0;
328  if (!std::isfinite(this->y))
329  this->y = 0;
330  if (!std::isfinite(this->z))
331  this->z = 0;
332  if (!std::isfinite(this->w))
333  this->w = 1;
334 
335  if (ignition::math::equal(this->w, 0.0) &&
336  ignition::math::equal(this->x, 0.0) &&
337  ignition::math::equal(this->y, 0.0) &&
338  ignition::math::equal(this->z, 0.0))
339  {
340  this->w = 1;
341  }
342  }
343 
346  public: Matrix3 GetAsMatrix3() const GAZEBO_DEPRECATED(8.0);
347 
350  public: Matrix4 GetAsMatrix4() const GAZEBO_DEPRECATED(8.0);
351 
354  public: Vector3 GetXAxis() const GAZEBO_DEPRECATED(8.0);
355 
358  public: Vector3 GetYAxis() const GAZEBO_DEPRECATED(8.0);
359 
362  public: Vector3 GetZAxis() const GAZEBO_DEPRECATED(8.0);
363 
366  public: void Round(int _precision) GAZEBO_DEPRECATED(8.0);
367 
371  public: double Dot(const Quaternion &_q) const
372  GAZEBO_DEPRECATED(8.0);
373 
384  public: static Quaternion Squad(double _fT, const Quaternion &_rkP,
385  const Quaternion &_rkA, const Quaternion &_rkB,
386  const Quaternion &_rkQ, bool _shortestPath = false)
387  GAZEBO_DEPRECATED(8.0);
388 
397  public: static Quaternion Slerp(double _fT, const Quaternion &_rkP,
398  const Quaternion &_rkQ, bool _shortestPath = false)
399  GAZEBO_DEPRECATED(8.0);
400 
407  public: Quaternion Integrate(const Vector3 &_angularVelocity,
408  const double _deltaT) const
409  GAZEBO_DEPRECATED(8.0);
410 
412  public: double w;
413 
415  public: double x;
416 
418  public: double y;
419 
421  public: double z;
422 
427  public: friend std::ostream &operator<<(std::ostream &_out,
428  const gazebo::math::Quaternion &_q)
429  GAZEBO_DEPRECATED(8.0)
430  {
431  Vector3 v(_q.GetAsEuler());
432  _out << precision(v.x, 6) << " " << precision(v.y, 6) << " "
433  << precision(v.z, 6);
434  return _out;
435  }
436 
441  public: friend std::istream &operator>>(std::istream &_in,
443  GAZEBO_DEPRECATED(8.0)
444  {
445  double roll, pitch, yaw;
446 
447  // Skip white spaces
448  _in.setf(std::ios_base::skipws);
449  _in >> roll >> pitch >> yaw;
450 
451  _q.SetFromEuler(Vector3(roll, pitch, yaw));
452 
453  return _in;
454  }
455  };
457  }
458 }
459 #ifndef _WIN32
460 #pragma GCC diagnostic pop
461 #endif
462 #endif
double x
X location.
Definition: Vector3.hh:333
double y
Y location.
Definition: Vector3.hh:336
Forward declarations for the common classes.
Definition: Animation.hh:33
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:44
static const double GAZEBO_DEPRECATED(8.0) MAX_D
Double maximum value. This value will be similar to 1.79769e+308.
Definition: Helpers.hh:140
Vector3 GetAsEuler() const GAZEBO_DEPRECATED(8.0)
Return the rotation in Euler angles.
A 3x3 matrix class.
Definition: Matrix4.hh:40
double y
y value of the quaternion
Definition: Quaternion.hh:418
friend std::istream & operator>>(std::istream &_in, gazebo::math::Quaternion &_q) GAZEBO_DEPRECATED(8.0)
Stream extraction operator.
Definition: Quaternion.hh:441
double z
Z location.
Definition: Vector3.hh:339
double w
w value of the quaternion
Definition: Quaternion.hh:412
void Correct() GAZEBO_DEPRECATED(8.0)
Correct any nan values in this quaternion.
Definition: Quaternion.hh:324
A 3x3 matrix class.
Definition: Matrix3.hh:40
Quaternion operator*(const Quaternion &_q) const GAZEBO_DEPRECATED(8.0)
Multiplication operator.
Definition: Quaternion.hh:258
void SetFromEuler(const Vector3 &_vec) GAZEBO_DEPRECATED(8.0)
Set the quaternion from Euler angles.
Vector3 RotateVector(const Vector3 &_vec) const GAZEBO_DEPRECATED(8.0)
Rotate a vector using the quaternion.
Definition: Quaternion.hh:305
A quaternion class.
Definition: Quaternion.hh:48
GAZEBO_VISIBLE void Set(common::Image &_img, const msgs::Image &_msg)
Convert a msgs::Image to a common::Image.
double x
x value of the quaternion
Definition: Quaternion.hh:415
Quaternion GetInverse() const GAZEBO_DEPRECATED(8.0)
Get the inverse of this quaternion.
Definition: Quaternion.hh:111
double z
z value of the quaternion
Definition: Quaternion.hh:421
friend std::ostream & operator<<(std::ostream &_out, const gazebo::math::Quaternion &_q) GAZEBO_DEPRECATED(8.0)
Stream insertion operator.
Definition: Quaternion.hh:427