45 #include <visp3/core/vpMath.h> 46 #include <visp3/core/vpMatrix.h> 49 #include <visp3/core/vpRotationMatrix.h> 53 #include <visp3/core/vpException.h> 56 #include <visp3/core/vpDebug.h> 58 const double vpRotationMatrix::threshold = 1e-6;
68 for (
unsigned int i=0 ; i < 3 ; i++) {
69 for (
unsigned int j=0 ; j < 3; j++) {
90 for (
unsigned int i=0; i<3; i++) {
91 for (
unsigned int j=0; j<3; j++) {
119 "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
123 for (
unsigned int i=0; i<3; i++) {
124 for (
unsigned int j=0; j<3; j++) {
125 (*this)[i][j] = M[i][j];
131 "Cannot set a rotation matrix from a matrix that is not a rotation matrix"));
145 for (
unsigned int i=0;i<3;i++) {
146 for (
unsigned int j=0;j<3;j++) {
148 for (
unsigned int k=0;k<3;k++)
174 "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
179 for (
unsigned int i=0;i<3;i++) {
180 for (
unsigned int j=0;j<3;j++) {
182 for (
unsigned int k=0;k<3;k++)
183 s +=(*
this)[i][k] * M[k][j];
224 "Cannot multiply a (3x3) rotation matrix by a %d dimension column vector",
229 for (
unsigned int j=0;j<
colNum;j++) {
231 for (
unsigned int i=0;i<
rowNum;i++) {
232 v_out[i] +=
rowPtrs[i][j] * vj;
248 for (
unsigned int j=0;j<3;j++)
251 for (
unsigned int j=0;j<3;j++) {
252 for (
unsigned int i=0;i<3;i++) {
268 for (
unsigned int i=0;i<
rowNum;i++)
269 for(
unsigned int j=0;j<
colNum;j++)
281 for (
unsigned int i=0;i<
rowNum;i++)
282 for(
unsigned int j=0;j<
colNum;j++)
297 bool isRotation = true ;
301 for (i=0 ; i < 3 ; i++) {
302 for (j=0 ; j < 3 ; j++) {
304 if (fabs(RtR[i][j]-1) > threshold) isRotation = false ;
307 if (fabs(RtR[i][j]) > threshold) isRotation = false ;
313 for (i=0 ; i < 3 ; i++) {
316 vpMath::sqr(RtR[2][i])) - 1) > threshold) isRotation = false ;
320 for (i=0 ; i < 3 ; i++) {
323 vpMath::sqr(RtR[i][2])) - 1) > threshold) isRotation = false ;
424 Rt[j][i] = (*
this)[i][j];
472 for (
unsigned int i=0; i<3; i++)
473 std::cout << tu[i] <<
" " ;
475 std::cout << std::endl ;
491 double theta, si, co, sinc, mcosc;
494 theta = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
500 R[0][0] = co + mcosc*v[0]*v[0];
501 R[0][1] = -sinc*v[2] + mcosc*v[0]*v[1];
502 R[0][2] = sinc*v[1] + mcosc*v[0]*v[2];
503 R[1][0] = sinc*v[2] + mcosc*v[1]*v[0];
504 R[1][1] = co + mcosc*v[1]*v[1];
505 R[1][2] = -sinc*v[0] + mcosc*v[1]*v[2];
506 R[2][0] = -sinc*v[1] + mcosc*v[2]*v[0];
507 R[2][1] = sinc*v[0] + mcosc*v[2]*v[1];
508 R[2][2] = co + mcosc*v[2]*v[2];
510 for (i=0;i<3;i++)
for (j=0;j<3;j++) (*
this)[i][j] = R[i][j];
521 for (
unsigned int i=0 ; i < 3 ; i++)
522 for (
unsigned int j=0 ; j < 3; j++)
523 (*
this)[i][j] = M[i][j] ;
549 double c0,c1,c2,s0,s1,s2;
558 (*this)[0][0] = c0*c1*c2 - s0*s2;
559 (*this)[0][1] = -c0*c1*s2 - s0*c2;
560 (*this)[0][2] = c0*s1;
561 (*this)[1][0] = s0*c1*c2+c0*s2 ;
562 (*this)[1][1] = -s0*c1*s2 + c0*c2 ;
563 (*this)[1][2] = s0*s1;
564 (*this)[2][0] = -s1*c2;
565 (*this)[2][1] = s1*s2;
583 double c0,c1,c2,s0,s1,s2;
592 (*this)[0][0] = c1*c2;
593 (*this)[0][1] = -c1*s2;
595 (*this)[1][0] = c0*s2+s0*s1*c2;
596 (*this)[1][1] = c0*c2-s0*s1*s2;
597 (*this)[1][2] = -s0*c1;
598 (*this)[2][0] = -c0*s1*c2+s0*s2;
599 (*this)[2][1] = c0*s1*s2+c2*s0;
600 (*this)[2][2] = c0*c1;
616 double c0,c1,c2,s0,s1,s2;
625 (*this)[0][0] = c0*c1 ;
626 (*this)[0][1] = c0*s1*s2 - s0*c2 ;
627 (*this)[0][2] = c0*s1*c2 + s0*s2 ;
629 (*this)[1][0] = s0*c1 ;
630 (*this)[1][1] = s0*s1*s2 + c0*c2 ;
631 (*this)[1][2] = s0*s1*c2 - c0*s2 ;
633 (*this)[2][0] = -s1 ;
634 (*this)[2][1] = c1*s2 ;
635 (*this)[2][2] = c1*c2 ;
665 (*this)[0][0] = a*a+b*b-c*c-d*d;
666 (*this)[0][1] = 2*b*c-2*a*d;
667 (*this)[0][2] = 2*a*c+2*b*d;
669 (*this)[1][0] = 2*a*d+2*b*c;
670 (*this)[1][1] = a*a-b*b+c*c-d*d;
671 (*this)[1][2] = 2*c*d-2*a*b;
673 (*this)[2][0] = 2*b*d-2*a*c;
674 (*this)[2][1] = 2*a*b+2*c*d;
675 (*this)[2][2] = a*a-b*b-c*c+d*d;
686 unsigned int Rrow = R.
getRows() ;
687 unsigned int Rcol = R.
getCols() ;
689 for (
unsigned int i=0;i<Rrow;i++)
690 for(
unsigned int j=0;j<Rcol;j++)
738 "Unable to extract a column vector from the homogeneous matrix"));
739 unsigned int nb_rows =
getRows();
741 for (
unsigned int i=0 ; i < nb_rows ; i++)
742 c[i] = (*
this)[i][j];
746 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) 761 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) Implementation of a matrix and operations on matrices.
Implementation of an homogeneous matrix and operations on such kind of matrices.
double z() const
Returns z-component of the quaternion.
error that can be emited by ViSP classes.
unsigned int getRows() const
Return the number of rows of the 2D array.
Implementation of a generic 2D array used as vase class of matrices and vectors.
Implementation of a rotation vector as Euler angle minimal representation.
vp_deprecated void setIdentity()
vpRotationMatrix inverse() const
vpRotationMatrix & operator=(const vpRotationMatrix &R)
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix t() const
static double sinc(double x)
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
Return the number of columns of the 2D array.
unsigned int rowNum
Number of rows in the array.
vpRotationMatrix & operator*=(const double x)
static double mcosc(double cosx, double x)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
static double sqr(double x)
vpColVector getCol(const unsigned int j) const
Implementation of a rotation vector as quaternion angle minimal representation.
double y() const
Returns y-component of the quaternion.
unsigned int colNum
Number of columns in the array.
double x() const
Returns x-component of the quaternion.
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as Euler angle minimal representation.
double w() const
Returns w-component of the quaternion.
vpThetaUVector getThetaUVector()
Implementation of a rotation vector as Euler angle minimal representation.
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
bool isARotationMatrix() const
double ** rowPtrs
Address of the first element of each rows.
vpTranslationVector operator*(const vpTranslationVector &tv) const