49 #include <visp/vpMath.h>
50 #include <visp/vpMatrix.h>
53 #include <visp/vpRotationMatrix.h>
57 #include <visp/vpException.h>
58 #include <visp/vpMatrixException.h>
61 #include <visp/vpDebug.h>
63 const double vpRotationMatrix::threshold = 1e-6;
64 const double vpRotationMatrix::minimum = 0.00001;
66 #define vpDEBUG_LEVEL1 0
85 for (i=0 ; i < 3 ; i++)
86 for (j=0 ; j < 3; j++)
123 for (
unsigned int i=0; i<3; i++)
125 for (
unsigned int j=0; j<3; j++)
147 "m is not a rotation matrix !!!!!"));
150 for (
unsigned int i=0; i<3; i++)
152 for (
unsigned int j=0; j<3; j++)
154 (*this)[i][j] = m[i][j];
162 "m is not a rotation matrix !!!!!"));
174 for (
unsigned int i=0;i<3;i++)
175 for (
unsigned int j=0;j<3;j++)
178 for (
unsigned int k=0;k<3;k++)
202 vpERROR_TRACE(
"The matrix is not a 3 by 3 dimension matrix") ;
204 "The matrix is not a 3 by 3 dimension matrix"));
208 for (
unsigned int i=0;i<3;i++)
209 for (
unsigned int j=0;j<3;j++)
212 for (
unsigned int k=0;k<3;k++)
213 s +=(*
this)[i][k] * B[k][j];
252 vpERROR_TRACE(
"The column vector is not a 3 dimension vector") ;
254 "The column vector is not a 3 dimension vector"));
273 "Cannot add two rotation matrices !!!!!"));
285 vpERROR_TRACE(
"Cannot substract two rotation matrices !!!!! ") ;
287 "Cannot substract two rotation matrices !!!!!"));
296 for (
unsigned int j=0;j<3;j++)p[j]=0 ;
298 for (
unsigned int j=0;j<3;j++) {
299 for (
unsigned int i=0;i<3;i++) {
317 bool isRotation = true ;
321 for (i=0 ; i < 3 ; i++)
322 for (j=0 ; j < 3 ; j++)
325 if (fabs(RtR[i][j]-1) > threshold) isRotation = false ;
329 if (fabs(RtR[i][j]) > threshold) isRotation = false ;
334 for (i=0 ; i < 3 ; i++)
337 vpMath::sqr(RtR[2][i])) - 1) > threshold) isRotation = false ;
340 for (i=0 ; i < 3 ; i++)
343 vpMath::sqr(RtR[i][2])) - 1) > threshold) isRotation = false ;
428 Rt[j][i] = (*
this)[i][j];
461 for (
unsigned int i=0; i<3; i++)
463 for (
unsigned int j=0; j<3; j++)
464 std::cout << R[i][j] <<
" " ;
465 std::cout << std::endl ;
477 for (
unsigned int i=0; i<3; i++)
478 std::cout << tu[i] <<
" " ;
480 std::cout << std::endl ;
500 double theta, si, co, sinc, mcosc;
503 theta = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
509 R[0][0] = co + mcosc*v[0]*v[0];
510 R[0][1] = -sinc*v[2] + mcosc*v[0]*v[1];
511 R[0][2] = sinc*v[1] + mcosc*v[0]*v[2];
512 R[1][0] = sinc*v[2] + mcosc*v[1]*v[0];
513 R[1][1] = co + mcosc*v[1]*v[1];
514 R[1][2] = -sinc*v[0] + mcosc*v[1]*v[2];
515 R[2][0] = -sinc*v[1] + mcosc*v[2]*v[0];
516 R[2][1] = sinc*v[0] + mcosc*v[2]*v[1];
517 R[2][2] = co + mcosc*v[2]*v[2];
519 for (i=0;i<3;i++)
for (j=0;j<3;j++) (*
this)[i][j] = R[i][j];
521 #if (vpDEBUG_LEVEL1) // test new version wrt old version
525 double sinu,cosi,mcosi,u[3],ang;
527 ang = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
530 for (i=0;i<3;i++) u[i] = v[i]/ang;
534 R_old[0][0] = cosi + mcosi*u[0]*u[0];
535 R_old[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
536 R_old[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
537 R_old[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
538 R_old[1][1] = cosi + mcosi*u[1]*u[1];
539 R_old[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
540 R_old[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
541 R_old[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
542 R_old[2][2] = cosi + mcosi*u[2]*u[2];
548 for(j=0;j<3;j++) R_old[i][j] = 0.0;
559 if (fabs(R_old[i][j] - R[i][j]) > 1.e-4) pb = 1;
563 printf(
"vpRotationMatrix::buildFrom(const vpThetaUVector &v)\n");
564 std::cout <<
" theta " << theta << std::endl;
565 std::cout <<
" R : " << std::endl << R << std::endl;
566 std::cout <<
" R_old : " << std::endl << R_old << std::endl;
585 double c0,c1,c2,s0,s1,s2;
594 (*this)[0][0] = c0*c1*c2 - s0*s2;
595 (*this)[0][1] = -c0*c1*s2 - s0*c2;
596 (*this)[0][2] = c0*s1;
597 (*this)[1][0] = s0*c1*c2+c0*s2 ;
598 (*this)[1][1] = -s0*c1*s2 + c0*c2 ;
599 (*this)[1][2] = s0*s1;
600 (*this)[2][0] = -s1*c2;
601 (*this)[2][1] = s1*s2;
620 double c0,c1,c2,s0,s1,s2;
629 (*this)[0][0] = c1*c2;
630 (*this)[0][1] = -c1*s2;
632 (*this)[1][0] = c0*s2+s0*s1*c2;
633 (*this)[1][1] = c0*c2-s0*s1*s2;
634 (*this)[1][2] = -s0*c1;
635 (*this)[2][0] = -c0*s1*c2+s0*s2;
636 (*this)[2][1] = c0*s1*s2+c2*s0;
637 (*this)[2][2] = c0*c1;
654 double c0,c1,c2,s0,s1,s2;
663 (*this)[0][0] = c0*c1 ;
664 (*this)[0][1] = c0*s1*s2 - s0*c2 ;
665 (*this)[0][2] = c0*s1*c2 + s0*s2 ;
667 (*this)[1][0] = s0*c1 ;
668 (*this)[1][1] = s0*s1*s2 + c0*c2 ;
669 (*this)[1][2] = s0*s1*c2 - c0*s2 ;
671 (*this)[2][0] = -s1 ;
672 (*this)[2][1] = c1*s2 ;
673 (*this)[2][2] = c1*c2 ;
699 (*this)[0][0] = a*a+b*b-c*c-d*d;
700 (*this)[0][1] = 2*b*c-2*a*d;
701 (*this)[0][2] = 2*a*c+2*b*d;
703 (*this)[1][0] = 2*a*d+2*b*c;
704 (*this)[1][1] = a*a-b*b+c*c-d*d;
705 (*this)[1][2] = 2*c*d-2*a*b;
707 (*this)[2][0] = 2*b*d-2*a*c;
708 (*this)[2][1] = 2*a*b+2*c*d;
709 (*this)[2][2] = a*a-b*b-c*c+d*d;
712 #undef vpDEBUG_LEVEL1