21 #include "RMatrixBase.hh"
22 #include <Base/Common/W32Compat.hh>
23 #include <Base/Debug/Exception.hh>
24 #include <Base/Common/CompareFloatingPoint.hh>
25 #include <Base/Geometry/Quaternion.hh>
29 # define _USE_MATH_DEFINES
35 #define R_CONSTRAINT_ACCURACY 1e-10
38 #define MINIMUM_ROTATION 1e-6
42 #define MAX_QUATERNION_COS_ERROR 1e-12
47 #include <Base/Common/BIASpragma.hh>
62 if (!
Check(R_CONSTRAINT_ACCURACY)) {
63 #ifdef R_CONSTRAINTS_WARNING
64 BIASWARN(
"Rotation matrix is invalid: "
66 <<
"\nEnforcing orthonormality and positive determinant!\n");
79 BEXCEPTION(
"Can not initialize a zero rotation matrix!");
89 BEXCEPTION(
"Wrong rotation matrix size " << mat.
num_rows() <<
"x"
92 if (!
Check(R_CONSTRAINT_ACCURACY)) {
93 #ifdef R_CONSTRAINTS_WARNING
94 BIASWARN(
"Rotation matrix is invalid: "
96 <<
"\nEnforcing orthonormality and positive determinant!\n");
107 if (!
Check(R_CONSTRAINT_ACCURACY)) {
108 #ifdef R_CONSTRAINTS_WARNING
109 BIASWARN(
"Rotation matrix is invalid: "
111 <<
"\nEnforcing orthonormality and positive determinant!\n");
131 SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
132 ROTATION_MATRIX_TYPE PhiZ)
138 Rx[1][1] = cos(PhiX);
139 Rx[1][2] = -sin(PhiX);
140 Rx[2][1] = -Rx[1][2];
143 Ry[0][0] = cos(PhiY);
144 Ry[0][2] = sin(PhiY);
145 Ry[2][0] = -Ry[0][2];
148 Rz[0][0] = cos(PhiZ);
149 Rz[0][1] = -sin(PhiZ);
150 Rz[1][0] = -Rz[0][1];
158 SetZYX(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
159 ROTATION_MATRIX_TYPE PhiZ)
165 Rx[1][1] = cos(PhiX);
166 Rx[1][2] = -sin(PhiX);
167 Rx[2][1] = -Rx[1][2];
170 Ry[0][0] = cos(PhiY);
171 Ry[0][2] = sin(PhiY);
172 Ry[2][0] = -Ry[0][2];
175 Rz[0][0] = cos(PhiZ);
176 Rz[0][1] = -sin(PhiZ);
177 Rz[1][0] = -Rz[0][1];
185 SetYXZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
186 ROTATION_MATRIX_TYPE PhiZ)
192 Rx[1][1] = cos(PhiX);
193 Rx[1][2] = -sin(PhiX);
194 Rx[2][1] = -Rx[1][2];
197 Ry[0][0] = cos(PhiY);
198 Ry[0][2] = sin(PhiY);
199 Ry[2][0] = -Ry[0][2];
202 Rz[0][0] = cos(PhiZ);
203 Rz[0][1] = -sin(PhiZ);
204 Rz[1][0] = -Rz[0][1];
212 SetZXY(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
213 ROTATION_MATRIX_TYPE PhiZ)
219 Rx[1][1] = cos(PhiX);
220 Rx[1][2] = -sin(PhiX);
221 Rx[2][1] = -Rx[1][2];
224 Ry[0][0] = cos(PhiY);
225 Ry[0][2] = sin(PhiY);
226 Ry[2][0] = -Ry[0][2];
229 Rz[0][0] = cos(PhiZ);
230 Rz[0][1] = -sin(PhiZ);
231 Rz[1][0] = -Rz[0][1];
238 SetYZX(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
239 ROTATION_MATRIX_TYPE PhiZ)
245 Rx[1][1] = cos(PhiX);
246 Rx[1][2] = -sin(PhiX);
247 Rx[2][1] = -Rx[1][2];
250 Ry[0][0] = cos(PhiY);
251 Ry[0][2] = sin(PhiY);
252 Ry[2][0] = -Ry[0][2];
255 Rz[0][0] = cos(PhiZ);
256 Rz[0][1] = -sin(PhiZ);
257 Rz[1][0] = -Rz[0][1];
265 SetXZY(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
266 ROTATION_MATRIX_TYPE PhiZ)
272 Rx[1][1] = cos(PhiX);
273 Rx[1][2] = -sin(PhiX);
274 Rx[2][1] = -Rx[1][2];
277 Ry[0][0] = cos(PhiY);
278 Ry[0][2] = sin(PhiY);
279 Ry[2][0] = -Ry[0][2];
282 Rz[0][0] = cos(PhiZ);
283 Rz[0][1] = -sin(PhiZ);
284 Rz[1][0] = -Rz[0][1];
294 if (
Equal(fabs(phi), 0.0)){
298 double wnorm = wa.
NormL2();
300 BEXCEPTION(
"Vector "<<wa<<
" is close to zero (norm = "<<wnorm
301 <<
"), solution is instable!");
316 Omega[2][0] = -w[1] ;
322 Sin_O = Omega * sin(phi);
324 Cos_O_O = (Omega * Omega) * (1-cos(phi));
334 double dAngle = w.
NormL2();
335 if (fabs(dAngle) < MINIMUM_ROTATION) {
349 if (!
Check(R_CONSTRAINT_ACCURACY)) {
356 BEXCEPTION(
"Rotation matrix is invalid: "
358 <<
"\nCall EnforceConstraints() to enforce orthonormality "
359 <<
"and positive determinant before quaternion conversion!\n");
368 double tr = (*this)[0][0] + (*this)[1][1] + (*this)[2][2];
373 double s = sqrt(tr + 1.0);
378 quat[0] = ((*this)[2][1] - (*this)[1][2]) / s;
379 quat[1] = ((*this)[0][2] - (*this)[2][0]) / s;
380 quat[2] = ((*this)[1][0] - (*this)[0][1]) / s;
392 if((*
this)[1][1] > (*this)[0][0])
396 if((*
this)[2][2] > (*this)[i][i])
403 s = sqrt((*
this)[i][i] - ((*
this)[j][j] + (*
this)[k][k]) + 1.0);
409 quat[j] = ((*this)[i][j] + (*this)[j][i]) / s;
410 quat[k] = ((*this)[i][k] + (*this)[k][i]) / s;
412 quat[3] = ((*this)[k][j] - (*this)[j][k] ) / s;
515 ROTATION_MATRIX_TYPE& Angle)
const
529 ROTATION_MATRIX_TYPE Angle;
531 #ifdef BIASASSERT_ISACTIVE
534 #else //avoid warning in release builds
546 ROTATION_MATRIX_TYPE Angle;
548 #ifdef BIASASSERT_ISACTIVE
551 #else //avoid warning in release builds
577 if (fabs((*
this)[0][2]) == 1.0) {
578 BIASWARN(
"Rotation for Y-axis is +-PI/2, can not decompose X-/Z-component!");
581 PhiY = asin((*
this)[0][2]);
582 if (BIAS_ISNAN(PhiY) || BIAS_ISINF(PhiY))
return -2;
583 PhiX = atan2(-(*
this)[1][2],(*
this)[2][2]);
584 if (BIAS_ISNAN(PhiX) || BIAS_ISINF(PhiX))
return -1;
585 PhiZ = atan2(-(*
this)[0][1],(*
this)[0][0]);
586 if (BIAS_ISNAN(PhiZ) || BIAS_ISINF(PhiZ))
return -3;
604 if (fabs((*
this)[2][0]) == 1.0) {
605 BIASWARN(
"Rotation for Y-axis is +-PI/2, can not decompose X-/Z-component!");
608 PhiY = asin(-(*
this)[2][0]);
609 if (BIAS_ISNAN(PhiY) || BIAS_ISINF(PhiY))
return -2;
610 PhiX = atan2((*
this)[2][1],(*
this)[2][2]);
611 if (BIAS_ISNAN(PhiX) || BIAS_ISINF(PhiX))
return -1;
612 PhiZ = atan2((*
this)[1][0],(*
this)[0][0]);
613 if (BIAS_ISNAN(PhiZ) || BIAS_ISINF(PhiZ))
return -3;
631 if (fabs((*
this)[2][1]) == 1.0) {
632 BIASWARN(
"Rotation for X-axis is +-PI/2, can not decompose Y-/Z-component!");
635 PhiX = asin((*
this)[2][1]);
636 if (BIAS_ISNAN(PhiX) || BIAS_ISINF(PhiX))
return -1;
637 PhiY = atan2(-(*
this)[2][0],(*
this)[2][2]);
638 if (BIAS_ISNAN(PhiY) || BIAS_ISINF(PhiY))
return -2;
639 PhiZ = atan2(-(*
this)[0][1],(*
this)[1][1]);
640 if (BIAS_ISNAN(PhiZ) || BIAS_ISINF(PhiZ))
return -3;
658 if (fabs((*
this)[1][2]) == 1.0) {
659 BIASWARN(
"Rotation for X-axis is +-PI/2, can not decompose Y-/Z-component!");
662 PhiX = asin(-(*
this)[1][2]);
663 if (BIAS_ISNAN(PhiX) || BIAS_ISINF(PhiX))
return -1;
664 PhiY = atan2((*
this)[0][2],(*
this)[2][2]);
665 if (BIAS_ISNAN(PhiY) || BIAS_ISINF(PhiY))
return -2;
666 PhiZ = atan2((*
this)[1][0],(*
this)[1][1]);
667 if (BIAS_ISNAN(PhiZ) || BIAS_ISINF(PhiZ))
return -3;
685 if (fabs((*
this)[0][1]) == 1.0) {
686 BIASWARN(
"Rotation for Z-axis is +-PI/2, can not decompose X-/Y-component!");
689 PhiZ = asin(-(*
this)[0][1]);
690 if (BIAS_ISNAN(PhiZ) || BIAS_ISINF(PhiZ))
return -3;
691 PhiX = atan2((*
this)[2][1],(*
this)[1][1]);
692 if (BIAS_ISNAN(PhiX) || BIAS_ISINF(PhiX))
return -1;
693 PhiY = atan2((*
this)[0][2],(*
this)[0][0]);
694 if (BIAS_ISNAN(PhiY) || BIAS_ISINF(PhiY))
return -2;
712 if (fabs((*
this)[1][0]) == 1.0) {
713 BIASWARN(
"Rotation for Z-axis is +-PI/2, can not decompose X-/Y-component!");
716 PhiZ = asin((*
this)[1][0]);
717 if (BIAS_ISNAN(PhiZ) || BIAS_ISINF(PhiZ))
return -3;
718 PhiX = atan2(-(*
this)[1][2],(*
this)[1][1]);
719 if (BIAS_ISNAN(PhiX) || BIAS_ISINF(PhiX))
return -1;
720 PhiY = atan2(-(*
this)[2][0],(*
this)[0][0]);
721 if (BIAS_ISNAN(PhiY) || BIAS_ISINF(PhiY))
return -2;
739 for (
register int i=0; i<9; i++){
741 numeric_limits<double>::epsilon()){
754 const double norm = 1.0/qu.
NormL2();
755 const double q0 = qu[0]*norm;
756 const double q1 = qu[1]*norm;
757 const double q2 = qu[2]*norm;
758 const double q3 = qu[3]*norm;
762 *pData++ = 1.0 - 2.0 * (q1 * q1 + q2 * q2);
763 *pData++ = 2.0 * (q0 * q1 - q2 * q3);
764 *pData++ = 2.0 * (q2 * q0 + q1 * q3);
766 *pData++ = 2.0 * (q0 * q1 + q2 * q3);
767 *pData++ = 1.0 - 2.0 * (q2 * q2 + q0 * q0);
768 *pData++ = 2.0 * (q1 * q2 - q0 * q3);
770 *pData++ = 2.0 * (q2 * q0 - q1 * q3);
771 *pData++ = 2.0 * (q1 * q2 + q0 * q3);
772 *pData++ = 1.0 - 2.0 * (q1 * q1 + q0 * q0);
775 #ifdef WANT_TO_COMPARE_WITH_OLD_CODE
782 RCompare[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
783 RCompare[1][0] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
784 RCompare[2][0] = 2.0 * (q[2] * q[0] - q[1] * q[3]);
786 RCompare[0][1] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
787 RCompare[1][1] = 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]);
788 RCompare[2][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
790 RCompare[0][2] = 2.0 * (q[2] * q[0] + q[1] * q[3]);
791 RCompare[1][2] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
792 RCompare[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]);
794 for (
unsigned int i=0; i<3; i++) {
795 for (
unsigned int j=0; j<3; j++) {
796 if (!
Equal((*
this)[i][j], RCompare[i][j], 1e-12)) {
797 cout<<
"Matrices are not same !"<<*
this<<
" "<<RCompare<<endl;
798 cout<<
" at "<<i<<
" "<<j<<
" "<< ((*this)[i][j])-(RCompare[i][j])<<endl;
817 if (!
Check(R_CONSTRAINT_ACCURACY)) {
818 #ifdef R_CONSTRAINTS_WARNING
819 BIASWARN(
"Rotation matrix is invalid: "
821 <<
"\nEnforcing orthonormality and positive determinant!\n");
841 y0(z0.CrossProduct(x0).GetNormalized());
843 (*this).SetFromColumnVectors(x0, y0, z0);
844 if (!
Check(R_CONSTRAINT_ACCURACY)) {
845 #ifdef R_CONSTRAINTS_WARNING
846 BIASWARN(
"Rotation matrix is invalid: "
848 <<
"\nEnforcing orthonormality and positive determinant!\n");
891 Check(
const double eps,
const bool verbose)
const
898 for (
register unsigned int i = 0; i < 3 && ok; i++) {
905 ok =
Equal(c[0].ScalarProduct(c[1]), 0.0, eps);
907 ok =
Equal(c[0].ScalarProduct(c[2]), 0.0, eps);
909 ok =
Equal(c[1].ScalarProduct(c[2]), 0.0, eps);
911 if (!ok && verbose) {
921 ostringstream res(
" ");
924 if (!
Equal(det, 1.0, eps)) {
925 res <<
"det != 1 (det=" << setprecision(30) << det <<
") ";
928 for (
register unsigned int i = 0; i < 3; i++) {
931 res <<
"col[" << i <<
"].NormL2() != 1) (= " << c[i].
NormL2() <<
") ";
934 if (!
Equal(c[0].ScalarProduct(c[1]), 0.0, eps)){
935 res <<
"col[0].ScalarProduct(c[1]) != 0 (= "
938 if (!
Equal(c[0].ScalarProduct(c[2]), 0.0, eps)){
939 res <<
"col[0].ScalarProduct(c[2]) != 0 (= "
942 if (!
Equal(c[1].ScalarProduct(c[2]), 0.0, eps)){
943 res <<
"col[1].ScalarProduct(c[2]) != 0 (= "
958 for (
register unsigned int i = 0; i < 3; i++) {
961 BEXCEPTION(
"Rotation matrix has not full rank. Can not enforce "
962 "unit length of zero column vector!");
977 for (
register unsigned int i = 0; i < 3; i++) {
int GetRotationAnglesZYX(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order ZYX.
void SetFromOrthogonalBasis(const BIAS::Vector3< ROTATION_MATRIX_TYPE > &ex, const BIAS::Vector3< ROTATION_MATRIX_TYPE > &ey, const BIAS::Vector3< ROTATION_MATRIX_TYPE > &ez)
MatrixInitType
can be passed to matrix constructors to init the matrix with the most often used values ...
void SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order XYZ.
Subscript num_cols() const
void SetColumn(const unsigned int col, const Vector3< ROTATION_MATRIX_TYPE > &c)
void SetXZY(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order XZY.
void SetZYX(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order ZYX.
int GetRotationAnglesYZX(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order YZX.
void ScalarProduct(const Vector3< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
BIAS::Vector3< ROTATION_MATRIX_TYPE > GetRotationAxis() const
Interface for axis component of GetRotationAxisAngle()
BIAS::Vector3< T > GetNormalized() const
return a normalized vector of this
void GetColumn(const unsigned int col, Vector3< ROTATION_MATRIX_TYPE > &r) const
extract one column ('Spalte') from this matrix (for convenience)
int SetFromQuaternion(const Quaternion< ROTATION_MATRIX_TYPE > &q)
Set rotation matrix from a quaternion.
bool Check(const double eps=std::numeric_limits< double >::epsilon(), const bool verbose=false) const
Check if this is a rotation matrix, i.e.
void SetYXZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order YXZ.
void SetZXY(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order ZXY.
int GetAxisAngle(Vector3< QUAT_TYPE > &axis, QUAT_TYPE &angle) const
Returns rotation in axis and angle notation (angle in radians).
double NormL2() const
Return the L2 norm: sqrt(a^2 + b^2 + c^2 + d^2)
int GetQuaternion(Quaternion< ROTATION_MATRIX_TYPE > &quat) const
Calculates quaternion representation for this rotation matrix.
void Normalize()
Scales quaternion to unit length, i.e.
virtual void EnforceConstraints()
Enforce orthonormality constraint on rotation matrix and sets determinant to +1.
int GetRotationAnglesXZY(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order XZY.
std::string GetCheckFailureReason(const double eps=std::numeric_limits< double >::epsilon()) const
Return the reason for the constraint check failure in a human readable string.
int SetFromOriUp(BIAS::Vector3< ROTATION_MATRIX_TYPE > ori, BIAS::Vector3< ROTATION_MATRIX_TYPE > up)
Set rotation matrix from orientation and up vector.
void Set(const Vector3< ROTATION_MATRIX_TYPE > &w, const ROTATION_MATRIX_TYPE phi)
Set from rotation axis w and angle phi (in rad)
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
int SetFromOriUpGL(BIAS::Vector3< ROTATION_MATRIX_TYPE > ori, BIAS::Vector3< ROTATION_MATRIX_TYPE > up)
Set rotation matrix from orientation and up vector.
int GetRotationAnglesZXY(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order ZXY.
void SetFromRowVectors(const BIAS::Vector3< ROTATION_MATRIX_TYPE > &v0, const BIAS::Vector3< ROTATION_MATRIX_TYPE > &v1, const BIAS::Vector3< ROTATION_MATRIX_TYPE > &v2)
set this matrix from 3 vectors, each representating a row
RMatrixBase()
Constructor setting matrix to identity.
is a 'fixed size' quadratic matrix of dim.
class Vector3 contains a Vector of fixed dim.
void SetFromAxisAngle(Vector3< ROTATION_MATRIX_TYPE > w)
Set from rotation axis * angle (modified Rodrigues vector)
bool Equal(const T left, const T right, const T eps)
comparison function for floating point values See http://www.boost.org/libs/test/doc/components/test_...
void SetFromHV(const BIAS::Vector3< ROTATION_MATRIX_TYPE > &xh, const BIAS::Vector3< ROTATION_MATRIX_TYPE > &vy)
Set rotation matrix from two vectors from an orthonormal basis.
void MultiplyIP(const QUAT_TYPE &scalar)
Multiplication (in place) of an scalar.
int GetRotationAnglesXYZ(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order XYZ.
ROTATION_MATRIX_TYPE GetDeterminant() const
returns the Determinant |A| of this
Implements a 3D rotation matrix.
Subscript num_rows() const
int GetRotationAxisAngle(Vector3< ROTATION_MATRIX_TYPE > &axis, ROTATION_MATRIX_TYPE &angle) const
Calculates angle and rotation axis representation for this rotation matrix.
int GetRotationAnglesYXZ(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order YXZ.
void SetFromColumnVectors(const BIAS::Vector3< ROTATION_MATRIX_TYPE > &v0, const BIAS::Vector3< ROTATION_MATRIX_TYPE > &v1, const BIAS::Vector3< ROTATION_MATRIX_TYPE > &v2)
set this matrix from 3 vectors each representating a column
class for rotation with axis and angle
ROTATION_MATRIX_TYPE GetRotationAngle() const
Interface for angle component of GetRotationAxisAngle()
Vector3< T > & Normalize()
normalize this vector to length 1
void SetYZX(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order YZX.
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
void SetIdentity()
set the elements of this matrix to the identity matrix (possibly overriding the inherited method) ...