4#ifndef BALL_MATHS_QUATERNION_H
5#define BALL_MATHS_QUATERNION_H
11#include <Eigen/Geometry>
101 void set(
const Eigen::Quaternion<T>& q);
110 void set(
const T& a,
const T& b,
const T&
c,
const T& d);
288 void dump(std::ostream& s = std::cout,
Size depth = 0)
const;
293 template <
typename T>
300 template <
typename T>
306 template <
typename T>
312 template <
typename T>
318 template <
typename T>
325 template <
typename T>
330 template <
typename T>
336 template <
typename T>
339 Eigen::Quaternion<T>::operator= (q);
342 template <
typename T>
345 Eigen::Quaternion<T>::operator= (q);
348 template <
typename T>
358 template <
typename T>
366 template <
typename T>
374 template <
typename T>
378 return Eigen::Quaternion<T>::isApprox(q);
381 template <
typename T>
385 Eigen::Quaternion<T>::setIdentity();
388 template <
typename T>
392 Eigen::Quaternion<T>::normalize();
397 template <
typename T>
406 template <
typename T>
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);
415 set(q.w(), q.x(), q.y(), q.z());
418 template <
typename T>
421 T half_yaw = yaw / 2.0;
422 T half_pitch = pitch / 2.0;
423 T half_roll = roll / 2.0;
425 T cosYaw = cos(half_yaw);
426 T sinYaw = sin(half_yaw);
428 T cosPitch = cos(half_pitch);
429 T sinPitch = sin(half_pitch);
431 T cosRoll = cos(half_roll);
432 T sinRoll = sin(half_roll);
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;
440 template <
typename T>
443 Eigen::AngleAxis<T> aa(*
this);
445 Eigen::Matrix<T, 3, 1> eigen_axis = aa.axis();
446 axis.
set(eigen_axis[0], eigen_axis[1], eigen_axis[2]);
451 template <
typename T>
455 getRotationMatrix(matrix);
456 T sinYaw, cosYaw, sinPitch, cosPitch, sinRoll, cosRoll;
458 sinPitch = -matrix(2,0);
459 cosPitch = sqrt(1 - sinPitch*sinPitch);
463 sinRoll = matrix(2,1) / cosPitch;
464 cosRoll = matrix(2,2) / cosPitch;
465 sinYaw = matrix(1,0) / cosPitch;
466 cosYaw = matrix(0,0) / cosPitch;
470 sinRoll = -matrix(1,2);
471 cosRoll = matrix(1,1);
477 yaw = atan2(sinYaw, cosYaw);
480 pitch = atan2(sinPitch, cosPitch);
483 roll = atan2(sinRoll, cosRoll);
486 template <
typename T>
493 template <
typename T>
496 Eigen::AngleAxis<T> aa(*
this);
501 template <
typename T>
504 Eigen::AngleAxis<T> aa(*
this);
505 Eigen::Matrix<T, 3, 1> eigen_axis = aa.axis();
507 return TVector3<T>(eigen_axis[0], eigen_axis[1], eigen_axis[2]);
510 template <
typename T>
513 Eigen::Matrix<T, 3, 3> eigen_rot = this->toRotationMatrix();
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,
525 template <
typename T>
529 return Eigen::Quaternion<T>::inverse();
532 template <
typename T>
536 return Eigen::Quaternion<T>::conjugate();
539 template <
typename T>
542 return Eigen::Quaternion<T>::coeffs()[3];
545 template <
typename T>
548 return Eigen::Quaternion<T>::coeffs()[3];
551 template <
typename T>
554 return Eigen::Quaternion<T>::coeffs()[0];
557 template <
typename T>
560 return Eigen::Quaternion<T>::coeffs()[0];
563 template <
typename T>
566 return Eigen::Quaternion<T>::coeffs()[1];
569 template <
typename T>
572 return Eigen::Quaternion<T>::coeffs()[1];
575 template <
typename T>
578 return Eigen::Quaternion<T>::coeffs()[2];
581 template <
typename T>
584 return Eigen::Quaternion<T>::coeffs()[2];
587 template <
typename T>
591 s >>
c >> q.
w() >>
c >> q.
i() >>
c >> q.
j() >>
c >> q.
k() >>
c;
595 template <
typename T>
598 s <<
'(' << q.
w() <<
',' << q.
i() <<
','
599 << q.
j() <<
',' << q.
k() <<
')';
604 template <
typename T>
612 s <<
" w: " << w() << std::endl;
615 s <<
" i: " << i() << std::endl;
618 s <<
" j: " << j() << std::endl;
621 s <<
" k: " <<
k() << std::endl;
#define BALL_DUMP_STREAM_PREFIX(os)
#define BALL_DUMP_STREAM_SUFFIX(os)
#define BALL_DUMP_DEPTH(os, depth)
#define BALL_DUMP_HEADER(os, cl, ob)
BALL_EXPORT std::ostream & operator<<(std::ostream &os, const Exception::GeneralException &e)
std::istream & operator>>(std::istream &is, TRegularData1D< ValueType > &grid)
Input operator.
TQuaternion< float > Quaternion
BALL_EXTERN_VARIABLE const double k
BALL_EXTERN_VARIABLE const double c
BALL_EXTERN_VARIABLE double EPSILON
TQuaternion(const Eigen::Quaternion< T > &q)
void set(const TQuaternion &q)
TQuaternion(const T &a, const T &b, const T &c, const T &d)
void dump(std::ostream &s=std::cout, Size depth=0) const
TQuaternion getConjugate() const
void fromAxisAngle(const TVector3< T > &axis, const T &angle)
void fromEulerAngles(const T &yaw, const T &pitch, const T &roll)
void get(TQuaternion &q) const
void swap(TQuaternion &q)
TQuaternion(const TQuaternion &q)
TQuaternion(const TVector3< T > &axis, const T &angle)
TMatrix4x4< T > & getRotationMatrix(TMatrix4x4< T > &m) const
void toAxisAngle(TVector3< T > &axis, T &angle)
TQuaternion & operator=(const TQuaternion &q)
bool operator==(const TQuaternion< T > &q)
Equality operator Returns true if the given Quaternion q is equal to this.
TQuaternion< T > & normalize()
void set(const T &a, const T &b, const T &c, const T &d)
TQuaternion getInverse() const
void set(const Eigen::Quaternion< T > &q)
void toEulerAngles(T &yaw, T &pitch, T &roll)