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;
630 #endif // BALL_MATHS_QUATERNION_H