All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Quaternion.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 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 /* 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 "gazebo/math/Helpers.hh"
30 #include "gazebo/math/Angle.hh"
31 #include "gazebo/math/Vector3.hh"
32 #include "gazebo/math/Matrix3.hh"
33 #include "gazebo/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 
152  public: void SetFromEuler(const Vector3 &_vec);
153 
158  public: void SetFromEuler(double _roll, double _pitch, double _yaw);
159 
162  public: Vector3 GetAsEuler() const;
163 
166  public: static Quaternion EulerToQuaternion(const Vector3 &_vec);
167 
172  public: static Quaternion EulerToQuaternion(double _x,
173  double _y,
174  double _z);
175 
178  public: double GetRoll();
179 
182  public: double GetPitch();
183 
186  public: double GetYaw();
187 
191  public: void GetAsAxis(Vector3 &_axis, double &_angle) const;
192 
195  public: void Scale(double _scale);
196 
200  public: Quaternion operator+(const Quaternion &_qt) const;
201 
205  public: Quaternion operator+=(const Quaternion &_qt);
206 
210  public: Quaternion operator-(const Quaternion &_qt) const;
211 
215  public: Quaternion operator-=(const Quaternion &_qt);
216 
220  public: inline Quaternion operator*(const Quaternion &_q) const
221  {
222  return Quaternion(
223  this->w*_q.w - this->x*_q.x - this->y*_q.y - this->z*_q.z,
224  this->w*_q.x + this->x*_q.w + this->y*_q.z - this->z*_q.y,
225  this->w*_q.y - this->x*_q.z + this->y*_q.w + this->z*_q.x,
226  this->w*_q.z + this->x*_q.y - this->y*_q.x + this->z*_q.w);
227  }
228 
232  public: Quaternion operator*(const double &_f) const;
233 
237  public: Quaternion operator*=(const Quaternion &qt);
238 
241  public: Vector3 operator*(const Vector3 &_v) const;
242 
246  public: bool operator ==(const Quaternion &_qt) const;
247 
251  public: bool operator!=(const Quaternion &_qt) const;
252 
255  public: Quaternion operator-() const;
256 
260  public: inline Vector3 RotateVector(const Vector3 &_vec) const
261  {
262  Quaternion tmp(0.0, _vec.x, _vec.y, _vec.z);
263  tmp = (*this) * (tmp * this->GetInverse());
264  return Vector3(tmp.x, tmp.y, tmp.z);
265  }
266 
270  public: Vector3 RotateVectorReverse(Vector3 _vec) const;
271 
274  public: bool IsFinite() const;
275 
277  public: inline void Correct()
278  {
279  if (!finite(this->x))
280  this->x = 0;
281  if (!finite(this->y))
282  this->y = 0;
283  if (!finite(this->z))
284  this->z = 0;
285  if (!finite(this->w))
286  this->w = 1;
287 
288  if (math::equal(this->w, 0.0) &&
289  math::equal(this->x, 0.0) &&
290  math::equal(this->y, 0.0) &&
291  math::equal(this->z, 0.0))
292  {
293  this->w = 1;
294  }
295  }
296 
298  public: Matrix3 GetAsMatrix3() const;
299 
302  public: Matrix4 GetAsMatrix4() const;
303 
306  public: Vector3 GetXAxis() const;
307 
310  public: Vector3 GetYAxis() const;
311 
314  public: Vector3 GetZAxis() const;
315 
318  public: void Round(int _precision);
319 
323  public: double Dot(const Quaternion &_q) const;
324 
334  public: static Quaternion Squad(double _fT, const Quaternion &_rkP,
335  const Quaternion &_rkA, const Quaternion &_rkB,
336  const Quaternion &_rkQ, bool _shortestPath = false);
337 
345  public: static Quaternion Slerp(double _fT, const Quaternion &_rkP,
346  const Quaternion &_rkQ, bool _shortestPath = false);
347 
348 
350  public: double w;
351 
353  public: double x;
354 
356  public: double y;
357 
359  public: double z;
360 
365  public: friend std::ostream &operator<<(std::ostream &_out,
366  const gazebo::math::Quaternion &_q)
367  {
368  Vector3 v(_q.GetAsEuler());
369  _out << precision(v.x, 6) << " " << precision(v.y, 6) << " "
370  << precision(v.z, 6);
371  return _out;
372  }
373 
378  public: friend std::istream &operator>>(std::istream &_in,
380  {
381  Angle roll, pitch, yaw;
382 
383  // Skip white spaces
384  _in.setf(std::ios_base::skipws);
385  _in >> roll >> pitch >> yaw;
386 
387  _q.SetFromEuler(Vector3(*roll, *pitch, *yaw));
388 
389  return _in;
390  }
391  };
393  }
394 }
395 #endif
396