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