mt
 All Classes Files Functions Enumerations Groups Pages
rotation.h
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2006 by Adolfo Rodriguez *
3  * adolfo.rodriguez@upc.edu *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the *
17  * Free Software Foundation, Inc., *
18  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19  ***************************************************************************/
20 
22 
23 // HEADER GUARD
24 #ifndef MT_ROTATION_H
25 #define MT_ROTATION_H
26 
27 // MT LIBRARY HEADERS
28 #include <mt/exception.h>
29 #include <mt/unit3.h>
30 #include <mt/matrix3x3.h>
31 #include <mt/plane3.h>
32 #include <mt/quaternion.h>
33 
34 
36 
37 namespace mt
38 {
39 
40 
42 
45 
99 
100 class Rotation : public Quaternion
101 {
102 public:
103 
104 // LIFECYCLE
105 
107  Rotation();
108 
112  Rotation(const Scalar& x,
113  const Scalar& y,
114  const Scalar& z,
115  const Scalar& w);
116 
120  explicit Rotation(const Scalar* r);
121 
123  Rotation(const Unit3& axis,
124  const Scalar& angle);
125 
132  Rotation(const Scalar& yaw,
133  const Scalar& pitch,
134  const Scalar& roll);
135 
137  Rotation(const Matrix3x3& mat);
138 
140  Rotation(const Unit3& u,
141  const Unit3& v,
142  const Unit3& r,
143  const Unit3& s);
144 
147  Rotation(const Unit3& u,
148  const Unit3& v);
149 
151  Rotation(const Quaternion& q);
152 
153  // Compiler generated copy constructor for Rotation input is being used
154 
155  // Compiler generated destructor is being used
156 
157 
158 // OPERATORS
159 
160  // Compiler generated assignment operator for Rotation input is being used
161 
165  Rotation& operator=(const Quaternion& q);
166 
168  Vector3 operator()(const Vector3& v) const;
169 
172  bool operator==(const Rotation& r) const;
173 
174  bool operator!=(const Rotation& r) const;
175 
176 // OPERATIONS
177 
179  Scalar angleCos(const Rotation& r) const;
180 
183  Scalar angle(const Rotation& r) const;
184 
186  Rotation inverse() const;
187 
188 // ACCESS
189 
207  void getAxisAngle(Unit3& axis,
208  Scalar& angle) const;
209 
255  void getYpr(Scalar& yaw,
256  Scalar& pitch,
257  Scalar& roll) const;
258 
271  Matrix3x3 getMatrix() const;
272 
274  void setValue(const Scalar& x,
275  const Scalar& y,
276  const Scalar& z,
277  const Scalar& w);
278 
280  void setValue(const Scalar* r);
281 
294  void setAxisAngle(const Unit3& axis,
295  const Scalar& angle);
296 
334  void setYpr(const Scalar& yaw,
335  const Scalar& pitch,
336  const Scalar& roll);
337 
412  void setMatrix(const Matrix3x3& mat);
413 
450  void setTwoPairs(const Unit3& u,
451  const Unit3& v,
452  const Unit3& r,
453  const Unit3& s);
454 
460  void setOnePair(const Unit3& u,
461  const Unit3& v);
462 };
463 
464 
466 
467 // FUNCTIONS
468 
470 Scalar angleCos(const Rotation& r1,
471  const Rotation& r2);
472 
475 Scalar angle(const Rotation& r1,
476  const Rotation& r2);
477 
479 Rotation inverse(const Rotation& r);
480 
481 
482 // OPERATORS
483 std::ostream& operator<<(std::ostream& os,
484  const Rotation& q);
485 
486 
488 
489 // LIFECYCLE
490 
492 
493 
494 inline Rotation::Rotation(const Scalar& x,
495  const Scalar& y,
496  const Scalar& z,
497  const Scalar& w)
498 {
499  setValue(x, y, z, w);
500 }
501 
502 
503 inline Rotation::Rotation(const Scalar* r)
504 {
505  setValue(r);
506 }
507 
508 
509 inline Rotation::Rotation(const Unit3& axis,
510  const Scalar& angle)
511 {
512  setAxisAngle(axis, angle);
513 }
514 
515 
516 inline Rotation::Rotation(const Scalar& yaw,
517  const Scalar& pitch,
518  const Scalar& roll)
519 {
520  setYpr(yaw, pitch, roll);
521 }
522 
523 
524 inline Rotation::Rotation(const Matrix3x3& mat)
525 {
526  setMatrix(mat);
527 }
528 
529 
530 inline Rotation::Rotation(const Unit3& u,
531  const Unit3& v,
532  const Unit3& r,
533  const Unit3& s)
534 {
535  setTwoPairs(u, v, r, s);
536 }
537 
538 
539 inline Rotation::Rotation(const Unit3& u,
540  const Unit3& v)
541 {
542  setOnePair(u, v);
543 }
544 
545 
547 {
548  normalize();
549 }
550 
551 
552 // OPERATORS
553 
555 {
556  if (this != &q)
557  {
558  // Assignation
560  normalize();
561  }
562  return *this;
563 }
564 
565 
566 inline Vector3 Rotation::operator()(const Vector3& v) const
567 {
568  const Quaternion q(*this * (v * conjugate()));
569  return Vector3(q[0], q[1], q[2]);
570 }
571 
572 
573 inline bool Rotation::operator==(const Rotation& r) const
574 {
575  return (angle(r) == 0.0);
576 }
577 
578 
579 inline bool Rotation::operator!=(const Rotation& r) const
580 {
581  return !(*this == r);
582 }
583 
584 
585 // OPERATIONS
586 
587 inline Scalar Rotation::angleCos(const Rotation& r) const
588 {
589  Scalar ang_cos(dot(r));
590  return ang_cos;
591 }
592 
593 
594 inline Scalar Rotation::angle(const Rotation& r) const
595 {
596  const Scalar ang_cos(angleCos(r));
597  return acos(ang_cos);
598 }
599 
600 
602 {
603  return conjugate();
604 }
605 
606 
607 // ACCESS
608 
609 inline void Rotation::getAxisAngle(Unit3& axis,
610  Scalar& angle) const
611 {
612  angle = Scalar(2.0) * acos(m_co[3]);
613 
614  if (abs(m_co[3]) != Scalar(1.0))
615  {
616  // Angle is nonzero
617  const Scalar den(sqrt(Scalar(1.0) - sq(m_co[3])));
618 
619  axis.setValue(m_co[0] / den,
620  m_co[1] / den,
621  m_co[2] / den);
622  }
623  // If the angle equals zero, the current rotation axis is maintained
624 }
625 
626 
627 inline void Rotation::getYpr(Scalar& yaw,
628  Scalar& pitch,
629  Scalar& roll) const
630 {
631  const Scalar s_pitch(Scalar(2.0) * (m_co[1] * m_co[3] - m_co[0] * m_co[2]));
632  pitch = asin(s_pitch); // belongs to [-pi/2, pi/2]
633 
634  if (abs(s_pitch) == Scalar(1.0))
635  {
636  // Singularity
637  const Scalar s(Scalar(2.0) / length2());
638  const Scalar r_13(s * (m_co[0] * m_co[2] + m_co[1] * m_co[3]));
639  const Scalar r_23(s * (m_co[1] * m_co[2] - m_co[0] * m_co[3]));
640 
641 
642  if (s_pitch == Scalar(1.0))
643  {
644  // Pitch = pi/2
645  yaw = roll - atan2(-r_23, r_13);
646  }
647  else
648  {
649  // Pitch = -pi/2
650  yaw = atan2(-r_23, -r_13) - roll;
651  }
652  }
653  else
654  {
655  // No singularities
656  const Scalar s_yaw (Scalar(2.0) * (m_co[0] * m_co[1] + m_co[2] * m_co[3]));
657  const Scalar s_roll(Scalar(2.0) * (m_co[0] * m_co[3] + m_co[1] * m_co[2]));
658 
659  const Scalar c_yaw ( sq(m_co[0]) - sq(m_co[1]) - sq(m_co[2]) +
660  sq(m_co[3]));
661  const Scalar c_roll(-sq(m_co[0]) - sq(m_co[1]) + sq(m_co[2]) +
662  sq(m_co[3]));
663 
664  yaw = atan2(s_yaw , c_yaw );
665  roll = atan2(s_roll, c_roll);
666  }
667 
668  // Normalizes angles to the interval [0, 2pi)
669  // Pitch has been already normalized
670  yaw = mt::normalize(yaw, Scalar(0.0), TWO_PI);
671  pitch = mt::normalize(pitch, Scalar(0.0), TWO_PI);
672  roll = mt::normalize(roll, Scalar(0.0), TWO_PI);
673 }
674 
675 
677 {
678  const Scalar xx(Scalar(2.0) * m_co[0] * m_co[0]);
679  const Scalar yy(Scalar(2.0) * m_co[1] * m_co[1]);
680  const Scalar zz(Scalar(2.0) * m_co[2] * m_co[2]);
681  // const Scalar ww(2.0 * m_co[3] * m_co[3]);
682 
683  const Scalar xy(Scalar(2.0) * m_co[0] * m_co[1]);
684  const Scalar xz(Scalar(2.0) * m_co[0] * m_co[2]);
685  const Scalar xw(Scalar(2.0) * m_co[0] * m_co[3]);
686  const Scalar yz(Scalar(2.0) * m_co[1] * m_co[2]);
687  const Scalar yw(Scalar(2.0) * m_co[1] * m_co[3]);
688  const Scalar zw(Scalar(2.0) * m_co[2] * m_co[3]);
689 
690  return Matrix3x3(Scalar(1.0) - yy - zz, xy - zw, xz + yw,
691  xy + zw, Scalar(1.0) - xx - zz, yz - xw,
692  xz - yw, yz + xw, Scalar(1.0) - xx - yy);
693 }
694 
695 
696 inline void Rotation::setValue(const Scalar& x,
697  const Scalar& y,
698  const Scalar& z,
699  const Scalar& w)
700 {
701  Quaternion::setValue(x, y, z, w);
702  normalize();
703 }
704 
705 
706 inline void Rotation::setValue(const Scalar* r)
707 {
709  normalize();
710 }
711 
712 
713 inline void Rotation::setAxisAngle(const Unit3& axis,
714  const Scalar& angle)
715 {
716  // Normalizes angle to the interval [0, 2pi)
717  const Scalar angle_n (mt::normalize(angle, Scalar(0.0), TWO_PI));
718 
719  const Scalar s = sin(angle_n * Scalar(0.5));
720  const Scalar c = cos(angle_n * Scalar(0.5));
721 
722  setValue(axis[0] * s,
723  axis[1] * s,
724  axis[2] * s,
725  c);
726 }
727 
728 
729 inline void Rotation::setYpr(const Scalar& yaw,
730  const Scalar& pitch,
731  const Scalar& roll)
732 {
733 
734  // Normalizes yaw, pitch and roll angles to the interval [0, 2pi)
735  const Scalar yaw_n (mt::normalize(yaw, Scalar(0.0), TWO_PI));
736  const Scalar pitch_n(mt::normalize(pitch, Scalar(0.0), TWO_PI));
737  const Scalar roll_n (mt::normalize(roll, Scalar(0.0), TWO_PI));
738 
739  const Scalar half_yaw = yaw_n * Scalar(0.5);
740  const Scalar half_pitch = pitch_n * Scalar(0.5);
741  const Scalar half_roll = roll_n * Scalar(0.5);
742 
743  const Scalar cy = cos(half_yaw);
744  const Scalar sy = sin(half_yaw);
745 
746  const Scalar cp = cos(half_pitch);
747  const Scalar sp = sin(half_pitch);
748 
749  const Scalar cr = cos(half_roll);
750  const Scalar sr = sin(half_roll);
751 
752  setValue(sr * cp * cy - cr * sp * sy,
753  cr * sp * cy + sr * cp * sy,
754  cr * cp * sy - sr * sp * cy,
755  cr * cp * cy + sr * sp * sy);
756 
757 }
758 
759 
760 inline void Rotation::setMatrix(const Matrix3x3& mat)
761 {
762 
763  // Checks that matrix is orthogonal and represents a right-handed coordinate
764  // system
765  #ifdef MT_USE_BASIC_SCALAR
766  const Scalar det(mat.determinant());
767  const bool is_compliant = det == Scalar(1.0);
768  util::Assert(is_compliant, Exception("Matrix does not comply with \
769 orthogonality and right-hand system requirements."));
770  #endif
771 
772  if (mat[1][1] >= -mat[2][2] &&
773  mat[0][0] >= -mat[1][1] &&
774  mat[0][0] >= -mat[2][2])
775  {
776  const Scalar k = Scalar(1.0) + mat[0][0] + mat[1][1] + mat[2][2];
777  const Scalar s = Scalar(2.0) * sqrt(k);
778  m_co[0] = (mat[2][1] - mat[1][2]) / s;
779  m_co[1] = (mat[0][2] - mat[2][0]) / s;
780  m_co[2] = (mat[1][0] - mat[0][1]) / s;
781  m_co[3] = Scalar(0.25) * s;
782  }
783  else if (mat[1][1] < -mat[2][2] &&
784  mat[0][0] >= mat[1][1] &&
785  mat[0][0] >= mat[2][2])
786  {
787  const Scalar k = Scalar(1.0) + mat[0][0] - mat[1][1] - mat[2][2];
788  const Scalar s = Scalar(2.0) * sqrt(k);
789  m_co[0] = Scalar(0.25) * s;
790  m_co[1] = (mat[1][0] + mat[0][1]) / s;
791  m_co[2] = (mat[0][2] + mat[2][0]) / s;
792  m_co[3] = (mat[2][1] - mat[1][2]) / s;
793  }
794  else if (mat[1][1] >= mat[2][2] &&
795  mat[0][0] < mat[1][1] &&
796  mat[0][0] < -mat[2][2])
797  {
798  const Scalar k = Scalar(1.0) - mat[0][0] + mat[1][1] - mat[2][2];
799  const Scalar s = Scalar(2.0) * sqrt(k);
800  m_co[0] = (mat[1][0] + mat[0][1]) / s;
801  m_co[1] = Scalar(0.25) * s;
802  m_co[2] = (mat[2][1] + mat[1][2]) / s;
803  m_co[3] = (mat[0][2] - mat[2][0]) / s;
804  }
805  else
806  {
807  const Scalar k = Scalar(1.0) - mat[0][0] - mat[1][1] + mat[2][2];
808  const Scalar s = Scalar(2.0) * sqrt(k);
809  m_co[0] = (mat[0][2] + mat[2][0]) / s;
810  m_co[1] = (mat[2][1] + mat[1][2]) / s;
811  m_co[2] = Scalar(0.25) * s;
812  m_co[3] = (mat[1][0] - mat[0][1]) / s;
813  }
814 
815 }
816 
817 
818 inline void Rotation::setTwoPairs(const Unit3& u,
819  const Unit3& v,
820  const Unit3& r,
821  const Unit3& s)
822 {
823  // Checks that pairs of input vectors are independent
824  #ifdef MT_USE_BASIC_SCALAR
825  const Scalar dot_uv = abs(u.dot(v));
826  const Scalar dot_rs = abs(r.dot(s));
827 
828  util::Assert((dot_uv != 1.0 && dot_rs != 1.0),
829  Exception("Pairs of unit vectors are not independent."));
830  #endif
831 
832  // Orthonormal basis associated to u and v
833  Unit3 vu;
834  Unit3 w;
835  orthonormalBasis(u, v, vu, w);
836 
837  // Orthonormal basis associated to r and s
838  Unit3 sr;
839  Unit3 t;
840  orthonormalBasis(r, s, sr, t);
841 
842  const Matrix3x3 M1(r[0], sr[0], t[0],
843  r[1], sr[1], t[1],
844  r[2], sr[2], t[2]);
845 
846  const Matrix3x3 M2(u[0], vu[0], w[0],
847  u[1], vu[1], w[1],
848  u[2], vu[2], w[2]);
849 
850  const Matrix3x3 Mrot = timesTranspose(M1, M2);
851  setMatrix(Mrot);
852 }
853 
854 
855 inline void Rotation::setOnePair(const Unit3& u,
856  const Unit3& v)
857 {
858  // Rotation angle
859  const Scalar ang_cos = u.angleCos(v);
860  const Scalar ang = acos(ang_cos);
861 
862  // Rotation axis
863  Unit3 axis;
864 
865  if (abs(ang_cos) == 1.0)
866  {
867  // (Anti)parallel vectors
868  // Selects a vector normal to v from the infinite possibilities
869  Plane3 P;
870  P.setNormal(v);
871  axis = P.getSupportVector();
872  }
873  else
874  {
875  axis = cross(u, v);
876  }
877 
878  // Sets rotation value
879  setAxisAngle(axis, ang);
880 }
881 
882 
884 
885 // OPERATORS
886 
887 inline std::ostream& operator<<(std::ostream& os,
888  const Rotation& r)
889 {
890  Unit3 axis;
891  Scalar angle;
892 
893  r.getAxisAngle(axis, angle);
894 
895  return os << "axis: " << axis << ", angle: " << angle;
896 }
897 
898 
899 // FUNCTIONS
900 
901 inline Scalar angleCos(const Rotation& r1,
902  const Rotation& r2)
903 {
904  return r1.angleCos(r2);
905 }
906 
907 
908 inline Scalar angle(const Rotation& r1,
909  const Rotation& r2)
910 {
911  return r1.angle(r2);
912 }
913 
914 
915 inline Rotation inverse(const Rotation& r)
916 {
917  return r.inverse();
918 }
919 
920 
921 } // mt
922 
923 #endif // MT_ROTATION_H