28 #include <mt/exception.h>
124 const Scalar&
angle);
174 bool operator!=(
const Rotation& r)
const;
208 Scalar& angle)
const;
295 const Scalar& angle);
334 void setYpr(
const Scalar& yaw,
483 std::ostream& operator<<(std::ostream& os,
569 return Vector3(q[0], q[1], q[2]);
575 return (
angle(r) == 0.0);
579 inline bool Rotation::operator!=(
const Rotation& r)
const
581 return !(*
this == r);
589 Scalar ang_cos(
dot(r));
597 return acos(ang_cos);
612 angle = Scalar(2.0) * acos(m_co[3]);
614 if (abs(m_co[3]) != Scalar(1.0))
617 const Scalar den(sqrt(Scalar(1.0) - sq(m_co[3])));
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);
634 if (abs(s_pitch) == Scalar(1.0))
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]));
642 if (s_pitch == Scalar(1.0))
645 yaw = roll - atan2(-r_23, r_13);
650 yaw = atan2(-r_23, -r_13) - roll;
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]));
659 const Scalar c_yaw ( sq(m_co[0]) - sq(m_co[1]) - sq(m_co[2]) +
661 const Scalar c_roll(-sq(m_co[0]) - sq(m_co[1]) + sq(m_co[2]) +
664 yaw = atan2(s_yaw , c_yaw );
665 roll = atan2(s_roll, c_roll);
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]);
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]);
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);
717 const Scalar angle_n (mt::normalize(angle, Scalar(0.0), TWO_PI));
719 const Scalar s = sin(angle_n * Scalar(0.5));
720 const Scalar c = cos(angle_n * Scalar(0.5));
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));
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);
743 const Scalar cy = cos(half_yaw);
744 const Scalar sy = sin(half_yaw);
746 const Scalar cp = cos(half_pitch);
747 const Scalar sp = sin(half_pitch);
749 const Scalar cr = cos(half_roll);
750 const Scalar sr = sin(half_roll);
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);
765 #ifdef MT_USE_BASIC_SCALAR
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."));
772 if (mat[1][1] >= -mat[2][2] &&
773 mat[0][0] >= -mat[1][1] &&
774 mat[0][0] >= -mat[2][2])
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;
783 else if (mat[1][1] < -mat[2][2] &&
784 mat[0][0] >= mat[1][1] &&
785 mat[0][0] >= mat[2][2])
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;
794 else if (mat[1][1] >= mat[2][2] &&
795 mat[0][0] < mat[1][1] &&
796 mat[0][0] < -mat[2][2])
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;
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;
824 #ifdef MT_USE_BASIC_SCALAR
825 const Scalar dot_uv = abs(u.
dot(v));
826 const Scalar dot_rs = abs(r.
dot(s));
828 util::Assert((dot_uv != 1.0 && dot_rs != 1.0),
829 Exception(
"Pairs of unit vectors are not independent."));
835 orthonormalBasis(u, v, vu, w);
840 orthonormalBasis(r, s, sr, t);
850 const Matrix3x3 Mrot = timesTranspose(M1, M2);
859 const Scalar ang_cos = u.
angleCos(v);
860 const Scalar ang = acos(ang_cos);
865 if (abs(ang_cos) == 1.0)
887 inline std::ostream& operator<<(std::ostream& os,
895 return os <<
"axis: " << axis <<
", angle: " << angle;
901 inline Scalar angleCos(
const Rotation& r1,
904 return r1.angleCos(r2);
908 inline Scalar angle(
const Rotation& r1,
915 inline Rotation inverse(
const Rotation& r)
923 #endif // MT_ROTATION_H