quaternion.h
Go to the documentation of this file.
1 
7 #ifndef CQUATERNION_H
8 #define CQUATERNION_H
9 
10 #include <argos3/core/utility/math/vector3.h>
11 #include <argos3/core/utility/math/matrix/rotationmatrix3.h>
12 
13 namespace argos {
14 
15  class CQuaternion {
16 
17  public:
19  m_fValues[0] = 1.0;
20  m_fValues[1] = 0.0;
21  m_fValues[2] = 0.0;
22  m_fValues[3] = 0.0;
23  }
24 
25  CQuaternion(const CQuaternion& c_quaternion) {
26  *this = c_quaternion;
27  }
28 
29  CQuaternion(Real f_real,
30  Real f_img1,
31  Real f_img2,
32  Real f_img3) {
33  m_fValues[0] = f_real;
34  m_fValues[1] = f_img1;
35  m_fValues[2] = f_img2;
36  m_fValues[3] = f_img3;
37  }
38 
39  CQuaternion(const CRadians& c_radians,
40  const CVector3& c_vector3) {
41  FromAngleAxis(c_radians, c_vector3);
42  }
43 
44  inline CQuaternion(const CVector3& c_vector1,
45  const CVector3& c_vector2) {
46  BetweenTwoVectors(c_vector1, c_vector2);
47  }
48 
49  inline Real GetW() const {
50  return m_fValues[0];
51  }
52 
53  inline Real GetX() const {
54  return m_fValues[1];
55  }
56 
57  inline Real GetY() const {
58  return m_fValues[2];
59  }
60 
61  inline Real GetZ() const {
62  return m_fValues[3];
63  }
64 
65  inline void SetW(Real f_w) {
66  m_fValues[0] = f_w;
67  }
68 
69  inline void SetX(Real f_x) {
70  m_fValues[1] = f_x;
71  }
72 
73  inline void SetY(Real f_y) {
74  m_fValues[2] = f_y;
75  }
76 
77  inline void SetZ(Real f_z) {
78  m_fValues[3] = f_z;
79  }
80 
81  inline void Set(Real f_w,
82  Real f_x,
83  Real f_y,
84  Real f_z) {
85  m_fValues[0] = f_w;
86  m_fValues[1] = f_x;
87  m_fValues[2] = f_y;
88  m_fValues[3] = f_z;
89  }
90 
91  inline CQuaternion Conjugate() const {
92  return CQuaternion(m_fValues[0],
93  -m_fValues[1],
94  -m_fValues[2],
95  -m_fValues[3]);
96  }
97 
98  inline CQuaternion Inverse() const {
99  return CQuaternion(m_fValues[0],
100  -m_fValues[1],
101  -m_fValues[2],
102  -m_fValues[3]);
103  }
104 
105  inline Real Length() const {
106  return ::sqrt(SquareLength());
107  }
108 
109  inline Real SquareLength() const {
110  return
111  Square(m_fValues[0]) +
112  Square(m_fValues[1]) +
113  Square(m_fValues[2]) +
114  Square(m_fValues[3]);
115  }
116 
117  inline CQuaternion& Normalize() {
118  Real fInvLength = 1.0 / Length();
119  m_fValues[0] *= fInvLength;
120  m_fValues[1] *= fInvLength;
121  m_fValues[2] *= fInvLength;
122  m_fValues[3] *= fInvLength;
123  return *this;
124  }
125 
126  inline CQuaternion& FromAngleAxis(const CRadians& c_angle,
127  const CVector3& c_vector) {
128  CRadians cHalfAngle = c_angle * 0.5;
129  Real fSin, fCos;
130 #ifdef ARGOS_SINCOS
131  SinCos(cHalfAngle, fSin, fCos);
132 #else
133  fSin = Sin(cHalfAngle);
134  fCos = Cos(cHalfAngle);
135 #endif
136  m_fValues[0] = fCos;
137  m_fValues[1] = c_vector.GetX() * fSin;
138  m_fValues[2] = c_vector.GetY() * fSin;
139  m_fValues[3] = c_vector.GetZ() * fSin;
140  return *this;
141  }
142 
143  inline void ToAngleAxis(CRadians& c_angle,
144  CVector3& c_vector) const {
145  Real fSquareLength =
146  Square(m_fValues[1]) +
147  Square(m_fValues[2]) +
148  Square(m_fValues[3]);
149  if(fSquareLength > 0.0f) {
150  c_angle = 2.0f * ACos(m_fValues[0]);
151  Real fInvLength = 1.0f / ::sqrt(fSquareLength);
152  c_vector.Set(m_fValues[1] * fInvLength,
153  m_fValues[2] * fInvLength,
154  m_fValues[3] * fInvLength);
155  }
156  else {
157  /* By default, to ease the support of robot orientation, no rotation refers to the Z axis */
158  c_angle = CRadians::ZERO;
159  c_vector = CVector3::Z;
160  }
161  }
162 
163  inline CQuaternion& FromEulerAngles(const CRadians& c_z_angle,
164  const CRadians& c_y_angle,
165  const CRadians& c_x_angle) {
166  (*this) = CQuaternion(c_x_angle, CVector3::X) *
167  CQuaternion(c_y_angle, CVector3::Y) *
168  CQuaternion(c_z_angle, CVector3::Z);
169  return (*this);
170  }
171 
172  inline void ToEulerAngles(CRadians& c_z_angle,
173  CRadians& c_y_angle,
174  CRadians& c_x_angle) const {
175  /* With the ZYX convention, gimbal lock happens when
176  cos(y_angle) = 0, that is when y_angle = +- pi/2
177  In this condition, the Z and X axis overlap and we
178  lose one degree of freedom. It's a problem of the
179  Euler representation of rotations that is not
180  present when we deal with quaternions.
181  For reasons of speed, we consider gimbal lock
182  happened when fTest > 0.499 and when fTest < -0.499.
183  */
184  /* Computed to understand if we have gimbal lock or not */
185  Real fTest =
186  m_fValues[1] * m_fValues[3] +
187  m_fValues[0] * m_fValues[2];
188 
189  if(fTest > 0.49999f) {
190  /* Gimbal lock */
191  c_x_angle = CRadians::ZERO;
192  c_y_angle = CRadians::PI_OVER_TWO;
193  c_z_angle = ATan2(2.0f * (m_fValues[1] * m_fValues[2] + m_fValues[0] * m_fValues[3]),
194  1.0f - 2.0f * (m_fValues[1] * m_fValues[1] + m_fValues[3] * m_fValues[3]));
195  }
196  else if(fTest < -0.49999f) {
197  /* Gimbal lock */
198  c_x_angle = CRadians::ZERO;
199  c_y_angle = -CRadians::PI_OVER_TWO;
200  c_z_angle = ATan2(2.0f * (m_fValues[1] * m_fValues[2] + m_fValues[0] * m_fValues[3]),
201  1.0f - 2.0f * (m_fValues[1] * m_fValues[1] + m_fValues[3] * m_fValues[3]));
202  }
203  else {
204  /* Normal case */
205  Real fSqValues[4] = {
206  Square(m_fValues[0]),
207  Square(m_fValues[1]),
208  Square(m_fValues[2]),
209  Square(m_fValues[3])
210  };
211 
212  c_x_angle = ATan2(2.0 * (m_fValues[0] * m_fValues[1] - m_fValues[2] * m_fValues[3]),
213  fSqValues[0] - fSqValues[1] - fSqValues[2] + fSqValues[3]);
214  c_y_angle = ASin(2.0 * (m_fValues[1] * m_fValues[3] + m_fValues[0] * m_fValues[2]));
215  c_z_angle = ATan2(2.0 * (m_fValues[0] * m_fValues[3] - m_fValues[1] * m_fValues[2]),
216  fSqValues[0] + fSqValues[1] - fSqValues[2] - fSqValues[3]);
217  }
218  }
219 
220  inline CQuaternion& BetweenTwoVectors(const CVector3& c_vector1,
221  const CVector3& c_vector2) {
222  Real fProd =
223  c_vector1.DotProduct(c_vector2) /
224  Sqrt(c_vector1.SquareLength() * c_vector2.SquareLength());
225  if(fProd > 0.999999f) {
226  /* The two vectors are parallel, no rotation */
227  m_fValues[0] = 1.0;
228  m_fValues[1] = 0.0;
229  m_fValues[2] = 0.0;
230  m_fValues[3] = 0.0;
231  }
232  else if(fProd < -0.999999f) {
233  /* The two vectors are anti-parallel */
234  /* We need to set an arbitrary rotation axis */
235  /* To find it, we calculate the cross product of c_vector1 with either X or Y,
236  depending on which is not coplanar with c_vector1 */
237  CVector3 cRotAxis = c_vector1;
238  if(Abs(c_vector1.DotProduct(CVector3::X)) < 0.999999) {
239  /* Use the X axis */
240  cRotAxis.CrossProduct(CVector3::X);
241  }
242  else {
243  /* Use the Y axis */
244  cRotAxis.CrossProduct(CVector3::Y);
245  }
246  /* The wanted quaternion is a rotation around cRotAxis by 180 degrees */
247  FromAngleAxis(CRadians::PI, cRotAxis);
248  }
249  else {
250  /* The two vectors are not parallel nor anti-parallel */
251  m_fValues[0] = Sqrt(c_vector1.SquareLength() * c_vector2.SquareLength()) + fProd;
252  CVector3 cCrossProd(c_vector1);
253  cCrossProd.CrossProduct(c_vector2);
254  m_fValues[1] = cCrossProd.GetX();
255  m_fValues[2] = cCrossProd.GetY();
256  m_fValues[3] = cCrossProd.GetZ();
257  Normalize();
258  }
259  return *this;
260  }
261 
262  inline bool operator==(const CQuaternion& c_quaternion) {
263  return (m_fValues[0] == c_quaternion.m_fValues[0] &&
264  m_fValues[1] == c_quaternion.m_fValues[1] &&
265  m_fValues[2] == c_quaternion.m_fValues[2] &&
266  m_fValues[3] == c_quaternion.m_fValues[3]);
267  }
268 
269  inline CQuaternion& operator=(const CQuaternion& c_quaternion) {
270  if (&c_quaternion != this) {
271  m_fValues[0] = c_quaternion.m_fValues[0];
272  m_fValues[1] = c_quaternion.m_fValues[1];
273  m_fValues[2] = c_quaternion.m_fValues[2];
274  m_fValues[3] = c_quaternion.m_fValues[3];
275  }
276  return *this;
277  }
278 
279  inline CQuaternion& operator+=(const CQuaternion& c_quaternion) {
280  m_fValues[0] += c_quaternion.m_fValues[0];
281  m_fValues[1] += c_quaternion.m_fValues[1];
282  m_fValues[2] += c_quaternion.m_fValues[2];
283  m_fValues[3] += c_quaternion.m_fValues[3];
284  return *this;
285  }
286 
287  inline CQuaternion& operator-=(const CQuaternion& c_quaternion) {
288  m_fValues[0] -= c_quaternion.m_fValues[0];
289  m_fValues[1] -= c_quaternion.m_fValues[1];
290  m_fValues[2] -= c_quaternion.m_fValues[2];
291  m_fValues[3] -= c_quaternion.m_fValues[3];
292  return *this;
293  }
294 
295  inline CQuaternion& operator*=(const CQuaternion& c_quaternion) {
296  Real newv[4];
297  newv[0] = m_fValues[0] * c_quaternion.m_fValues[0] -
298  m_fValues[1] * c_quaternion.m_fValues[1] -
299  m_fValues[2] * c_quaternion.m_fValues[2] -
300  m_fValues[3] * c_quaternion.m_fValues[3];
301  newv[1] = m_fValues[0] * c_quaternion.m_fValues[1] +
302  m_fValues[1] * c_quaternion.m_fValues[0] +
303  m_fValues[2] * c_quaternion.m_fValues[3] -
304  m_fValues[3] * c_quaternion.m_fValues[2];
305  newv[2] = m_fValues[0] * c_quaternion.m_fValues[2] -
306  m_fValues[1] * c_quaternion.m_fValues[3] +
307  m_fValues[2] * c_quaternion.m_fValues[0] +
308  m_fValues[3] * c_quaternion.m_fValues[1];
309  newv[3] = m_fValues[0] * c_quaternion.m_fValues[3] +
310  m_fValues[1] * c_quaternion.m_fValues[2] -
311  m_fValues[2] * c_quaternion.m_fValues[1] +
312  m_fValues[3] * c_quaternion.m_fValues[0];
313  m_fValues[0] = newv[0];
314  m_fValues[1] = newv[1];
315  m_fValues[2] = newv[2];
316  m_fValues[3] = newv[3];
317  return *this;
318  }
319 
320  inline CQuaternion operator+(const CQuaternion& c_quaternion) const {
321  CQuaternion result(*this);
322  result += c_quaternion;
323  return result;
324  }
325 
326  inline CQuaternion operator-(const CQuaternion& c_quaternion) const {
327  CQuaternion result(*this);
328  result -= c_quaternion;
329  return result;
330  }
331 
332  inline CQuaternion operator*(const CQuaternion& c_quaternion) const {
333  CQuaternion result(*this);
334  result *= c_quaternion;
335  return result;
336  }
337 
341  operator CRotationMatrix3() const {
342  Real fS = 2.0f / SquareLength();
343  Real fXS = m_fValues[1] * fS; Real fYS = m_fValues[2] * fS; Real fZS = m_fValues[3] * fS;
344  Real fWX = m_fValues[0] * fXS; Real fWY = m_fValues[0] * fYS; Real fWZ = m_fValues[0] * fZS;
345  Real fXX = m_fValues[1] * fXS; Real fXY = m_fValues[1] * fYS; Real fXZ = m_fValues[1] * fZS;
346  Real fYY = m_fValues[2] * fYS; Real fYZ = m_fValues[2] * fZS; Real fZZ = m_fValues[3] * fZS;
347  /* return a new rotation matrix */
348  return CRotationMatrix3(1.0f - (fYY + fZZ), fXY - fWZ, fXZ + fWY,
349  fXY + fWZ, 1.0f - (fXX + fZZ), fYZ - fWX,
350  fXZ - fWY, fYZ + fWX, 1.0f - (fXX + fYY));
351  }
352 
360  inline friend std::ostream& operator<<(std::ostream& c_os, const CQuaternion& c_quaternion) {
361  CRadians cZAngle, cYAngle, cXAngle;
362  c_quaternion.ToEulerAngles(cZAngle, cYAngle, cXAngle);
363  c_os << ToDegrees(cZAngle).GetValue() << ","
364  << ToDegrees(cYAngle).GetValue() << ","
365  << ToDegrees(cXAngle).GetValue();
366  return c_os;
367  }
368 
376  inline friend std::istream& operator>>(std::istream& c_is, CQuaternion& c_quaternion) {
377  Real fValues[3];
378  ParseValues<Real>(c_is, 3, fValues, ',');
379  c_quaternion.FromEulerAngles(ToRadians(CDegrees(fValues[0])),
380  ToRadians(CDegrees(fValues[1])),
381  ToRadians(CDegrees(fValues[2])));
382  return c_is;
383  }
384 
385  private:
386 
387  Real m_fValues[4];
388  };
389 
390 }
391 
392 #endif
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
#define Sqrt
Definition: general.h:64
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
T Square(const T &t_v)
Returns the square of the value of the passed argument.
Definition: general.h:128
CRadians ACos(Real f_value)
Computes the arccosine of the passed value.
Definition: angles.h:622
Real Cos(const CRadians &c_radians)
Computes the cosine of the passed value in radians.
Definition: angles.h:595
CDegrees ToDegrees(const CRadians &c_radians)
Converts CRadians to CDegrees.
Definition: angles.h:489
void SinCos(const CRadians &c_radians, Real &f_sin, Real &f_cos)
Computes the sine and cosine of the passed value in radians.
Definition: angles.h:574
Real Sin(const CRadians &c_radians)
Computes the sine of the passed value in radians.
Definition: angles.h:586
CRadians ATan2(const Real f_y, const Real f_x)
Computes the arctangent of the passed values.
Definition: angles.h:633
CRadians ToRadians(const CDegrees &c_degrees)
Converts CDegrees to CRadians.
Definition: angles.h:498
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Definition: general.h:25
CRadians ASin(Real f_value)
Computes the arcsine of the passed value.
Definition: angles.h:613
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
static const CRadians PI
The PI constant.
Definition: angles.h:49
static const CRadians PI_OVER_TWO
Set to PI / 2.
Definition: angles.h:59
static const CRadians ZERO
Set to zero radians.
Definition: angles.h:79
It defines the basic type CDegrees, used to store an angle value in degrees.
Definition: angles.h:288
Real GetValue() const
Returns the value in degrees.
Definition: angles.h:322
CQuaternion Inverse() const
Definition: quaternion.h:98
CQuaternion & operator=(const CQuaternion &c_quaternion)
Definition: quaternion.h:269
void SetX(Real f_x)
Definition: quaternion.h:69
Real GetW() const
Definition: quaternion.h:49
CQuaternion & operator*=(const CQuaternion &c_quaternion)
Definition: quaternion.h:295
CQuaternion & FromEulerAngles(const CRadians &c_z_angle, const CRadians &c_y_angle, const CRadians &c_x_angle)
Definition: quaternion.h:163
void ToAngleAxis(CRadians &c_angle, CVector3 &c_vector) const
Definition: quaternion.h:143
Real GetX() const
Definition: quaternion.h:53
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:172
friend std::istream & operator>>(std::istream &c_is, CQuaternion &c_quaternion)
Deserializes the contents of a stream and stores it into the passed quaternion.
Definition: quaternion.h:376
CQuaternion & FromAngleAxis(const CRadians &c_angle, const CVector3 &c_vector)
Definition: quaternion.h:126
CQuaternion & BetweenTwoVectors(const CVector3 &c_vector1, const CVector3 &c_vector2)
Definition: quaternion.h:220
Real Length() const
Definition: quaternion.h:105
void SetZ(Real f_z)
Definition: quaternion.h:77
CQuaternion operator*(const CQuaternion &c_quaternion) const
Definition: quaternion.h:332
CQuaternion & Normalize()
Definition: quaternion.h:117
Real SquareLength() const
Definition: quaternion.h:109
CQuaternion(const CQuaternion &c_quaternion)
Definition: quaternion.h:25
bool operator==(const CQuaternion &c_quaternion)
Definition: quaternion.h:262
CQuaternion(const CRadians &c_radians, const CVector3 &c_vector3)
Definition: quaternion.h:39
Real GetZ() const
Definition: quaternion.h:61
Real GetY() const
Definition: quaternion.h:57
void SetW(Real f_w)
Definition: quaternion.h:65
CQuaternion operator+(const CQuaternion &c_quaternion) const
Definition: quaternion.h:320
CQuaternion & operator+=(const CQuaternion &c_quaternion)
Definition: quaternion.h:279
CQuaternion Conjugate() const
Definition: quaternion.h:91
CQuaternion operator-(const CQuaternion &c_quaternion) const
Definition: quaternion.h:326
CQuaternion(const CVector3 &c_vector1, const CVector3 &c_vector2)
Definition: quaternion.h:44
CQuaternion(Real f_real, Real f_img1, Real f_img2, Real f_img3)
Definition: quaternion.h:29
CQuaternion & operator-=(const CQuaternion &c_quaternion)
Definition: quaternion.h:287
friend std::ostream & operator<<(std::ostream &c_os, const CQuaternion &c_quaternion)
Serializes the contents of the passed quaternion into a stream as Euler angles in the Z,...
Definition: quaternion.h:360
void SetY(Real f_y)
Definition: quaternion.h:73
void Set(Real f_w, Real f_x, Real f_y, Real f_z)
Definition: quaternion.h:81
A 3D vector class.
Definition: vector3.h:31
static const CVector3 Y
The y axis.
Definition: vector3.h:39
CVector3 & CrossProduct(const CVector3 &c_vector3)
Calculates the cross product between this vector and the passed one.
Definition: vector3.h:382
Real SquareLength() const
Returns the square length of this vector.
Definition: vector3.h:219
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:105
Real DotProduct(const CVector3 &c_vector3) const
Returns the dot product between this vector and the passed one.
Definition: vector3.h:369
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:155
static const CVector3 X
The x axis.
Definition: vector3.h:36
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:121
static const CVector3 Z
The z axis.
Definition: vector3.h:42
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:137