BALL  1.5.0
quaternion.h
Go to the documentation of this file.
1 // -*- Mode: C++; tab-width: 2; -*-
2 // vi: set ts=2:
3 
4 #ifndef BALL_MATHS_QUATERNION_H
5 #define BALL_MATHS_QUATERNION_H
6 
7 
8 #include <BALL/MATHS/matrix44.h>
9 #include <BALL/MATHS/vector3.h>
10 
11 #include <Eigen/Geometry>
12 #include <iostream>
13 
14 
15 namespace BALL
16 {
22  template <typename T>
24  : public Eigen::Quaternion<T> //{/*...*/};
25  {
26 
27  public:
32 
39 
46 
52  TQuaternion(const Eigen::Quaternion<T>& q);
53 
62  TQuaternion(const T& a, const T& b, const T& c, const T& d);
63 
70  TQuaternion(const TVector3<T>& axis, const T& angle);
71 
78 
83  void clear();
84 
86 
90 
95  void set(const TQuaternion& q);
96 
101  void set(const Eigen::Quaternion<T>& q);
102 
110  void set(const T& a, const T& b, const T& c, const T& d);
111 
117 
122  TQuaternion& operator = (const Eigen::Quaternion<T>& q);
123 
128  bool operator == (const TQuaternion<T>& q);
129 
135  void setIdentity();
136 
143 
148  void swap(TQuaternion& q);
149 
155  void fromAxisAngle(const TVector3<T>& axis, const T& angle);
156 
164  void fromEulerAngles(const T& yaw, const T& pitch, const T& roll);
165 
172  void toAxisAngle(TVector3<T>& axis, T& angle);
173 
181  void toEulerAngles(T& yaw, T& pitch, T& roll);
182 
188  void get(TQuaternion& q) const;
189 
194  T getAngle() const;
195 
201 
208 
214 
222 
226 
231  T& w();
232 
237  const T& w() const;
238 
243  T& i();
244 
249  const T& i() const;
250 
255  T& j();
256 
261  const T& j() const;
262 
267  T& k();
268 
273  const T& k() const;
274 
275 
277 
280 
288  void dump(std::ostream& s = std::cout, Size depth = 0) const;
290  };
292 
293  template <typename T>
295  : Eigen::Quaternion<T>()
296  {
297  this->setIdentity();
298  }
299 
300  template <typename T>
302  : Eigen::Quaternion<T>(q)
303  {
304  }
305 
306  template <typename T>
307  TQuaternion<T>::TQuaternion(const Eigen::Quaternion<T>& q)
308  : Eigen::Quaternion<T>(q)
309  {
310  }
311 
312  template <typename T>
313  TQuaternion<T>::TQuaternion(const T& a, const T& b, const T& c, const T& d)
314  : Eigen::Quaternion<T>(a, b, c, d)
315  {
316  }
317 
318  template <typename T>
319  TQuaternion<T>::TQuaternion(const TVector3<T>& axis, const T& angle)
320  : Eigen::Quaternion<T>()
321  {
322  fromAxisAngle(axis, angle);
323  }
324 
325  template <typename T>
327  {
328  }
329 
330  template <typename T>
332  {
333  this->setIdentity();
334  }
335 
336  template <typename T>
338  {
339  Eigen::Quaternion<T>::operator= (q);
340  }
341 
342  template <typename T>
343  void TQuaternion<T>::set(const Eigen::Quaternion<T>& q)
344  {
345  Eigen::Quaternion<T>::operator= (q);
346  }
347 
348  template <typename T>
350  void TQuaternion<T>::set(const T& a, const T& b, const T& c, const T& d)
351  {
352  w() = a;
353  i() = b;
354  j() = c;
355  k() = d;
356  }
357 
358  template <typename T>
360  TQuaternion<T>& TQuaternion<T>::operator = (const Eigen::Quaternion<T>& q)
361  {
362  set(q);
363  return *this;
364  }
365 
366  template <typename T>
369  {
370  set(q);
371  return *this;
372  }
373 
374  template <typename T>
377  {
378  return Eigen::Quaternion<T>::isApprox(q);
379  }
380 
381  template <typename T>
384  {
385  Eigen::Quaternion<T>::setIdentity();
386  }
387 
388  template <typename T>
391  {
392  Eigen::Quaternion<T>::normalize();
393 
394  return *this;
395  }
396 
397  template <typename T>
399  {
400  TQuaternion<T> tmp(q);
401 
402  q = *this;
403  *this = tmp;
404  }
405 
406  template <typename T>
407  void TQuaternion<T>::fromAxisAngle(const TVector3<T>& axis, const T& angle)
408  {
409  TVector3<T> axis_normalized(axis);
410  axis_normalized.normalize();
411 
412  Eigen::AngleAxis<T> aa(angle, Eigen::Matrix<T, 3, 1>(axis_normalized.x, axis_normalized.y, axis_normalized.z));
413  Eigen::Quaternion<T> q(aa);
414 
415  set(q.w(), q.x(), q.y(), q.z());
416  }
417 
418  template <typename T>
419  void TQuaternion<T>::fromEulerAngles(const T& yaw, const T& pitch, const T& roll)
420  {
421  T half_yaw = yaw / 2.0;
422  T half_pitch = pitch / 2.0;
423  T half_roll = roll / 2.0;
424 
425  T cosYaw = cos(half_yaw);
426  T sinYaw = sin(half_yaw);
427 
428  T cosPitch = cos(half_pitch);
429  T sinPitch = sin(half_pitch);
430 
431  T cosRoll = cos(half_roll);
432  T sinRoll = sin(half_roll);
433 
434  w() = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
435  i() = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
436  j() = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
437  k() = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
438  }
439 
440  template <typename T>
442  {
443  Eigen::AngleAxis<T> aa(*this);
444 
445  Eigen::Matrix<T, 3, 1> eigen_axis = aa.axis();
446  axis.set(eigen_axis[0], eigen_axis[1], eigen_axis[2]);
447  angle = aa.angle();
448  }
449 
450 
451  template <typename T>
452  void TQuaternion<T>::toEulerAngles(T& yaw, T& pitch, T& roll)
453  {
454  TMatrix4x4<T> matrix;
455  getRotationMatrix(matrix);
456  T sinYaw, cosYaw, sinPitch, cosPitch, sinRoll, cosRoll;
457 
458  sinPitch = -matrix(2,0);
459  cosPitch = sqrt(1 - sinPitch*sinPitch);
460 
461  if ( fabs(cosPitch) > Constants::EPSILON)
462  {
463  sinRoll = matrix(2,1) / cosPitch;
464  cosRoll = matrix(2,2) / cosPitch;
465  sinYaw = matrix(1,0) / cosPitch;
466  cosYaw = matrix(0,0) / cosPitch;
467  }
468  else
469  {
470  sinRoll = -matrix(1,2);
471  cosRoll = matrix(1,1);
472  sinYaw = 0;
473  cosYaw = 1;
474  }
475 
476  // yaw
477  yaw = atan2(sinYaw, cosYaw);
478 
479  // pitch
480  pitch = atan2(sinPitch, cosPitch);
481 
482  // roll
483  roll = atan2(sinRoll, cosRoll);
484  }
485 
486  template <typename T>
489  {
490  q.set(*this);
491  }
492 
493  template <typename T>
495  {
496  Eigen::AngleAxis<T> aa(*this);
497 
498  return aa.angle();
499  }
500 
501  template <typename T>
503  {
504  Eigen::AngleAxis<T> aa(*this);
505  Eigen::Matrix<T, 3, 1> eigen_axis = aa.axis();
506 
507  return TVector3<T>(eigen_axis[0], eigen_axis[1], eigen_axis[2]);
508  }
509 
510  template <typename T>
512  {
513  Eigen::Matrix<T, 3, 3> eigen_rot = this->toRotationMatrix();
514 
515  m.set(
516  eigen_rot(0, 0), eigen_rot(0, 1), eigen_rot(0, 2), 0.0,
517  eigen_rot(1, 0), eigen_rot(1, 1), eigen_rot(1, 2), 0.0,
518  eigen_rot(2, 0), eigen_rot(2, 1), eigen_rot(2, 2), 0.0,
519  0.0, 0.0, 0.0, 1.0
520  );
521 
522  return m;
523  }
524 
525  template <typename T>
528  {
529  return Eigen::Quaternion<T>::inverse();
530  }
531 
532  template <typename T>
535  {
536  return Eigen::Quaternion<T>::conjugate();
537  }
538 
539  template <typename T>
540  const T& TQuaternion<T>::w() const
541  {
542  return Eigen::Quaternion<T>::coeffs()[3];
543  }
544 
545  template <typename T>
547  {
548  return Eigen::Quaternion<T>::coeffs()[3];
549  }
550 
551  template <typename T>
552  const T& TQuaternion<T>::i() const
553  {
554  return Eigen::Quaternion<T>::coeffs()[0];
555  }
556 
557  template <typename T>
559  {
560  return Eigen::Quaternion<T>::coeffs()[0];
561  }
562 
563  template <typename T>
564  const T& TQuaternion<T>::j() const
565  {
566  return Eigen::Quaternion<T>::coeffs()[1];
567  }
568 
569  template <typename T>
571  {
572  return Eigen::Quaternion<T>::coeffs()[1];
573  }
574 
575  template <typename T>
576  const T& TQuaternion<T>::k() const
577  {
578  return Eigen::Quaternion<T>::coeffs()[2];
579  }
580 
581  template <typename T>
583  {
584  return Eigen::Quaternion<T>::coeffs()[2];
585  }
586 
587  template <typename T>
588  std::istream& operator >>(std::istream& s, TQuaternion<T>& q)
589  {
590  char c;
591  s >> c >> q.w() >> c >> q.i() >> c >> q.j() >> c >> q.k() >> c;
592  return s;
593  }
594 
595  template <typename T>
596  std::ostream& operator << (std::ostream& s, const TQuaternion<T>& q)
597  {
598  s << '(' << q.w() << ',' << q.i() << ','
599  << q.j() << ',' << q.k() << ')';
600 
601  return s;
602  }
603 
604  template <typename T>
605  void TQuaternion<T>::dump(std::ostream& s, Size depth) const
606  {
608 
609  BALL_DUMP_HEADER(s, this, this);
610 
611  BALL_DUMP_DEPTH(s, depth);
612  s << " w: " << w() << std::endl;
613 
614  BALL_DUMP_DEPTH(s, depth);
615  s << " i: " << i() << std::endl;
616 
617  BALL_DUMP_DEPTH(s, depth);
618  s << " j: " << j() << std::endl;
619 
620  BALL_DUMP_DEPTH(s, depth);
621  s << " k: " << k() << std::endl;
622 
624  }
625 
627 
628 } // namespace BALL
629 
630 #endif // BALL_MATHS_QUATERNION_H
#define BALL_INLINE
Definition: debug.h:15
#define BALL_DUMP_STREAM_PREFIX(os)
Definition: macros.h:391
#define BALL_DUMP_STREAM_SUFFIX(os)
Definition: macros.h:395
#define BALL_DUMP_DEPTH(os, depth)
Definition: macros.h:390
#define BALL_DUMP_HEADER(os, cl, ob)
Definition: macros.h:393
BALL_EXPORT std::ostream & operator<<(std::ostream &os, const Exception::GeneralException &e)
Definition: constants.h:13
std::istream & operator>>(std::istream &is, TRegularData1D< ValueType > &grid)
Input operator.
TQuaternion< float > Quaternion
Definition: quaternion.h:626
BALL_EXTERN_VARIABLE const double k
Definition: constants.h:93
BALL_EXTERN_VARIABLE const double c
Definition: constants.h:149
BALL_EXTERN_VARIABLE double EPSILON
Definition: constants.h:43
Default Type.
Definition: matrix44.h:68
void set(const T *ptr)
Definition: matrix44.h:825
TQuaternion(const Eigen::Quaternion< T > &q)
Definition: quaternion.h:307
void set(const TQuaternion &q)
Definition: quaternion.h:337
TVector3< T > getAxis()
Definition: quaternion.h:502
TQuaternion(const T &a, const T &b, const T &c, const T &d)
Definition: quaternion.h:313
const T & j() const
Definition: quaternion.h:564
void dump(std::ostream &s=std::cout, Size depth=0) const
Definition: quaternion.h:605
TQuaternion getConjugate() const
Definition: quaternion.h:534
const T & k() const
Definition: quaternion.h:576
void fromAxisAngle(const TVector3< T > &axis, const T &angle)
Definition: quaternion.h:407
void fromEulerAngles(const T &yaw, const T &pitch, const T &roll)
Definition: quaternion.h:419
void get(TQuaternion &q) const
Definition: quaternion.h:488
void swap(TQuaternion &q)
Definition: quaternion.h:398
const T & i() const
Definition: quaternion.h:552
TQuaternion(const TQuaternion &q)
Definition: quaternion.h:301
TQuaternion(const TVector3< T > &axis, const T &angle)
Definition: quaternion.h:319
TMatrix4x4< T > & getRotationMatrix(TMatrix4x4< T > &m) const
Definition: quaternion.h:511
const T & w() const
Definition: quaternion.h:540
void toAxisAngle(TVector3< T > &axis, T &angle)
Definition: quaternion.h:441
TQuaternion & operator=(const TQuaternion &q)
Definition: quaternion.h:368
bool operator==(const TQuaternion< T > &q)
Equality operator Returns true if the given Quaternion q is equal to this.
Definition: quaternion.h:376
T getAngle() const
Definition: quaternion.h:494
TQuaternion< T > & normalize()
Definition: quaternion.h:390
void set(const T &a, const T &b, const T &c, const T &d)
Definition: quaternion.h:350
TQuaternion getInverse() const
Definition: quaternion.h:527
void set(const Eigen::Quaternion< T > &q)
Definition: quaternion.h:343
void toEulerAngles(T &yaw, T &pitch, T &roll)
Definition: quaternion.h:452
TVector3 & normalize()
Definition: vector3.h:770
void set(const T *ptr)
Definition: vector3.h:615