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 #include "gazebo/util/system.hh"
35 
36 namespace gazebo
37 {
38  namespace math
39  {
42 
46  {
48  public: Quaternion();
49 
55  public: Quaternion(const double &_w, const double &_x, const double &_y,
56  const double &_z);
57 
62  public: Quaternion(const double &_roll, const double &_pitch,
63  const double &_yaw);
64 
68  public: Quaternion(const Vector3 &_axis, const double &_angle);
69 
72  public: Quaternion(const Vector3 &_rpy);
73 
76  public: Quaternion(const Quaternion &_qt);
77 
79  public: ~Quaternion();
80 
83  public: Quaternion &operator =(const Quaternion &_qt);
84 
86  public: void Invert();
87 
90  public: inline Quaternion GetInverse() const
91  {
92  double s = 0;
93  Quaternion q(this->w, this->x, this->y, this->z);
94 
95  // use s to test if quaternion is valid
96  s = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
97 
98  if (math::equal(s, 0.0))
99  {
100  q.w = 1.0;
101  q.x = 0.0;
102  q.y = 0.0;
103  q.z = 0.0;
104  }
105  else
106  {
107  // deal with non-normalized quaternion
108  // div by s so q * qinv = identity
109  q.w = q.w / s;
110  q.x = -q.x / s;
111  q.y = -q.y / s;
112  q.z = -q.z / s;
113  }
114  return q;
115  }
116 
118  public: void SetToIdentity();
119 
122  public: Quaternion GetLog() const;
123 
126  public: Quaternion GetExp() const;
127 
129  public: void Normalize();
130 
136  public: void SetFromAxis(double _x, double _y, double _z, double _a);
137 
141  public: void SetFromAxis(const Vector3 &_axis, double _a);
142 
148  public: void Set(double _u, double _x, double _y, double _z);
149 
153  public: void SetFromEuler(const Vector3 &_vec);
154 
159  public: void SetFromEuler(double _roll, double _pitch, double _yaw);
160 
163  public: Vector3 GetAsEuler() const;
164 
167  public: static Quaternion EulerToQuaternion(const Vector3 &_vec);
168 
173  public: static Quaternion EulerToQuaternion(double _x,
174  double _y,
175  double _z);
176 
179  public: double GetRoll();
180 
183  public: double GetPitch();
184 
187  public: double GetYaw();
188 
192  public: void GetAsAxis(Vector3 &_axis, double &_angle) const;
193 
196  public: void Scale(double _scale);
197 
201  public: Quaternion operator+(const Quaternion &_qt) const;
202 
206  public: Quaternion operator+=(const Quaternion &_qt);
207 
211  public: Quaternion operator-(const Quaternion &_qt) const;
212 
216  public: Quaternion operator-=(const Quaternion &_qt);
217 
221  public: inline Quaternion operator*(const Quaternion &_q) const
222  {
223  return Quaternion(
224  this->w*_q.w - this->x*_q.x - this->y*_q.y - this->z*_q.z,
225  this->w*_q.x + this->x*_q.w + this->y*_q.z - this->z*_q.y,
226  this->w*_q.y - this->x*_q.z + this->y*_q.w + this->z*_q.x,
227  this->w*_q.z + this->x*_q.y - this->y*_q.x + this->z*_q.w);
228  }
229 
233  public: Quaternion operator*(const double &_f) const;
234 
238  public: Quaternion operator*=(const Quaternion &qt);
239 
242  public: Vector3 operator*(const Vector3 &_v) const;
243 
247  public: bool operator ==(const Quaternion &_qt) const;
248 
252  public: bool operator!=(const Quaternion &_qt) const;
253 
256  public: Quaternion operator-() const;
257 
261  public: inline Vector3 RotateVector(const Vector3 &_vec) const
262  {
263  Quaternion tmp(0.0, _vec.x, _vec.y, _vec.z);
264  tmp = (*this) * (tmp * this->GetInverse());
265  return Vector3(tmp.x, tmp.y, tmp.z);
266  }
267 
271  public: Vector3 RotateVectorReverse(Vector3 _vec) const;
272 
275  public: bool IsFinite() const;
276 
278  public: inline void Correct()
279  {
280  if (!std::isfinite(this->x))
281  this->x = 0;
282  if (!std::isfinite(this->y))
283  this->y = 0;
284  if (!std::isfinite(this->z))
285  this->z = 0;
286  if (!std::isfinite(this->w))
287  this->w = 1;
288 
289  if (math::equal(this->w, 0.0) &&
290  math::equal(this->x, 0.0) &&
291  math::equal(this->y, 0.0) &&
292  math::equal(this->z, 0.0))
293  {
294  this->w = 1;
295  }
296  }
297 
299  public: Matrix3 GetAsMatrix3() const;
300 
303  public: Matrix4 GetAsMatrix4() const;
304 
307  public: Vector3 GetXAxis() const;
308 
311  public: Vector3 GetYAxis() const;
312 
315  public: Vector3 GetZAxis() const;
316 
319  public: void Round(int _precision);
320 
324  public: double Dot(const Quaternion &_q) const;
325 
335  public: static Quaternion Squad(double _fT, const Quaternion &_rkP,
336  const Quaternion &_rkA, const Quaternion &_rkB,
337  const Quaternion &_rkQ, bool _shortestPath = false);
338 
346  public: static Quaternion Slerp(double _fT, const Quaternion &_rkP,
347  const Quaternion &_rkQ, bool _shortestPath = false);
348 
349 
351  public: double w;
352 
354  public: double x;
355 
357  public: double y;
358 
360  public: double z;
361 
366  public: friend std::ostream &operator<<(std::ostream &_out,
367  const gazebo::math::Quaternion &_q)
368  {
369  Vector3 v(_q.GetAsEuler());
370  _out << precision(v.x, 6) << " " << precision(v.y, 6) << " "
371  << precision(v.z, 6);
372  return _out;
373  }
374 
379  public: friend std::istream &operator>>(std::istream &_in,
381  {
382  Angle roll, pitch, yaw;
383 
384  // Skip white spaces
385  _in.setf(std::ios_base::skipws);
386  _in >> roll >> pitch >> yaw;
387 
388  _q.SetFromEuler(Vector3(*roll, *pitch, *yaw));
389 
390  return _in;
391  }
392  };
394  }
395 }
396 #endif
397