Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
RMatrixBase.cpp
1 /* This file is part of the BIAS library (Basic ImageAlgorithmS).
2 
3  Copyright (C) 2003-2009 (see file CONTACT for details)
4  Multimediale Systeme der Informationsverarbeitung
5  Institut fuer Informatik
6  Christian-Albrechts-Universitaet Kiel
7 
8  BIAS is free software; you can redistribute it and/or modify
9  it under the terms of the GNU Lesser General Public License as published by
10  the Free Software Foundation; either version 2.1 of the License, or
11  (at your option) any later version.
12 
13  BIAS is distributed in the hope that it will be useful,
14  but WITHOUT ANY WARRANTY; without even the implied warranty of
15  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  GNU Lesser General Public License for more details.
17 
18  You should have received a copy of the GNU Lesser General Public License
19  along with BIAS; if not, write to the Free Software
20  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */
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>
26 
27 
28 #ifdef WIN32
29 # define _USE_MATH_DEFINES
30 #endif //WIN32
31 
32 #include <math.h>
33 
34 // accuracy for constraint check in Check() function calls
35 #define R_CONSTRAINT_ACCURACY 1e-10
36 
37 // if rotation angle is below this value, assume zero rotation
38 #define MINIMUM_ROTATION 1e-6
39 
40 // if real part of quaternion = cos(alpha) is larger than this + 1,
41 // GetQuaternion() will produce an error and return -1
42 #define MAX_QUATERNION_COS_ERROR 1e-12
43 
44 // show warnings when orthonormality constraints are enforced
45 //#define R_CONSTRAINTS_WARNING
46 
47 #include <Base/Common/BIASpragma.hh>
48 
49 using namespace BIAS;
50 using namespace std;
51 
52 RMatrixBase::
53 RMatrixBase()
54  : Matrix3x3<ROTATION_MATRIX_TYPE>(MatrixIdentity)
55 {}
56 
57 
58 RMatrixBase::
59 RMatrixBase(const RMatrixBase& r)
60  : Matrix3x3<ROTATION_MATRIX_TYPE>(r)
61 {
62  if (!Check(R_CONSTRAINT_ACCURACY)) {
63 #ifdef R_CONSTRAINTS_WARNING
64  BIASWARN("Rotation matrix is invalid: "
65  << GetCheckFailureReason(R_CONSTRAINT_ACCURACY)
66  << "\nEnforcing orthonormality and positive determinant!\n");
67 #endif
68  EnforceConstraints();
69  }
70 }
71 
72 
73 RMatrixBase::
74 RMatrixBase(const MatrixInitType& i) :
75  Matrix3x3<ROTATION_MATRIX_TYPE>(i)
76 {
77  if (i != MatrixIdentity) {
78  //SetIdentity();
79  BEXCEPTION("Can not initialize a zero rotation matrix!");
80  }
81 }
82 
83 
84 RMatrixBase::
85 RMatrixBase(const TNT::Matrix<ROTATION_MATRIX_TYPE>& mat)
86  : Matrix3x3<ROTATION_MATRIX_TYPE>(mat)
87 {
88  if ((mat.num_rows() != 3) || (mat.num_cols() != 3)) {
89  BEXCEPTION("Wrong rotation matrix size " << mat.num_rows() << "x"
90  << mat.num_cols());
91  }
92  if (!Check(R_CONSTRAINT_ACCURACY)) {
93 #ifdef R_CONSTRAINTS_WARNING
94  BIASWARN("Rotation matrix is invalid: "
95  << GetCheckFailureReason(R_CONSTRAINT_ACCURACY)
96  << "\nEnforcing orthonormality and positive determinant!\n");
97 #endif
98  EnforceConstraints();
99  }
100 }
101 
102 
103 RMatrixBase::
104 RMatrixBase(const Matrix3x3<ROTATION_MATRIX_TYPE>& mat)
105  : Matrix3x3<ROTATION_MATRIX_TYPE>(mat)
106 {
107  if (!Check(R_CONSTRAINT_ACCURACY)) {
108 #ifdef R_CONSTRAINTS_WARNING
109  BIASWARN("Rotation matrix is invalid: "
110  << GetCheckFailureReason(R_CONSTRAINT_ACCURACY)
111  << "\nEnforcing orthonormality and positive determinant!\n");
112 #endif
113  EnforceConstraints();
114  }
115 }
116 
117 
118 RMatrixBase::
119 RMatrixBase(Vector3<ROTATION_MATRIX_TYPE> w, ROTATION_MATRIX_TYPE phi)
120 {
121  Set(w, phi);
122 }
123 
124 
125 RMatrixBase::
126 ~RMatrixBase()
127 {}
128 
129 
130 void RMatrixBase::
131 SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
132  ROTATION_MATRIX_TYPE PhiZ)
133 {
134  RMatrixBase Rx(MatrixIdentity);
135  RMatrixBase Ry(MatrixIdentity);
136  RMatrixBase Rz(MatrixIdentity);
137 
138  Rx[1][1] = cos(PhiX);
139  Rx[1][2] = -sin(PhiX);
140  Rx[2][1] = -Rx[1][2]; // = sin(PhiX);
141  Rx[2][2] = Rx[1][1]; // = cos(PhiX);
142 
143  Ry[0][0] = cos(PhiY);
144  Ry[0][2] = sin(PhiY);
145  Ry[2][0] = -Ry[0][2]; // = -sin(PhiY);
146  Ry[2][2] = Ry[0][0]; // = cos(PhiY);
147 
148  Rz[0][0] = cos(PhiZ);
149  Rz[0][1] = -sin(PhiZ);
150  Rz[1][0] = -Rz[0][1]; // = sin(PhiZ);
151  Rz[1][1] = Rz[0][0]; // = cos(PhiZ);
152 
153  (*this) = Rx*Ry*Rz;
154 }
155 
156 
157 void RMatrixBase::
158 SetZYX(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
159  ROTATION_MATRIX_TYPE PhiZ)
160 {
161  RMatrixBase Rx(MatrixIdentity);
162  RMatrixBase Ry(MatrixIdentity);
163  RMatrixBase Rz(MatrixIdentity);
164 
165  Rx[1][1] = cos(PhiX);
166  Rx[1][2] = -sin(PhiX);
167  Rx[2][1] = -Rx[1][2]; // = sin(PhiX);
168  Rx[2][2] = Rx[1][1]; // = cos(PhiX);
169 
170  Ry[0][0] = cos(PhiY);
171  Ry[0][2] = sin(PhiY);
172  Ry[2][0] = -Ry[0][2]; // = -sin(PhiY);
173  Ry[2][2] = Ry[0][0]; // = cos(PhiY);
174 
175  Rz[0][0] = cos(PhiZ);
176  Rz[0][1] = -sin(PhiZ);
177  Rz[1][0] = -Rz[0][1]; // = sin(PhiZ);
178  Rz[1][1] = Rz[0][0]; // = cos(PhiZ);
179 
180  (*this) = Rz*Ry*Rx;
181 }
182 
183 
184 void RMatrixBase::
185 SetYXZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
186  ROTATION_MATRIX_TYPE PhiZ)
187 {
188  RMatrixBase Rx(MatrixIdentity);
189  RMatrixBase Ry(MatrixIdentity);
190  RMatrixBase Rz(MatrixIdentity);
191 
192  Rx[1][1] = cos(PhiX);
193  Rx[1][2] = -sin(PhiX);
194  Rx[2][1] = -Rx[1][2]; // = sin(PhiX);
195  Rx[2][2] = Rx[1][1]; // = cos(PhiX);
196 
197  Ry[0][0] = cos(PhiY);
198  Ry[0][2] = sin(PhiY);
199  Ry[2][0] = -Ry[0][2]; // = -sin(PhiY);
200  Ry[2][2] = Ry[0][0]; // = cos(PhiY);
201 
202  Rz[0][0] = cos(PhiZ);
203  Rz[0][1] = -sin(PhiZ);
204  Rz[1][0] = -Rz[0][1]; // = sin(PhiZ);
205  Rz[1][1] = Rz[0][0]; // = cos(PhiZ);
206 
207  (*this) = Ry*Rx*Rz;
208 }
209 
210 
211 void RMatrixBase::
212 SetZXY(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
213  ROTATION_MATRIX_TYPE PhiZ)
214 {
215  RMatrixBase Rx(MatrixIdentity);
216  RMatrixBase Ry(MatrixIdentity);
217  RMatrixBase Rz(MatrixIdentity);
218 
219  Rx[1][1] = cos(PhiX);
220  Rx[1][2] = -sin(PhiX);
221  Rx[2][1] = -Rx[1][2]; // = sin(PhiX);
222  Rx[2][2] = Rx[1][1]; // = cos(PhiX);
223 
224  Ry[0][0] = cos(PhiY);
225  Ry[0][2] = sin(PhiY);
226  Ry[2][0] = -Ry[0][2]; // = -sin(PhiY);
227  Ry[2][2] = Ry[0][0]; // = cos(PhiY);
228 
229  Rz[0][0] = cos(PhiZ);
230  Rz[0][1] = -sin(PhiZ);
231  Rz[1][0] = -Rz[0][1]; // = sin(PhiZ);
232  Rz[1][1] = Rz[0][0]; // = cos(PhiZ);
233 
234  (*this) = Rz*Rx*Ry;
235 }
236 
237 void RMatrixBase::
238 SetYZX(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
239  ROTATION_MATRIX_TYPE PhiZ)
240 {
241  RMatrixBase Rx(MatrixIdentity);
242  RMatrixBase Ry(MatrixIdentity);
243  RMatrixBase Rz(MatrixIdentity);
244 
245  Rx[1][1] = cos(PhiX);
246  Rx[1][2] = -sin(PhiX);
247  Rx[2][1] = -Rx[1][2]; // = sin(PhiX);
248  Rx[2][2] = Rx[1][1]; // = cos(PhiX);
249 
250  Ry[0][0] = cos(PhiY);
251  Ry[0][2] = sin(PhiY);
252  Ry[2][0] = -Ry[0][2]; // = -sin(PhiY);
253  Ry[2][2] = Ry[0][0]; // = cos(PhiY);
254 
255  Rz[0][0] = cos(PhiZ);
256  Rz[0][1] = -sin(PhiZ);
257  Rz[1][0] = -Rz[0][1]; // = sin(PhiZ);
258  Rz[1][1] = Rz[0][0]; // = cos(PhiZ);
259 
260  (*this) = Ry*Rz*Rx;
261 }
262 
263 
264 void RMatrixBase::
265 SetXZY(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY,
266  ROTATION_MATRIX_TYPE PhiZ)
267 {
268  RMatrixBase Rx(MatrixIdentity);
269  RMatrixBase Ry(MatrixIdentity);
270  RMatrixBase Rz(MatrixIdentity);
271 
272  Rx[1][1] = cos(PhiX);
273  Rx[1][2] = -sin(PhiX);
274  Rx[2][1] = -Rx[1][2]; // = sin(PhiX);
275  Rx[2][2] = Rx[1][1]; // = cos(PhiX);
276 
277  Ry[0][0] = cos(PhiY);
278  Ry[0][2] = sin(PhiY);
279  Ry[2][0] = -Ry[0][2]; // = -sin(PhiY);
280  Ry[2][2] = Ry[0][0]; // = cos(PhiY);
281 
282  Rz[0][0] = cos(PhiZ);
283  Rz[0][1] = -sin(PhiZ);
284  Rz[1][0] = -Rz[0][1]; // = sin(PhiZ);
285  Rz[1][1] = Rz[0][0]; // = cos(PhiZ);
286 
287  (*this) = Rx*Rz*Ry;
288 }
289 
290 void RMatrixBase::
291 Set(const Vector3<ROTATION_MATRIX_TYPE> &wa, const ROTATION_MATRIX_TYPE phi)
292 {
293  // zero rotation results in identity matrix
294  if (Equal(fabs(phi), 0.0)){
295  SetIdentity();
296  return;
297  }
298  double wnorm = wa.NormL2();
299  if (wnorm<1e-6){
300  BEXCEPTION("Vector "<<wa<<" is close to zero (norm = "<<wnorm
301  <<"), solution is instable!");
302  }
303 
304  Vector3<ROTATION_MATRIX_TYPE> w(wa);
305  w /= wnorm;
306 
307  Matrix3x3<ROTATION_MATRIX_TYPE> Omega, I, A;
308  I.SetIdentity();
309 
310  Omega[0][0] = 0.0;
311  Omega[0][1] = -w[2];
312  Omega[0][2] = w[1];
313  Omega[1][0] = w[2];
314  Omega[1][1] = 0.0;
315  Omega[1][2] = -w[0];
316  Omega[2][0] = -w[1] ;
317  Omega[2][1] = w[0];
318  Omega[2][2] = 0.0;
319 
320  Matrix3x3<ROTATION_MATRIX_TYPE> Sin_O, O_O, Cos_O_O;
321 
322  Sin_O = Omega * sin(phi);
323 
324  Cos_O_O = (Omega * Omega) * (1-cos(phi));
325 
326  A = Matrix3x3<ROTATION_MATRIX_TYPE>(I + Sin_O + Cos_O_O);
327  (*this) = RMatrixBase(I + Sin_O + Cos_O_O);
328 }
329 
330 
331 void RMatrixBase::
332 SetFromAxisAngle(Vector3<ROTATION_MATRIX_TYPE> w)
333 {
334  double dAngle = w.NormL2();
335  if (fabs(dAngle) < MINIMUM_ROTATION) {
336  SetIdentity();
337  } else {
338  w.Normalize();
339  Set(w, dAngle);
340  }
341 }
342 
343 
344 int RMatrixBase::
345 GetQuaternion(Quaternion<ROTATION_MATRIX_TYPE>& quat) const
346 {
347 #ifdef BIAS_DEBUG
348  // This function only works for true rotation matrices!
349  if (!Check(R_CONSTRAINT_ACCURACY)) {
350 //#ifdef R_CONSTRAINTS_WARNING
351  //BIASWARN("Rotation matrix is invalid: "
352  // << GetCheckFailureReason(R_CONSTRAINT_ACCURACY)
353  // << "\nCall EnforceConstraints() to enforce orthonormality "
354  // << "and positive determinant before quaternion conversion!\n");
355 //#endif
356  BEXCEPTION("Rotation matrix is invalid: "
357  << GetCheckFailureReason(R_CONSTRAINT_ACCURACY)
358  << "\nCall EnforceConstraints() to enforce orthonormality "
359  << "and positive determinant before quaternion conversion!\n");
360  return -1;
361  }
362 #endif
363 
364  // Conversion method from OpenSG
365  // @attention Removed old conversion in Linux build which had numerical
366  // issues for rotations close to 180 degree! (esquivel 03/2011)
367 
368  double tr = (*this)[0][0] + (*this)[1][1] + (*this)[2][2];
369 
370  if (tr > 0.0)
371  {
372  // Use default formula if trace is large enough
373  double s = sqrt(tr + 1.0);
374 
375  quat[3] = s * 0.5;
376 
377  s *= 2.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;
381 
382  }
383  else
384  {
385  // Use alternate formula using largest trace element
386  double s;
387  unsigned int i;
388  unsigned int j;
389  unsigned int k;
390 
391  // Find largest trace element i and its successors j, k
392  if((*this)[1][1] > (*this)[0][0])
393  i = 1;
394  else
395  i = 0;
396  if((*this)[2][2] > (*this)[i][i])
397  i = 2;
398 
399  j = (i + 1) % 3;
400  k = (j + 1) % 3;
401 
402  // Compute discriminator using largest trace element
403  s = sqrt((*this)[i][i] - ((*this)[j][j] + (*this)[k][k]) + 1.0);
404 
405  // Compute quaternion entries
406  quat[i] = s * 0.5;
407  s *= 2.0;
408 
409  quat[j] = ((*this)[i][j] + (*this)[j][i]) / s;
410  quat[k] = ((*this)[i][k] + (*this)[k][i]) / s;
411 
412  quat[3] = ((*this)[k][j] - (*this)[j][k] ) / s;
413  }
414 
415 /*
416  // Conversion method using derivation depending on max. denominator
417  // @see http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rotation_matrix_.E2.86.94_quaternion
418  // @deprecated
419 
420  // Determine max. denominator
421  double d[4] = { 1 + (*this)[0][0] - (*this)[1][1] - (*this)[2][2],
422  1 - (*this)[0][0] + (*this)[1][1] - (*this)[2][2],
423  1 - (*this)[0][0] - (*this)[1][1] + (*this)[2][2],
424  1 + (*this)[0][0] + (*this)[1][1] + (*this)[2][2] };
425  int maxidx = 0;
426  for (int i = 1; i < 4; i++) {
427  if (d[maxidx] < d[i]) maxidx = i;
428  }
429 
430  // Compute quaternion starting with max. denominator
431  if (maxidx == 0) {
432  quat[0]=0.5*sqrt(1 + (*this)[0][0] - (*this)[1][1] - (*this)[2][2]);
433  quat[3]=0.25*((*this)[2][1]-(*this)[1][2])/quat[0];
434  quat[1]=0.25*((*this)[0][1]+(*this)[1][0])/quat[0];
435  quat[2]=0.25*((*this)[0][2]+(*this)[2][0])/quat[0];
436  } else if (maxidx == 1) {
437  quat[1]=0.5*sqrt(1 - (*this)[0][0] + (*this)[1][1] - (*this)[2][2]);
438  quat[3]=0.25*((*this)[0][2]-(*this)[2][0])/quat[1];
439  quat[2]=0.25*((*this)[1][2]+(*this)[2][1])/quat[1];
440  quat[0]=0.25*((*this)[1][0]+(*this)[0][1])/quat[1];
441  } else if (maxidx == 2) {
442  quat[2]=0.5*sqrt(1 - (*this)[0][0] - (*this)[1][1] + (*this)[2][2]);
443  quat[3]=0.25*((*this)[1][0]-(*this)[0][1])/quat[2];
444  quat[0]=0.25*((*this)[2][0]+(*this)[0][2])/quat[2];
445  quat[1]=0.25*((*this)[2][1]+(*this)[1][2])/quat[2];
446  } else {
447  quat[3]=0.5*sqrt(1 + (*this)[0][0] + (*this)[1][1] + (*this)[2][2]);
448  quat[0]=0.25*((*this)[2][1]-(*this)[1][2])/quat[3];
449  quat[1]=0.25*((*this)[0][2]-(*this)[2][0])/quat[3];
450  quat[2]=0.25*((*this)[1][0]-(*this)[0][1])/quat[3];
451  }
452 */
453 
454 /*
455  // More compact implementation of the conversion method from OpenSG
456  // @deprecated
457 
458  double trace = (*this)[0][0] + (*this)[1][1] + (*this)[2][2];
459  if ( trace > 0 ) {
460  double s = 2.0 * sqrt(trace + 1.0);
461  quat[3] = 0.25 * s;
462  quat[0] = ( (*this)[2][1] - (*this)[1][2] ) / s;
463  quat[1] = ( (*this)[0][2] - (*this)[2][0] ) / s;
464  quat[2] = ( (*this)[1][0] - (*this)[0][1] ) / s;
465  } else {
466  if ( (*this)[0][0] > (*this)[1][1] && (*this)[0][0] > (*this)[2][2] ) {
467  double s = 2.0 * sqrt( 1.0 + (*this)[0][0] - (*this)[1][1] - (*this)[2][2]);
468  quat[3] = ((*this)[2][1] - (*this)[1][2] ) / s;
469  quat[0] = 0.25 * s;
470  quat[1] = ((*this)[0][1] + (*this)[1][0] ) / s;
471  quat[2] = ((*this)[0][2] + (*this)[2][0] ) / s;
472  } else if ((*this)[1][1] > (*this)[2][2]) {
473  double s = 2.0 * sqrt( 1.0 + (*this)[1][1] - (*this)[0][0] - (*this)[2][2]);
474  quat[3] = ((*this)[0][2] - (*this)[2][0] ) / s;
475  quat[0] = ((*this)[0][1] + (*this)[1][0] ) / s;
476  quat[1] = 0.25 * s;
477  quat[2] = ((*this)[1][2] + (*this)[2][1] ) / s;
478  } else {
479  double s = 2.0 * sqrt( 1.0 + (*this)[2][2] - (*this)[0][0] - (*this)[1][1] );
480  quat[3] = ((*this)[1][0] - (*this)[0][1] ) / s;
481  quat[0] = ((*this)[0][2] + (*this)[2][0] ) / s;
482  quat[1] = ((*this)[1][2] + (*this)[2][1] ) / s;
483  quat[2] = 0.25 * s;
484  }
485  }
486 */
487 
488 /*
489  // Old conversion method
490  // @see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
491  // @deprecated
492  // @attention Computes wrong quaternion for rotation close to 180 degree!
493 
494  quat[3] = sqrt( max( 0.0, 1.0 + (*this)[0][0] + (*this)[1][1] + (*this)[2][2] ) ) / 2.0;
495  quat[0] = sqrt( max( 0.0, 1.0 + (*this)[0][0] - (*this)[1][1] - (*this)[2][2] ) ) / 2.0;
496  quat[1] = sqrt( max( 0.0, 1.0 - (*this)[0][0] + (*this)[1][1] - (*this)[2][2] ) ) / 2.0;
497  quat[2] = sqrt( max( 0.0, 1.0 - (*this)[0][0] - (*this)[1][1] + (*this)[2][2] ) ) / 2.0;
498  quat[0] = copysign( quat[0], (*this)[2][1] - (*this)[1][2] ) ;
499  quat[1] = copysign( quat[1], (*this)[0][2] - (*this)[2][0] ) ;
500  quat[2] = copysign( quat[2], (*this)[1][0] - (*this)[0][1] ) ;
501 */
502 
503  // Normalize resulting quaternion
504  quat /= quat.NormL2();
505 
506  // Correct quaternion so that scalar part is non-negative
507  if (quat[3] < 0) quat.MultiplyIP(-1.0);
508 
509  return 0;
510 }
511 
512 
513 int RMatrixBase::
514 GetRotationAxisAngle(Vector3<ROTATION_MATRIX_TYPE>& Axis,
515  ROTATION_MATRIX_TYPE& Angle) const
516 {
517  // nicked from OpenSG
518  Quaternion<ROTATION_MATRIX_TYPE> quat;
519  if (GetQuaternion(quat)!=0)
520  return -1;
521  return quat.GetAxisAngle(Axis, Angle);
522 }
523 
524 
525 Vector3<ROTATION_MATRIX_TYPE> RMatrixBase::
526 GetRotationAxis() const
527 {
528  Vector3<ROTATION_MATRIX_TYPE> Axis;
529  ROTATION_MATRIX_TYPE Angle;
530 
531 #ifdef BIASASSERT_ISACTIVE
532  int res = GetRotationAxisAngle(Axis, Angle);
533  BIASASSERT(res==0);
534 #else //avoid warning in release builds
535  GetRotationAxisAngle(Axis, Angle);
536 #endif
537 
538  return Axis;
539 }
540 
541 
542 ROTATION_MATRIX_TYPE RMatrixBase::
543 GetRotationAngle() const
544 {
545  Vector3<ROTATION_MATRIX_TYPE> Axis;
546  ROTATION_MATRIX_TYPE Angle;
547 
548 #ifdef BIASASSERT_ISACTIVE
549  int res = GetRotationAxisAngle(Axis, Angle);
550  BIASASSERT(res==0);
551 #else //avoid warning in release builds
552  GetRotationAxisAngle(Axis, Angle);
553 #endif
554 
555  return Angle;
556 }
557 
558 
559 int RMatrixBase::
560 GetRotationAxisAngle(Vector3<ROTATION_MATRIX_TYPE>& w) const
561 {
562  double angle;
563  // nicked from above
564  Quaternion<ROTATION_MATRIX_TYPE> quat;
565  if (GetQuaternion(quat)!=0) return -1;
566  quat.GetAxisAngle(w , angle);
567  w.Normalize();
568  //get a three-parameter representation by encoding the angle into the axis:
569  w *= angle;
570  return 0;
571 }
572 
573 
574 int RMatrixBase::
575 GetRotationAnglesXYZ(double& PhiX, double& PhiY, double& PhiZ) const
576 {
577  if (fabs((*this)[0][2]) == 1.0) {
578  BIASWARN("Rotation for Y-axis is +-PI/2, can not decompose X-/Z-component!");
579  return -1;
580  }
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;
587 /*
588 #ifdef BIAS_DEBUG
589  RMatrixBase R;
590  R.SetXYZ(PhiX, PhiY, PhiZ);
591  if (R == (*this))
592  res = 0;
593  else
594  res = -1;
595 #endif
596 */
597  return 0;
598 }
599 
600 
601 int RMatrixBase::
602 GetRotationAnglesZYX(double& PhiX, double& PhiY, double& PhiZ) const
603 {
604  if (fabs((*this)[2][0]) == 1.0) {
605  BIASWARN("Rotation for Y-axis is +-PI/2, can not decompose X-/Z-component!");
606  return -1;
607  }
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;
614 /*
615 #ifdef BIAS_DEBUG
616  RMatrixBase R;
617  R.SetZYX(PhiX, PhiY, PhiZ);
618  if (R == (*this))
619  res = 0;
620  else
621  res = -1;
622 #endif
623 */
624  return 0;
625 }
626 
627 
628 int RMatrixBase::
629 GetRotationAnglesZXY(double& PhiX, double& PhiY, double& PhiZ) const
630 {
631  if (fabs((*this)[2][1]) == 1.0) {
632  BIASWARN("Rotation for X-axis is +-PI/2, can not decompose Y-/Z-component!");
633  return -1;
634  }
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;
641 /*
642 #ifdef BIAS_DEBUG
643  RMatrixBase R;
644  R.SetZXY(PhiX, PhiY, PhiZ);
645  if (R == (*this))
646  res = 0;
647  else
648  res = -1;
649 #endif
650 */
651  return 0;
652 }
653 
654 
655 int RMatrixBase::
656 GetRotationAnglesYXZ(double& PhiX, double& PhiY, double& PhiZ) const
657 {
658  if (fabs((*this)[1][2]) == 1.0) {
659  BIASWARN("Rotation for X-axis is +-PI/2, can not decompose Y-/Z-component!");
660  return -1;
661  }
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;
668 /*
669 #ifdef BIAS_DEBUG
670  RMatrixBase R;
671  R.SetYXZ(PhiX, PhiY, PhiZ);
672  if (R == (*this))
673  res = 0;
674  else
675  res = -1;
676 #endif
677 */
678  return 0;
679 }
680 
681 
682 int RMatrixBase::
683 GetRotationAnglesXZY(double& PhiX, double& PhiY, double& PhiZ) const
684 {
685  if (fabs((*this)[0][1]) == 1.0) {
686  BIASWARN("Rotation for Z-axis is +-PI/2, can not decompose X-/Y-component!");
687  return -1;
688  }
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;
695 /*
696 #ifdef BIAS_DEBUG
697  RMatrixBase R;
698  R.SetXZY(PhiX, PhiY, PhiZ);
699  if (R == (*this))
700  res = 0;
701  else
702  res = -1;
703 #endif
704 */
705  return 0;
706 }
707 
708 
709 int RMatrixBase::
710 GetRotationAnglesYZX(double& PhiX, double& PhiY, double& PhiZ) const
711 {
712  if (fabs((*this)[1][0]) == 1.0) {
713  BIASWARN("Rotation for Z-axis is +-PI/2, can not decompose X-/Y-component!");
714  return -1;
715  }
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;
722 /*
723 #ifdef BIAS_DEBUG
724  RMatrixBase R;
725  R.SetYZX(PhiX, PhiY, PhiZ);
726  if (R == (*this))
727  res = 0;
728  else
729  res = -1;
730 #endif
731 */
732  return 0;
733 }
734 
735 
736 bool BIAS::operator==(const RMatrixBase& left, const RMatrixBase& right)
737 {
738  bool res=true;
739  for (register int i=0; i<9; i++){
740  if (fabs(left.GetData()[i]-right.GetData()[i])>
741  numeric_limits<double>::epsilon()){
742  res=false;
743  break;
744  }
745  }
746  return res;
747 }
748 
749 
750 int RMatrixBase::
751 SetFromQuaternion(const Quaternion<ROTATION_MATRIX_TYPE> &qu)
752 {
753  // fast version, see below for old code:
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;
759 
760  double* pData = GetData();
761 
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);
765 
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);
769 
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);
773 
774 
775 #ifdef WANT_TO_COMPARE_WITH_OLD_CODE
776 
777  Quaternion<ROTATION_MATRIX_TYPE> q = qu; // de-const
778  q.Normalize();
779 
780  // nicked from OpenSG
781  RMatrixBase RCompare(MatrixZero);
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]);
785 
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]);
789 
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]);
793 
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;
799  BIASABORT;
800  }
801  }
802  }
803 #endif
804 
805  return 0;
806 }
807 
808 
809 void RMatrixBase::
810 SetFromOrthogonalBasis(const BIAS::Vector3<ROTATION_MATRIX_TYPE> &ex,
811  const BIAS::Vector3<ROTATION_MATRIX_TYPE> &ey,
812  const BIAS::Vector3<ROTATION_MATRIX_TYPE> &ez)
813 {
814  SetFromRowVectors(ex.GetNormalized(),
815  ey.GetNormalized(),
816  ez.GetNormalized());
817  if (!Check(R_CONSTRAINT_ACCURACY)) {
818 #ifdef R_CONSTRAINTS_WARNING
819  BIASWARN("Rotation matrix is invalid: "
820  << GetCheckFailureReason(R_CONSTRAINT_ACCURACY)
821  << "\nEnforcing orthonormality and positive determinant!\n");
822 #endif
823  EnforceConstraints();
824  }
825 }
826 
827 
828 void RMatrixBase::
829 SetFromHV(const BIAS::Vector3<ROTATION_MATRIX_TYPE> &xh,
830  const BIAS::Vector3<ROTATION_MATRIX_TYPE> &vy)
831 {
832  // normalize
833  const BIAS::Vector3<ROTATION_MATRIX_TYPE> x0(xh.GetNormalized());
834 
835  // compute base z in RHS
836  const BIAS::Vector3<ROTATION_MATRIX_TYPE>
837  z0(xh.CrossProduct(vy).GetNormalized());
838 
839  // compute y base orthogonal in RHS
840  const BIAS::Vector3<ROTATION_MATRIX_TYPE>
841  y0(z0.CrossProduct(x0).GetNormalized());
842 
843  (*this).SetFromColumnVectors(x0, y0, z0);
844  if (!Check(R_CONSTRAINT_ACCURACY)) {
845 #ifdef R_CONSTRAINTS_WARNING
846  BIASWARN("Rotation matrix is invalid: "
847  << GetCheckFailureReason(R_CONSTRAINT_ACCURACY)
848  << "\nEnforcing orthonormality and positive determinant!\n");
849 #endif
850  EnforceConstraints();
851  }
852 }
853 
854 
855 int RMatrixBase::
856 SetFromOriUpGL(BIAS::Vector3<ROTATION_MATRIX_TYPE> ori,
857  BIAS::Vector3<ROTATION_MATRIX_TYPE> up)
858 {
859  // calculate orthogonal vectors
860  BIAS::Vector3<double> right;
861  ori.Normalize();
862  ori.CrossProduct(up, right);
863  right.Normalize();
864  right.CrossProduct(ori, up);
865  up.Normalize();
866  ori *= -1.0; // we need the right handed system
867  SetFromColumnVectors(right, up, ori);
868 
869  return 0;
870 }
871 
872 
873 int RMatrixBase::
874 SetFromOriUp(BIAS::Vector3<ROTATION_MATRIX_TYPE> ori,
875  BIAS::Vector3<ROTATION_MATRIX_TYPE> up)
876 {
877  // calculate orthogonal vectors
878  BIAS::Vector3<double> right;
879  ori.Normalize();
880  ori.CrossProduct(up, right);
881  right.Normalize();
882  ori.CrossProduct(right, up);
883  up.Normalize();
884  SetFromColumnVectors(right, up, ori);
885 
886  return 0;
887 }
888 
889 
890 bool RMatrixBase::
891 Check(const double eps, const bool verbose) const
892 {
893  // check determinant
894  bool ok = Equal(GetDeterminant(), 1.0, eps);
895  // check unit column vector length
896  Vector3<double> c[3];
897  if (ok) {
898  for (register unsigned int i = 0; i < 3 && ok; i++) {
899  GetColumn(i, c[i]);
900  ok = Equal(c[i].NormL2(), 1.0, eps);
901  }
902  }
903  // check orthogonality
904  if (ok)
905  ok = Equal(c[0].ScalarProduct(c[1]), 0.0, eps);
906  if (ok)
907  ok = Equal(c[0].ScalarProduct(c[2]), 0.0, eps);
908  if (ok)
909  ok = Equal(c[1].ScalarProduct(c[2]), 0.0, eps);
910  // print result in case of failure
911  if (!ok && verbose) {
912  BIASWARN("Rotation matrix is invalid: " << GetCheckFailureReason(eps));
913  }
914  return ok;
915 }
916 
917 
918 std::string RMatrixBase::
919 GetCheckFailureReason(const double eps) const
920 {
921  ostringstream res(" ");
922 
923  double det = GetDeterminant();
924  if (!Equal(det, 1.0, eps)) {
925  res << "det != 1 (det=" << setprecision(30) << det << ") ";
926  }
927  Vector3<double> c[3];
928  for (register unsigned int i = 0; i < 3; i++) {
929  GetColumn(i, c[i]);
930  if (!Equal(c[i].NormL2(), 1.0, eps)){
931  res << "col[" << i << "].NormL2() != 1) (= " << c[i].NormL2() << ") ";
932  }
933  }
934  if (!Equal(c[0].ScalarProduct(c[1]), 0.0, eps)){
935  res << "col[0].ScalarProduct(c[1]) != 0 (= "
936  << c[0].ScalarProduct(c[1]) << ") ";
937  }
938  if (!Equal(c[0].ScalarProduct(c[2]), 0.0, eps)){
939  res << "col[0].ScalarProduct(c[2]) != 0 (= "
940  << c[0].ScalarProduct(c[2]) << ") ";
941  }
942  if (!Equal(c[1].ScalarProduct(c[2]), 0.0, eps)){
943  res << "col[1].ScalarProduct(c[2]) != 0 (= "
944  << c[1].ScalarProduct(c[2]) << ") ";
945  }
946  return res.str();
947 }
948 
949 
950 void RMatrixBase::EnforceConstraints()
951 {
952  // @todo Enforcing constraints is not working right now!
953 #ifdef BIAS_DEBUG
954  Matrix3x3<double> oldR = *this;
955 #endif
956  const double det = GetDeterminant();
957  Vector3<double> c[3];
958  for (register unsigned int i = 0; i < 3; i++) {
959  GetColumn(i, c[i]);
960  if (Equal(c[i].NormL2(), 0.0))
961  BEXCEPTION("Rotation matrix has not full rank. Can not enforce "
962  "unit length of zero column vector!");
963  c[i] /= c[i].NormL2();
964  }
965  // enforce orthonormality of c[0] and c[1];
966  c[1] -= c[0].ScalarProduct(c[1])*c[0];
967  c[1] /= c[1].NormL2();
968  // set c[2] perpendicular to c[0] and c[1]
969  c[0].CrossProduct(c[1], c[2]);
970  c[2] /= c[2].NormL2();
971  // flip c[0] and c[1] if determinant is negative
972  if (det < 0.0) {
973  c[0] *= -1.0;
974  c[1] *= -1.0;
975  }
976  // set columns
977  for (register unsigned int i = 0; i < 3; i++) {
978  SetColumn(i, c[i]);
979  }
980  BIASASSERT(GetDeterminant() > 0.0);
981 
982 /*
983  // flip c[2] if cross product inverted direction
984  // @todo check if this happens only when determinant is negative!!
985  int maxRow2 = 0;
986  if (fabs(c[2][1]) > fabs(c[2][maxRow2])) maxRow2 = 1;
987  if (fabs(c[2][2]) > fabs(c[2][maxRow2])) maxRow2 = 2;
988  if ((c[2][maxRow2] < 0 && oldR[maxRow2][2] > 0) ||
989  (c[2][maxRow2] > 0 && oldR[maxRow2][2] < 0)) {
990  c[2] *= -1.0;
991 #ifdef BIAS_DEBUG
992  // show warning when 3rd column is flipped (happens for input
993  // rotation matrix with determinant -1)
994  Matrix3x3<double> tmpR;
995  for (register unsigned int i = 0; i < 3; i++) tmpR.SetColumn(i, c[i]);
996  BIASWARN("\n [ATTENTION] Flipped 3rd column in R:\n- old R : "
997  << oldR << "- new R : " << tmpR << "- old determinant : "
998  << oldR.GetDeterminant() << "\n- new determinant : "
999  << tmpR.GetDeterminant() << endl);
1000  BIASASSERT(oldR.GetDeterminant() < 0);
1001 #endif
1002  } else {
1003  BIASASSERT(oldR.GetDeterminant() > 0);
1004  }
1005  // set columns
1006  for (register unsigned int i = 0; i < 3; i++) {
1007  SetColumn(i, c[i]);
1008  }
1009  if (GetDeterminant() < 0.0) *this *= -1.0;
1010 */
1011 
1012 /*
1013  if (!Check()) {
1014 #ifdef R_CONSTRAINTS_WARNING
1015  BIASERR("- R beforce constraint enforcement: " << oldR
1016  << "\n- R after constraint enforcement: "<< *this
1017  << "\n-> orthonormality enforcement failed with reason "
1018  << GetCheckFailureReason());
1019 #endif
1020  }
1021 */
1022 }
BIAS::RMatrixBase::GetRotationAnglesZYX
int GetRotationAnglesZYX(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order ZYX.
Definition: RMatrixBase.cpp:602
BIAS::RMatrixBase::SetFromOrthogonalBasis
void SetFromOrthogonalBasis(const BIAS::Vector3< ROTATION_MATRIX_TYPE > &ex, const BIAS::Vector3< ROTATION_MATRIX_TYPE > &ey, const BIAS::Vector3< ROTATION_MATRIX_TYPE > &ez)
Definition: RMatrixBase.cpp:810
BIAS::MatrixInitType
MatrixInitType
can be passed to matrix constructors to init the matrix with the most often used values ...
Definition: Matrix.hh:59
BIAS::RMatrixBase::SetXYZ
void SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order XYZ.
Definition: RMatrixBase.cpp:131
TNT::Matrix::num_cols
Subscript num_cols() const
Definition: cmat.h:320
BIAS::Matrix3x3< ROTATION_MATRIX_TYPE >::SetColumn
void SetColumn(const unsigned int col, const Vector3< ROTATION_MATRIX_TYPE > &c)
BIAS::RMatrixBase::SetXZY
void SetXZY(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order XZY.
Definition: RMatrixBase.cpp:265
BIAS::RMatrixBase::SetZYX
void SetZYX(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order ZYX.
Definition: RMatrixBase.cpp:158
BIAS::RMatrixBase::GetRotationAnglesYZX
int GetRotationAnglesYZX(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order YZX.
Definition: RMatrixBase.cpp:710
BIAS::Vector3::ScalarProduct
void ScalarProduct(const Vector3< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
Definition: Vector3.hh:603
BIAS::RMatrixBase::~RMatrixBase
virtual ~RMatrixBase()
Definition: RMatrixBase.cpp:126
BIAS::RMatrixBase::GetRotationAxis
BIAS::Vector3< ROTATION_MATRIX_TYPE > GetRotationAxis() const
Interface for axis component of GetRotationAxisAngle()
Definition: RMatrixBase.cpp:526
BIAS::Vector3::GetNormalized
BIAS::Vector3< T > GetNormalized() const
return a normalized vector of this
Definition: Vector3.cpp:47
BIAS::Matrix3x3< ROTATION_MATRIX_TYPE >::GetColumn
void GetColumn(const unsigned int col, Vector3< ROTATION_MATRIX_TYPE > &r) const
extract one column (&#39;Spalte&#39;) from this matrix (for convenience)
BIAS::RMatrixBase::SetFromQuaternion
int SetFromQuaternion(const Quaternion< ROTATION_MATRIX_TYPE > &q)
Set rotation matrix from a quaternion.
Definition: RMatrixBase.cpp:751
BIAS::RMatrixBase::Check
bool Check(const double eps=std::numeric_limits< double >::epsilon(), const bool verbose=false) const
Check if this is a rotation matrix, i.e.
Definition: RMatrixBase.cpp:891
BIAS::RMatrixBase::SetYXZ
void SetYXZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order YXZ.
Definition: RMatrixBase.cpp:185
BIAS::RMatrixBase::SetZXY
void SetZXY(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order ZXY.
Definition: RMatrixBase.cpp:212
BIAS::Quaternion::GetAxisAngle
int GetAxisAngle(Vector3< QUAT_TYPE > &axis, QUAT_TYPE &angle) const
Returns rotation in axis and angle notation (angle in radians).
BIAS::Vector4< QUAT_TYPE >::NormL2
double NormL2() const
Return the L2 norm: sqrt(a^2 + b^2 + c^2 + d^2)
BIAS::RMatrixBase::GetQuaternion
int GetQuaternion(Quaternion< ROTATION_MATRIX_TYPE > &quat) const
Calculates quaternion representation for this rotation matrix.
Definition: RMatrixBase.cpp:345
BIAS::Quaternion::Normalize
void Normalize()
Scales quaternion to unit length, i.e.
Definition: Quaternion.hh:236
BIAS::RMatrixBase::EnforceConstraints
virtual void EnforceConstraints()
Enforce orthonormality constraint on rotation matrix and sets determinant to +1.
Definition: RMatrixBase.cpp:950
BIAS::RMatrixBase::GetRotationAnglesXZY
int GetRotationAnglesXZY(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order XZY.
Definition: RMatrixBase.cpp:683
BIAS::RMatrixBase::GetCheckFailureReason
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.
Definition: RMatrixBase.cpp:919
BIAS::RMatrixBase::SetFromOriUp
int SetFromOriUp(BIAS::Vector3< ROTATION_MATRIX_TYPE > ori, BIAS::Vector3< ROTATION_MATRIX_TYPE > up)
Set rotation matrix from orientation and up vector.
Definition: RMatrixBase.cpp:874
BIAS::RMatrixBase::Set
void Set(const Vector3< ROTATION_MATRIX_TYPE > &w, const ROTATION_MATRIX_TYPE phi)
Set from rotation axis w and angle phi (in rad)
Definition: RMatrixBase.cpp:291
BIAS::Vector3::CrossProduct
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
Definition: Vector3.hh:594
BIAS::RMatrixBase::SetFromOriUpGL
int SetFromOriUpGL(BIAS::Vector3< ROTATION_MATRIX_TYPE > ori, BIAS::Vector3< ROTATION_MATRIX_TYPE > up)
Set rotation matrix from orientation and up vector.
Definition: RMatrixBase.cpp:856
BIAS::RMatrixBase::GetRotationAnglesZXY
int GetRotationAnglesZXY(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order ZXY.
Definition: RMatrixBase.cpp:629
BIAS::Matrix3x3< ROTATION_MATRIX_TYPE >::SetFromRowVectors
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
BIAS::RMatrixBase::RMatrixBase
RMatrixBase()
Constructor setting matrix to identity.
Definition: RMatrixBase.cpp:53
BIAS::Matrix3x3
is a &#39;fixed size&#39; quadratic matrix of dim.
Definition: Matrix.hh:54
BIAS::Vector3
class Vector3 contains a Vector of fixed dim.
Definition: Matrix.hh:53
BIAS::RMatrixBase::SetFromAxisAngle
void SetFromAxisAngle(Vector3< ROTATION_MATRIX_TYPE > w)
Set from rotation axis * angle (modified Rodrigues vector)
Definition: RMatrixBase.cpp:332
BIAS::Equal
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_...
Definition: CompareFloatingPoint.cpp:23
BIAS::RMatrixBase::SetFromHV
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.
Definition: RMatrixBase.cpp:829
BIAS::Vector4< QUAT_TYPE >::MultiplyIP
void MultiplyIP(const QUAT_TYPE &scalar)
Multiplication (in place) of an scalar.
BIAS::RMatrixBase::GetRotationAnglesXYZ
int GetRotationAnglesXYZ(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order XYZ.
Definition: RMatrixBase.cpp:575
BIAS::Matrix3x3< ROTATION_MATRIX_TYPE >::GetDeterminant
ROTATION_MATRIX_TYPE GetDeterminant() const
returns the Determinant |A| of this
BIAS::RMatrixBase
Implements a 3D rotation matrix.
Definition: RMatrixBase.hh:44
TNT::Matrix::num_rows
Subscript num_rows() const
Definition: cmat.h:319
BIAS::RMatrixBase::GetRotationAxisAngle
int GetRotationAxisAngle(Vector3< ROTATION_MATRIX_TYPE > &axis, ROTATION_MATRIX_TYPE &angle) const
Calculates angle and rotation axis representation for this rotation matrix.
Definition: RMatrixBase.cpp:514
BIAS::RMatrixBase::GetRotationAnglesYXZ
int GetRotationAnglesYXZ(double &PhiX, double &PhiY, double &PhiZ) const
Get Euler angles for this rotation matrix in order YXZ.
Definition: RMatrixBase.cpp:656
BIAS::Matrix3x3< ROTATION_MATRIX_TYPE >::SetFromColumnVectors
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
BIAS::Quaternion
class for rotation with axis and angle
Definition: DualQuaternionOperators.hh:32
BIAS::RMatrixBase::GetRotationAngle
ROTATION_MATRIX_TYPE GetRotationAngle() const
Interface for angle component of GetRotationAxisAngle()
Definition: RMatrixBase.cpp:543
BIAS::Vector3::Normalize
Vector3< T > & Normalize()
normalize this vector to length 1
Definition: Vector3.hh:663
BIAS::RMatrixBase::SetYZX
void SetYZX(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order YZX.
Definition: RMatrixBase.cpp:238
BIAS::Vector3::NormL2
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
Definition: Vector3.hh:633
BIAS::Matrix3x3< ROTATION_MATRIX_TYPE >::SetIdentity
void SetIdentity()
set the elements of this matrix to the identity matrix (possibly overriding the inherited method) ...
Generated on Tue Feb 6 2018 02:01:31 for Basic Image AlgorithmS Library by   doxygen 1.8.5