Main MRPT website > C++ reference for MRPT 1.9.9
CQuaternion.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9
10 #pragma once
11
14 #include <mrpt/core/exceptions.h>
15 #include <mrpt/core/bits_math.h> // square()
16
17 namespace mrpt
18 {
19 namespace math
20 {
21 // For use with a constructor of CQuaternion
23 {
25 };
26
27 /** A quaternion, which can represent a 3D rotation as pair \f$(r,\mathbf{u}) 28 *\f$, with a real part "r" and a 3D vector \f$\mathbf{u} = (x,y,z) \f$, or
29  *alternatively, q = r + ix + jy + kz.
30  *
31  * The elements of the quaternion can be accessed by either:
32  * - r(), x(), y(), z(), or
33  * - the operator [] with indices running from 0 (=r) to 3 (=z).
34  *
35  * Users will usually employ the type CQuaternionDouble instead of this
36  *template.
37  *
39  * - http://people.csail.mit.edu/bkph/articles/Quaternions.pdf
40  * - http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
41  *
42  * \ingroup mrpt_math_grp
43  * \sa mrpt::poses::CPose3D
44  */
45 template <class T>
46 class CQuaternion : public CArrayNumeric<T, 4>
47 {
49
50  public:
51  /* @{ Constructors
52  */
53
54  /** Can be used with UNINITIALIZED_QUATERNION as argument, does not
55  * initialize the 4 elements of the quaternion (use this constructor when
56  * speed is critical). */
58  /** Default constructor: construct a (1, (0,0,0) ) quaternion representing
59  * no rotation. */
60  inline CQuaternion()
61  {
62  (*this)[0] = 1;
63  (*this)[1] = 0;
64  (*this)[2] = 0;
65  (*this)[3] = 0;
66  }
67
68  /** Construct a quaternion from its parameters 'r', 'x', 'y', 'z', with q =
69  * r + ix + jy + kz. */
70  inline CQuaternion(const T r, const T x, const T y, const T z)
71  {
72  (*this)[0] = r;
73  (*this)[1] = x;
74  (*this)[2] = y;
75  (*this)[3] = z;
76  ASSERTMSG_(
77  std::abs(normSqr() - 1.0) < 1e-3,
79  "Initialization data for quaternion is not normalized: %f %f "
80  "%f %f -> sqrNorm=%f",
81  r, x, y, z, normSqr()));
82  }
83
84  /* @}
85  */
86
87  /** Return r coordinate of the quaternion */
88  inline T r() const { return (*this)[0]; }
89  /** Return x coordinate of the quaternion */
90  inline T x() const { return (*this)[1]; }
91  /** Return y coordinate of the quaternion */
92  inline T y() const { return (*this)[2]; }
93  /** Return z coordinate of the quaternion */
94  inline T z() const { return (*this)[3]; }
95  /** Set r coordinate of the quaternion */
96  inline void r(const T r) { (*this)[0] = r; }
97  /** Set x coordinate of the quaternion */
98  inline void x(const T x) { (*this)[1] = x; }
99  /** Set y coordinate of the quaternion */
100  inline void y(const T y) { (*this)[2] = y; }
101  /** Set z coordinate of the quaternion */
102  inline void z(const T z) { (*this)[3] = z; }
103  /** Set this quaternion to the rotation described by a 3D (Rodrigues)
104  * rotation vector \f$\mathbf{v} \f$:
105  * If \f$\mathbf{v}=0 \f$, then the quaternion is \f$\mathbf{q} = [1 ~ 106 * 0 ~ 0 ~ 0]^\top \f$, otherwise:
107  * \f[ \mathbf{q} = \left[ \begin{array}{c}
108  * \cos(\frac{\theta}{2}) \\
109  * v_x \frac{\sin(\frac{\theta}{2})}{\theta} \\
110  * v_y \frac{\sin(\frac{\theta}{2})}{\theta} \\
111  * v_z \frac{\sin(\frac{\theta}{2})}{\theta}
112  * \end{array} \right] \f]
113  * where \f$\theta = |\mathbf{v}| = \sqrt{v_x^2+v_y^2+v_z^2} \f$.
114  * \sa "Representing Attitude: Euler Angles, Unit Quaternions, and
115  * Rotation Vectors (2006)", James Diebel.
116  */
117  template <class ARRAYLIKE3>
118  void fromRodriguesVector(const ARRAYLIKE3& v)
119  {
120  if (v.size() != 3) THROW_EXCEPTION("Vector v must have a length=3");
121
122  const T x = v[0];
123  const T y = v[1];
124  const T z = v[2];
125  const T angle = std::sqrt(x * x + y * y + z * z);
126  if (angle < 1e-7)
127  {
128  (*this)[0] = 1;
129  (*this)[1] = static_cast<T>(0.5) * x;
130  (*this)[2] = static_cast<T>(0.5) * y;
131  (*this)[3] = static_cast<T>(0.5) * z;
132  }
133  else
134  {
135  const T s = (::sin(angle / 2)) / angle;
136  const T c = ::cos(angle / 2);
137  (*this)[0] = c;
138  (*this)[1] = x * s;
139  (*this)[2] = y * s;
140  (*this)[3] = z * s;
141  }
142  }
143
144  /** @name Lie Algebra methods
145  @{ */
146
147  /** Logarithm of the 3x3 matrix defined by this pose, generating the
148  * corresponding vector in the SO(3) Lie Algebra,
149  * which coincides with the so-called "rotation vector" (I don't have
150  * space here for the proof ;-).
151  * \param[out] out_ln The target vector, which can be: std::vector<>, or
152  * mrpt::math::CVectorDouble or any row or column Eigen::Matrix<>.
153  * \sa exp, mrpt::poses::SE_traits */
154  template <class ARRAYLIKE3>
155  inline void ln(ARRAYLIKE3& out_ln) const
156  {
157  if (out_ln.size() != 3) out_ln.resize(3);
158  this->ln_noresize(out_ln);
159  }
160  /** overload that returns by value */
161  template <class ARRAYLIKE3>
162  inline ARRAYLIKE3 ln() const
163  {
164  ARRAYLIKE3 out_ln;
165  this->ln(out_ln);
166  return out_ln;
167  }
168  /** Like ln() but does not try to resize the output vector. */
169  template <class ARRAYLIKE3>
170  void ln_noresize(ARRAYLIKE3& out_ln) const
171  {
172  using mrpt::square;
173  const T xyz_norm = std::sqrt(square(x()) + square(y()) + square(z()));
174  const T K = (xyz_norm < 1e-7) ? 2 : 2 * ::acos(r()) / xyz_norm;
175  out_ln[0] = K * x();
176  out_ln[1] = K * y();
177  out_ln[2] = K * z();
178  }
179
180  /** Exponential map from the SO(3) Lie Algebra to unit quaternions.
181  * \sa ln, mrpt::poses::SE_traits */
182  template <class ARRAYLIKE3>
183  inline static CQuaternion<T> exp(const ARRAYLIKE3& v)
184  {
186  q.fromRodriguesVector(v);
187  return q;
188  }
190  template <class ARRAYLIKE3>
191  inline static void exp(const ARRAYLIKE3& v, CQuaternion<T>& out_quat)
192  {
193  out_quat.fromRodriguesVector(v);
194  }
195
196  /** @} */ // end of Lie algebra
197
198  /** Calculate the "cross" product (or "composed rotation") of two
199  * quaternion: this = q1 x q2
200  * After the operation, "this" will represent the composed rotations of
201  * q1 and q2 (q2 applied "after" q1).
202  */
203  inline void crossProduct(const CQuaternion& q1, const CQuaternion& q2)
204  {
205  // First: compute result, then save in this object. In this way we avoid
206  // problems when q1 or q2 == *this !!
207  const T new_r = q1.r() * q2.r() - q1.x() * q2.x() - q1.y() * q2.y() -
208  q1.z() * q2.z();
209  const T new_x = q1.r() * q2.x() + q2.r() * q1.x() + q1.y() * q2.z() -
210  q2.y() * q1.z();
211  const T new_y = q1.r() * q2.y() + q2.r() * q1.y() + q1.z() * q2.x() -
212  q2.z() * q1.x();
213  const T new_z = q1.r() * q2.z() + q2.r() * q1.z() + q1.x() * q2.y() -
214  q2.x() * q1.y();
215  this->r(new_r);
216  this->x(new_x);
217  this->y(new_y);
218  this->z(new_z);
219  this->normalize();
220  }
221
222  /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this
223  * quaternion
224  */
225  void rotatePoint(
226  const double lx, const double ly, const double lz, double& gx,
227  double& gy, double& gz) const
228  {
229  const double t2 = r() * x();
230  const double t3 = r() * y();
231  const double t4 = r() * z();
232  const double t5 = -x() * x();
233  const double t6 = x() * y();
234  const double t7 = x() * z();
235  const double t8 = -y() * y();
236  const double t9 = y() * z();
237  const double t10 = -z() * z();
238  gx = 2 * ((t8 + t10) * lx + (t6 - t4) * ly + (t3 + t7) * lz) + lx;
239  gy = 2 * ((t4 + t6) * lx + (t5 + t10) * ly + (t9 - t2) * lz) + ly;
240  gz = 2 * ((t7 - t3) * lx + (t2 + t9) * ly + (t5 + t8) * lz) + lz;
241  }
242
243  /** Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse
244  * (conjugate) of this quaternion
245  */
246  void inverseRotatePoint(
247  const double lx, const double ly, const double lz, double& gx,
248  double& gy, double& gz) const
249  {
250  const double t2 = -r() * x();
251  const double t3 = -r() * y();
252  const double t4 = -r() * z();
253  const double t5 = -x() * x();
254  const double t6 = x() * y();
255  const double t7 = x() * z();
256  const double t8 = -y() * y();
257  const double t9 = y() * z();
258  const double t10 = -z() * z();
259  gx = 2 * ((t8 + t10) * lx + (t6 - t4) * ly + (t3 + t7) * lz) + lx;
260  gy = 2 * ((t4 + t6) * lx + (t5 + t10) * ly + (t9 - t2) * lz) + ly;
261  gz = 2 * ((t7 - t3) * lx + (t2 + t9) * ly + (t5 + t8) * lz) + lz;
262  }
263
264  /** Return the squared norm of the quaternion */
265  inline double normSqr() const
266  {
267  using mrpt::square;
268  return square(r()) + square(x()) + square(y()) + square(z());
269  }
270
271  /** Normalize this quaternion, so its norm becomes the unitity.
272  */
273  inline void normalize()
274  {
275  const T qq = 1.0 / std::sqrt(normSqr());
276  for (unsigned int i = 0; i < 4; i++) (*this)[i] *= qq;
277  }
278
279  /** Calculate the 4x4 Jacobian of the normalization operation of this
280  * quaternion.
281  * The output matrix can be a dynamic or fixed size (4x4) matrix.
282  */
283  template <class MATRIXLIKE>
284  void normalizationJacobian(MATRIXLIKE& J) const
285  {
286  const T n = 1.0 / std::pow(normSqr(), T(1.5));
287  J.setSize(4, 4);
288  J.get_unsafe(0, 0) = x() * x() + y() * y() + z() * z();
289  J.get_unsafe(0, 1) = -r() * x();
290  J.get_unsafe(0, 2) = -r() * y();
291  J.get_unsafe(0, 3) = -r() * z();
292
293  J.get_unsafe(1, 0) = -x() * r();
294  J.get_unsafe(1, 1) = r() * r() + y() * y() + z() * z();
295  J.get_unsafe(1, 2) = -x() * y();
296  J.get_unsafe(1, 3) = -x() * z();
297
298  J.get_unsafe(2, 0) = -y() * r();
299  J.get_unsafe(2, 1) = -y() * x();
300  J.get_unsafe(2, 2) = r() * r() + x() * x() + z() * z();
301  J.get_unsafe(2, 3) = -y() * z();
302
303  J.get_unsafe(3, 0) = -z() * r();
304  J.get_unsafe(3, 1) = -z() * x();
305  J.get_unsafe(3, 2) = -z() * y();
306  J.get_unsafe(3, 3) = r() * r() + x() * x() + y() * y();
307  J *= n;
308  }
309
310  /** Compute the Jacobian of the rotation composition operation \f$p = 311 * f(\cdot) = q_{this} \times r \f$, that is the 4x4 matrix \f$312 * \frac{\partial f}{\partial q_{this} } \f$.
313  * The output matrix can be a dynamic or fixed size (4x4) matrix.
314  */
315  template <class MATRIXLIKE>
316  inline void rotationJacobian(MATRIXLIKE& J) const
317  {
318  J.setSize(4, 4);
319  J.get_unsafe(0, 0) = r();
320  J.get_unsafe(0, 1) = -x();
321  J.get_unsafe(0, 2) = -y();
322  J.get_unsafe(0, 3) = -z();
323  J.get_unsafe(1, 0) = x();
324  J.get_unsafe(1, 1) = r();
325  J.get_unsafe(1, 2) = -z();
326  J.get_unsafe(1, 3) = y();
327  J.get_unsafe(2, 0) = y();
328  J.get_unsafe(2, 1) = z();
329  J.get_unsafe(2, 2) = r();
330  J.get_unsafe(2, 3) = -x();
331  J.get_unsafe(3, 0) = z();
332  J.get_unsafe(3, 1) = -y();
333  J.get_unsafe(3, 2) = x();
334  J.get_unsafe(3, 3) = r();
335  }
336
337  /** Calculate the 3x3 rotation matrix associated to this quaternion: \f[
338  * \mathbf{R} = \left(
339  * \begin{array}{ccc}
340  * q_r^2+q_x^2-q_y^2-q_z^2 & 2(q_x q_y - q_r q_z) & 2(q_z
341  * q_x+q_r q_y) \\
342  * 2(q_x q_y+q_r q_z) & q_r^2-q_x^2+q_y^2-q_z^2 & 2(q_y
343  * q_z-q_r q_x) \\
344  * 2(q_z q_x-q_r q_y) & 2(q_y q_z+q_r q_x) & q_r^2- q_x^2 - q_y^2 +
345  * q_z^2
346  * \end{array}
347  * \right)\f]
348  *
349  */
350  template <class MATRIXLIKE>
351  inline void rotationMatrix(MATRIXLIKE& M) const
352  {
353  M.setSize(3, 3);
355  }
356
357  /** Fill out the top-left 3x3 block of the given matrix with the rotation
358  * matrix associated to this quaternion (does not resize the matrix, for
359  * that, see rotationMatrix). */
360  template <class MATRIXLIKE>
361  inline void rotationMatrixNoResize(MATRIXLIKE& M) const
362  {
363  M.get_unsafe(0, 0) = r() * r() + x() * x() - y() * y() - z() * z();
364  M.get_unsafe(0, 1) = 2 * (x() * y() - r() * z());
365  M.get_unsafe(0, 2) = 2 * (z() * x() + r() * y());
366  M.get_unsafe(1, 0) = 2 * (x() * y() + r() * z());
367  M.get_unsafe(1, 1) = r() * r() - x() * x() + y() * y() - z() * z();
368  M.get_unsafe(1, 2) = 2 * (y() * z() - r() * x());
369  M.get_unsafe(2, 0) = 2 * (z() * x() - r() * y());
370  M.get_unsafe(2, 1) = 2 * (y() * z() + r() * x());
371  M.get_unsafe(2, 2) = r() * r() - x() * x() - y() * y() + z() * z();
372  }
373
374  /** Return the conjugate quaternion */
375  inline void conj(CQuaternion& q_out) const
376  {
377  q_out.r(r());
378  q_out.x(-x());
379  q_out.y(-y());
380  q_out.z(-z());
381  }
382
383  /** Return the conjugate quaternion */
384  inline CQuaternion conj() const
385  {
386  CQuaternion q_aux;
387  conj(q_aux);
388  return q_aux;
389  }
390
391  /** Return the yaw, pitch & roll angles associated to quaternion
392  * \sa For the equations, see The MRPT Book, or see
393  * http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf
394  * \sa rpy_and_jacobian
395  */
396  inline void rpy(T& roll, T& pitch, T& yaw) const
397  {
399  roll, pitch, yaw, static_cast<mrpt::math::CMatrixDouble*>(nullptr));
400  }
401
402  /** Return the yaw, pitch & roll angles associated to quaternion, and
403  * (optionally) the 3x4 Jacobian of the transformation.
404  * Note that both the angles and the Jacobian have one set of normal
405  * equations, plus other special formulas for the degenerated cases of
406  * |pitch|=90 degrees.
407  * \sa For the equations, see The MRPT Book, or
408  * http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf
409  * \sa rpy
410  */
411  template <class MATRIXLIKE>
412  void rpy_and_jacobian(
413  T& roll, T& pitch, T& yaw, MATRIXLIKE* out_dr_dq = nullptr,
414  bool resize_out_dr_dq_to3x4 = true) const
415  {
416  using mrpt::square;
417  using std::sqrt;
418
419  if (out_dr_dq && resize_out_dr_dq_to3x4) out_dr_dq->setSize(3, 4);
420  const T discr = r() * y() - x() * z();
421  if (fabs(discr) > 0.49999)
422  { // pitch = 90 deg
423  pitch = 0.5 * M_PI;
424  yaw = -2 * atan2(x(), r());
425  roll = 0;
426  if (out_dr_dq)
427  {
428  out_dr_dq->zeros();
429  out_dr_dq->get_unsafe(0, 0) = +2 / x();
430  out_dr_dq->get_unsafe(0, 2) = -2 * r() / (x() * x());
431  }
432  }
433  else if (discr < -0.49999)
434  { // pitch =-90 deg
435  pitch = -0.5 * M_PI;
436  yaw = 2 * atan2(x(), r());
437  roll = 0;
438  if (out_dr_dq)
439  {
440  out_dr_dq->zeros();
441  out_dr_dq->get_unsafe(0, 0) = -2 / x();
442  out_dr_dq->get_unsafe(0, 2) = +2 * r() / (x() * x());
443  }
444  }
445  else
446  { // Non-degenerate case:
447  yaw = ::atan2(
448  2 * (r() * z() + x() * y()), 1 - 2 * (y() * y() + z() * z()));
449  pitch = ::asin(2 * discr);
450  roll = ::atan2(
451  2 * (r() * x() + y() * z()), 1 - 2 * (x() * x() + y() * y()));
452  if (out_dr_dq)
453  {
454  // Auxiliary terms:
455  const double val1 = (2 * x() * x() + 2 * y() * y() - 1);
456  const double val12 = square(val1);
457  const double val2 = (2 * r() * x() + 2 * y() * z());
458  const double val22 = square(val2);
459  const double xy2 = 2 * x() * y();
460  const double rz2 = 2 * r() * z();
461  const double ry2 = 2 * r() * y();
462  const double val3 = (2 * y() * y() + 2 * z() * z() - 1);
463  const double val4 =
464  ((square(rz2 + xy2) / square(val3) + 1) * val3);
465  const double val5 = (4 * (rz2 + xy2)) / square(val3);
466  const double val6 =
467  1.0 / (square(rz2 + xy2) / square(val3) + 1);
468  const double val7 = 2.0 / sqrt(1 - square(ry2 - 2 * x() * z()));
469  const double val8 = (val22 / val12 + 1);
470  const double val9 = -2.0 / val8;
471  // row 1:
472  out_dr_dq->get_unsafe(0, 0) = -2 * z() / val4;
473  out_dr_dq->get_unsafe(0, 1) = -2 * y() / val4;
474  out_dr_dq->get_unsafe(0, 2) =
475  -(2 * x() / val3 - y() * val5) * val6;
476  out_dr_dq->get_unsafe(0, 3) =
477  -(2 * r() / val3 - z() * val5) * val6;
478  // row 2:
479  out_dr_dq->get_unsafe(1, 0) = y() * val7;
480  out_dr_dq->get_unsafe(1, 1) = -z() * val7;
481  out_dr_dq->get_unsafe(1, 2) = r() * val7;
482  out_dr_dq->get_unsafe(1, 3) = -x() * val7;
483  // row 3:
484  out_dr_dq->get_unsafe(2, 0) = val9 * x() / val1;
485  out_dr_dq->get_unsafe(2, 1) =
486  val9 * (r() / val1 - (2 * x() * val2) / val12);
487  out_dr_dq->get_unsafe(2, 2) =
488  val9 * (z() / val1 - (2 * y() * val2) / val12);
489  out_dr_dq->get_unsafe(2, 3) = val9 * y() / val1;
490  }
491  }
492  }
494  inline CQuaternion operator*(const T& factor)
495  {
496  CQuaternion q = *this;
497  q *= factor;
498  return q;
499  }
500
501 }; // end class
502
503 /** A quaternion of data type "double" */
505 /** A quaternion of data type "float" */
507
508 } // namespace math
509 } // end namespace mrpt
void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq=nullptr, bool resize_out_dr_dq_to3x4=true) const
Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of th...
Definition: CQuaternion.h:411
void inverseRotatePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz) const
Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion...
Definition: CQuaternion.h:245
static CQuaternion< T > exp(const ARRAYLIKE3 &v)
Exponential map from the SO(3) Lie Algebra to unit quaternions.
Definition: CQuaternion.h:182
void rotationMatrix(MATRIXLIKE &M) const
Calculate the 3x3 rotation matrix associated to this quaternion: .
Definition: CQuaternion.h:350
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:272
GLdouble GLdouble z
Definition: glext.h:3872
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
TConstructorFlags_Quaternions
Definition: CQuaternion.h:22
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:91
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
GLenum GLsizei n
Definition: glext.h:5074
void rotationMatrixNoResize(MATRIXLIKE &M) const
Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quate...
Definition: CQuaternion.h:360
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
GLdouble s
Definition: glext.h:3676
T square(const T x)
Inline function for the square of a number.
double normSqr() const
Return the squared norm of the quaternion.
Definition: CQuaternion.h:264
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:87
CQuaternion operator*(const T &factor)
Definition: CQuaternion.h:493
void crossProduct(const CQuaternion &q1, const CQuaternion &q2)
Calculate the "cross" product (or "composed rotation") of two quaternion: this = q1 x q2 After the op...
Definition: CQuaternion.h:202
const GLubyte * c
Definition: glext.h:6313
void rotationJacobian(MATRIXLIKE &J) const
Compute the Jacobian of the rotation composition operation , that is the 4x4 matrix ...
Definition: CQuaternion.h:315
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
void rotatePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz) const
Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion.
Definition: CQuaternion.h:224
void ln_noresize(ARRAYLIKE3 &out_ln) const
Like ln() but does not try to resize the output vector.
Definition: CQuaternion.h:169
ARRAYLIKE3 ln() const
Definition: CQuaternion.h:161
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:89
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:283
CQuaternion(const T r, const T x, const T y, const T z)
Construct a quaternion from its parameters &#39;r&#39;, &#39;x&#39;, &#39;y&#39;, &#39;z&#39;, with q = r + ix + jy + kz...
Definition: CQuaternion.h:70
GLenum GLint GLint y
Definition: glext.h:3538
GLenum GLint x
Definition: glext.h:3538
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:46
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:93
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:395
CQuaternion conj() const
Return the conjugate quaternion.
Definition: CQuaternion.h:383
CQuaternion()
Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation.
Definition: CQuaternion.h:60
CQuaternion(TConstructorFlags_Quaternions)
Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quat...
Definition: CQuaternion.h:57
void fromRodriguesVector(const ARRAYLIKE3 &v)
Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector : If ...
Definition: CQuaternion.h:117

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019