From 2832d2e76ddc397369ec7fe5618186d5a8fcaab3 Mon Sep 17 00:00:00 2001 From: ponchio Date: Wed, 21 Nov 2012 11:38:51 +0000 Subject: [PATCH] added a const --- vcg/math/deprecated_matrix33.h | 992 ++++++++++++++++----------------- 1 file changed, 496 insertions(+), 496 deletions(-) diff --git a/vcg/math/deprecated_matrix33.h b/vcg/math/deprecated_matrix33.h index 9aff05a8..6b0261b8 100644 --- a/vcg/math/deprecated_matrix33.h +++ b/vcg/math/deprecated_matrix33.h @@ -103,429 +103,429 @@ template class Matrix33Diag:public Point3{ public: - /** @name Matrix33 - Class Matrix33Diag. + /** @name Matrix33 + Class Matrix33Diag. This is the class for definition of a diagonal matrix 3x3. - @param S (Templete Parameter) Specifies the ScalarType field. + @param S (Templete Parameter) Specifies the ScalarType field. */ - Matrix33Diag(const S & p0,const S & p1,const S & p2):Point3(p0,p1,p2){}; - Matrix33Diag(const Point3&p ):Point3(p){}; + Matrix33Diag(const S & p0,const S & p1,const S & p2):Point3(p0,p1,p2){}; + Matrix33Diag(const Point3&p ):Point3(p){}; }; template /** @name Matrix33 - Class Matrix33. + Class Matrix33. This is the class for definition of a matrix 3x3. - @param S (Templete Parameter) Specifies the ScalarType field. + @param S (Templete Parameter) Specifies the ScalarType field. */ class Matrix33 { public: - typedef S ScalarType; + typedef S ScalarType; - /// Default constructor - inline Matrix33() {} + /// Default constructor + inline Matrix33() {} - /// Copy constructor - Matrix33( const Matrix33 & m ) - { - for(int i=0;i<9;++i) - a[i] = m.a[i]; - } + /// Copy constructor + Matrix33( const Matrix33 & m ) + { + for(int i=0;i<9;++i) + a[i] = m.a[i]; + } - /// create from array - Matrix33( const S * v ) - { - for(int i=0;i<9;++i) a[i] = v[i]; - } + /// create from array + Matrix33( const S * v ) + { + for(int i=0;i<9;++i) a[i] = v[i]; + } - /// create from Matrix44 excluding row and column k - Matrix33 (const Matrix44 & m, const int & k) - { - int i,j, r=0, c=0; - for(i = 0; i< 4;++i)if(i!=k){c=0; - for(j=0; j < 4;++j)if(j!=k) - { (*this)[r][c] = m[i][j]; ++c;} - ++r; - } - } + /// create from Matrix44 excluding row and column k + Matrix33 (const Matrix44 & m, const int & k) + { + int i,j, r=0, c=0; + for(i = 0; i< 4;++i)if(i!=k){c=0; + for(j=0; j < 4;++j)if(j!=k) + { (*this)[r][c] = m[i][j]; ++c;} + ++r; + } + } - template - void ToEigenMatrix(EigenMatrix33Type & m){ - for(int i = 0; i < 3; i++) - for(int j = 0; j < 3; j++) - m(i,j)=(*this)[i][j]; - } + template + void ToEigenMatrix(EigenMatrix33Type & m) const { + for(int i = 0; i < 3; i++) + for(int j = 0; j < 3; j++) + m(i,j)=(*this)[i][j]; + } - template - void FromEigenMatrix(const EigenMatrix33Type & m){ - for(int i = 0; i < 3; i++) - for(int j = 0; j < 3; j++) - (*this)[i][j]=m(i,j); - } + template + void FromEigenMatrix(const EigenMatrix33Type & m){ + for(int i = 0; i < 3; i++) + for(int j = 0; j < 3; j++) + (*this)[i][j]=m(i,j); + } - /// Number of columns - inline unsigned int ColumnsNumber() const - { - return 3; - }; + /// Number of columns + inline unsigned int ColumnsNumber() const + { + return 3; + }; - /// Number of rows - inline unsigned int RowsNumber() const - { - return 3; - }; + /// Number of rows + inline unsigned int RowsNumber() const + { + return 3; + }; - /// Assignment operator - Matrix33 & operator = ( const Matrix33 & m ) - { - for(int i=0;i<9;++i) - a[i] = m.a[i]; - return *this; - } + /// Assignment operator + Matrix33 & operator = ( const Matrix33 & m ) + { + for(int i=0;i<9;++i) + a[i] = m.a[i]; + return *this; + } - /// Operatore di indicizzazione - inline S * operator [] ( const int i ) - { - return a+i*3; - } - /// Operatore const di indicizzazione - inline const S * operator [] ( const int i ) const - { - return a+i*3; - } + /// Operatore di indicizzazione + inline S * operator [] ( const int i ) + { + return a+i*3; + } + /// Operatore const di indicizzazione + inline const S * operator [] ( const int i ) const + { + return a+i*3; + } - /// Modificatore somma per matrici 3x3 - Matrix33 & operator += ( const Matrix33 &m ) - { - for(int i=0;i<9;++i) - a[i] += m.a[i]; - return *this; - } + /// Modificatore somma per matrici 3x3 + Matrix33 & operator += ( const Matrix33 &m ) + { + for(int i=0;i<9;++i) + a[i] += m.a[i]; + return *this; + } - /// Modificatore somma per matrici 3x3 - Matrix33 & operator += ( const Matrix33Diag &p ) - { - a[0] += p[0]; - a[4] += p[1]; - a[8] += p[2]; - return *this; - } + /// Modificatore somma per matrici 3x3 + Matrix33 & operator += ( const Matrix33Diag &p ) + { + a[0] += p[0]; + a[4] += p[1]; + a[8] += p[2]; + return *this; + } - /// Modificatore sottrazione per matrici 3x3 - Matrix33 & operator -= ( const Matrix33 &m ) - { - for(int i=0;i<9;++i) - a[i] -= m.a[i]; - return *this; - } + /// Modificatore sottrazione per matrici 3x3 + Matrix33 & operator -= ( const Matrix33 &m ) + { + for(int i=0;i<9;++i) + a[i] -= m.a[i]; + return *this; + } - /// Modificatore somma per matrici 3x3 - Matrix33 & operator -= ( const Matrix33Diag &p ) - { - a[0] -= p[0]; - a[4] -= p[1]; - a[8] -= p[2]; - return *this; - } + /// Modificatore somma per matrici 3x3 + Matrix33 & operator -= ( const Matrix33Diag &p ) + { + a[0] -= p[0]; + a[4] -= p[1]; + a[8] -= p[2]; + return *this; + } - /// Modificatore divisione per scalare - Matrix33 & operator /= ( const S &s ) - { - for(int i=0;i<9;++i) - a[i] /= s; - return *this; - } + /// Modificatore divisione per scalare + Matrix33 & operator /= ( const S &s ) + { + for(int i=0;i<9;++i) + a[i] /= s; + return *this; + } - /// Modificatore prodotto per matrice - Matrix33 operator * ( const Matrix33< S> & t ) const - { - Matrix33 r; + /// Modificatore prodotto per matrice + Matrix33 operator * ( const Matrix33< S> & t ) const + { + Matrix33 r; - int i,j; - for(i=0;i<3;++i) - for(j=0;j<3;++j) - r[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j]; + int i,j; + for(i=0;i<3;++i) + for(j=0;j<3;++j) + r[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j]; - return r; - } + return r; + } - /// Modificatore prodotto per matrice - void operator *=( const Matrix33< S> & t ) - { + /// Modificatore prodotto per matrice + void operator *=( const Matrix33< S> & t ) + { Matrix33 r; - int i,j; - for(i=0;i<3;++i) - for(j=0;j<3;++j) + int i,j; + for(i=0;i<3;++i) + for(j=0;j<3;++j) r[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j]; for(i=0;i<9;++i) this->a[i] = r.a[i]; - } + } - /// Dot product with a diagonal matrix - Matrix33 operator * ( const Matrix33Diag< S> & t ) const - { - Matrix33 r; + /// Dot product with a diagonal matrix + Matrix33 operator * ( const Matrix33Diag< S> & t ) const + { + Matrix33 r; - int i,j; - for(i=0;i<3;++i) - for(j=0;j<3;++j) - r[i][j] = (*this)[i][j]*t[j]; + int i,j; + for(i=0;i<3;++i) + for(j=0;j<3;++j) + r[i][j] = (*this)[i][j]*t[j]; - return r; - } + return r; + } - /// Dot product modifier with a diagonal matrix - void operator *=( const Matrix33Diag< S> & t ) - { + /// Dot product modifier with a diagonal matrix + void operator *=( const Matrix33Diag< S> & t ) + { Matrix33 r; - int i,j; - for(i=0;i<3;++i) - for(j=0;j<3;++j) + int i,j; + for(i=0;i<3;++i) + for(j=0;j<3;++j) r[i][j] = (*this)[i][j]*t[j]; for(i=0;i<9;++i) this->a[i] = r.a[i]; } - /// Modificatore prodotto per costante - Matrix33 & operator *= ( const S t ) - { - for(int i=0;i<9;++i) - a[i] *= t; - return *this; - } + /// Modificatore prodotto per costante + Matrix33 & operator *= ( const S t ) + { + for(int i=0;i<9;++i) + a[i] *= t; + return *this; + } - /// Operatore prodotto per costante - Matrix33 operator * ( const S t ) const - { - Matrix33 r; - for(int i=0;i<9;++i) - r.a[i] = a[i]* t; + /// Operatore prodotto per costante + Matrix33 operator * ( const S t ) const + { + Matrix33 r; + for(int i=0;i<9;++i) + r.a[i] = a[i]* t; - return r; - } + return r; + } - /// Operatore sottrazione per matrici 3x3 - Matrix33 operator - ( const Matrix33 &m ) const - { - Matrix33 r; - for(int i=0;i<9;++i) - r.a[i] = a[i] - m.a[i]; + /// Operatore sottrazione per matrici 3x3 + Matrix33 operator - ( const Matrix33 &m ) const + { + Matrix33 r; + for(int i=0;i<9;++i) + r.a[i] = a[i] - m.a[i]; - return r; - } + return r; + } - /// Operatore sottrazione di matrici 3x3 con matrici diagonali - Matrix33 operator - ( const Matrix33Diag &p ) const - { - Matrix33 r=a; - r[0][0] -= p[0]; - r[1][1] -= p[1]; - r[2][2] -= p[2]; - return r; - } + /// Operatore sottrazione di matrici 3x3 con matrici diagonali + Matrix33 operator - ( const Matrix33Diag &p ) const + { + Matrix33 r=a; + r[0][0] -= p[0]; + r[1][1] -= p[1]; + r[2][2] -= p[2]; + return r; + } - /// Operatore sottrazione per matrici 3x3 - Matrix33 operator + ( const Matrix33 &m ) const - { - Matrix33 r; - for(int i=0;i<9;++i) - r.a[i] = a[i] + m.a[i]; + /// Operatore sottrazione per matrici 3x3 + Matrix33 operator + ( const Matrix33 &m ) const + { + Matrix33 r; + for(int i=0;i<9;++i) + r.a[i] = a[i] + m.a[i]; - return r; - } + return r; + } - /// Operatore addizione di matrici 3x3 con matrici diagonali - Matrix33 operator + ( const Matrix33Diag &p ) const - { - Matrix33 r=(*this); - r[0][0] += p[0]; - r[1][1] += p[1]; - r[2][2] += p[2]; - return r; - } + /// Operatore addizione di matrici 3x3 con matrici diagonali + Matrix33 operator + ( const Matrix33Diag &p ) const + { + Matrix33 r=(*this); + r[0][0] += p[0]; + r[1][1] += p[1]; + r[2][2] += p[2]; + return r; + } - /** Operatore per il prodotto matrice-vettore. - @param v A point in $R^{3}$ - @return Il vettore risultante in $R^{3}$ - */ - Point3 operator * ( const Point3 & v ) const - { - Point3 t; + /** Operatore per il prodotto matrice-vettore. + @param v A point in $R^{3}$ + @return Il vettore risultante in $R^{3}$ + */ + Point3 operator * ( const Point3 & v ) const + { + Point3 t; - t[0] = a[0]*v[0] + a[1]*v[1] + a[2]*v[2]; - t[1] = a[3]*v[0] + a[4]*v[1] + a[5]*v[2]; - t[2] = a[6]*v[0] + a[7]*v[1] + a[8]*v[2]; - return t; - } + t[0] = a[0]*v[0] + a[1]*v[1] + a[2]*v[2]; + t[1] = a[3]*v[0] + a[4]*v[1] + a[5]*v[2]; + t[2] = a[6]*v[0] + a[7]*v[1] + a[8]*v[2]; + return t; + } - void OuterProduct(Point3 const &p0, Point3 const &p1) { - Point3 row; - row = p1*p0[0]; - a[0] = row[0];a[1] = row[1];a[2] = row[2]; - row = p1*p0[1]; - a[3] = row[0]; a[4] = row[1]; a[5] = row[2]; - row = p1*p0[2]; - a[6] = row[0];a[7] = row[1];a[8] = row[2]; - } + void OuterProduct(Point3 const &p0, Point3 const &p1) { + Point3 row; + row = p1*p0[0]; + a[0] = row[0];a[1] = row[1];a[2] = row[2]; + row = p1*p0[1]; + a[3] = row[0]; a[4] = row[1]; a[5] = row[2]; + row = p1*p0[2]; + a[6] = row[0];a[7] = row[1];a[8] = row[2]; + } - Matrix33 & SetZero() { - for(int i=0;i<9;++i) a[i] =0; - return (*this); - } - Matrix33 & SetIdentity() { - for(int i=0;i<9;++i) a[i] =0; - a[0]=a[4]=a[8]=1.0; - return (*this); - } + Matrix33 & SetZero() { + for(int i=0;i<9;++i) a[i] =0; + return (*this); + } + Matrix33 & SetIdentity() { + for(int i=0;i<9;++i) a[i] =0; + a[0]=a[4]=a[8]=1.0; + return (*this); + } - Matrix33 & SetRotateRad(S angle, const Point3 & axis ) - { - S c = cos(angle); - S s = sin(angle); - S q = 1-c; - Point3 t = axis; - t.Normalize(); - a[0] = t[0]*t[0]*q + c; - a[1] = t[0]*t[1]*q - t[2]*s; - a[2] = t[0]*t[2]*q + t[1]*s; - a[3] = t[1]*t[0]*q + t[2]*s; - a[4] = t[1]*t[1]*q + c; - a[5] = t[1]*t[2]*q - t[0]*s; - a[6] = t[2]*t[0]*q -t[1]*s; - a[7] = t[2]*t[1]*q +t[0]*s; - a[8] = t[2]*t[2]*q +c; - return (*this); - } - Matrix33 & SetRotateDeg(S angle, const Point3 & axis ){ - return SetRotateRad(math::ToRad(angle),axis); - } + Matrix33 & SetRotateRad(S angle, const Point3 & axis ) + { + S c = cos(angle); + S s = sin(angle); + S q = 1-c; + Point3 t = axis; + t.Normalize(); + a[0] = t[0]*t[0]*q + c; + a[1] = t[0]*t[1]*q - t[2]*s; + a[2] = t[0]*t[2]*q + t[1]*s; + a[3] = t[1]*t[0]*q + t[2]*s; + a[4] = t[1]*t[1]*q + c; + a[5] = t[1]*t[2]*q - t[0]*s; + a[6] = t[2]*t[0]*q -t[1]*s; + a[7] = t[2]*t[1]*q +t[0]*s; + a[8] = t[2]*t[2]*q +c; + return (*this); + } + Matrix33 & SetRotateDeg(S angle, const Point3 & axis ){ + return SetRotateRad(math::ToRad(angle),axis); + } - /// Funzione per eseguire la trasposta della matrice - Matrix33 & Transpose() - { - math::Swap(a[1],a[3]); - math::Swap(a[2],a[6]); - math::Swap(a[5],a[7]); - return *this; - } + /// Funzione per eseguire la trasposta della matrice + Matrix33 & Transpose() + { + math::Swap(a[1],a[3]); + math::Swap(a[2],a[6]); + math::Swap(a[5],a[7]); + return *this; + } - // for the transistion to eigen - Matrix33 transpose() const - { - Matrix33 res = *this; - res.Transpose(); - return res; - } + // for the transistion to eigen + Matrix33 transpose() const + { + Matrix33 res = *this; + res.Transpose(); + return res; + } - void transposeInPlace() { this->Transpose(); } + void transposeInPlace() { this->Transpose(); } - /// Funzione per costruire una matrice diagonale dati i tre elem. - Matrix33 & SetDiagonal(S *v) - {int i,j; - for(i=0;i<3;i++) + /// Funzione per costruire una matrice diagonale dati i tre elem. + Matrix33 & SetDiagonal(S *v) + {int i,j; + for(i=0;i<3;i++) for(j=0;j<3;j++) if(i==j) (*this)[i][j] = v[i]; else (*this)[i][j] = 0; return *this; - } + } - /// Assegna l'n-simo vettore colonna - void SetColumn(const int n, S* v){ - assert( (n>=0) && (n<3) ); - a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2]; - }; + /// Assegna l'n-simo vettore colonna + void SetColumn(const int n, S* v){ + assert( (n>=0) && (n<3) ); + a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2]; + }; - /// Assegna l'n-simo vettore riga - void SetRow(const int n, S* v){ - assert( (n>=0) && (n<3) ); - int m=n*3; - a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2]; - }; + /// Assegna l'n-simo vettore riga + void SetRow(const int n, S* v){ + assert( (n>=0) && (n<3) ); + int m=n*3; + a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2]; + }; - /// Assegna l'n-simo vettore colonna - void SetColumn(const int n, const Point3 v){ - assert( (n>=0) && (n<3) ); - a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2]; - }; + /// Assegna l'n-simo vettore colonna + void SetColumn(const int n, const Point3 v){ + assert( (n>=0) && (n<3) ); + a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2]; + }; - /// Assegna l'n-simo vettore riga - void SetRow(const int n, const Point3 v){ - assert( (n>=0) && (n<3) ); - int m=n*3; - a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2]; - }; + /// Assegna l'n-simo vettore riga + void SetRow(const int n, const Point3 v){ + assert( (n>=0) && (n<3) ); + int m=n*3; + a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2]; + }; - /// Restituisce l'n-simo vettore colonna - Point3 GetColumn(const int n) const { - assert( (n>=0) && (n<3) ); - Point3 t; - t[0]=a[n]; t[1]=a[n+3]; t[2]=a[n+6]; - return t; - }; + /// Restituisce l'n-simo vettore colonna + Point3 GetColumn(const int n) const { + assert( (n>=0) && (n<3) ); + Point3 t; + t[0]=a[n]; t[1]=a[n+3]; t[2]=a[n+6]; + return t; + }; - /// Restituisce l'n-simo vettore riga - Point3 GetRow(const int n) const { - assert( (n>=0) && (n<3) ); - Point3 t; - int m=n*3; - t[0]=a[m]; t[1]=a[m+1]; t[2]=a[m+2]; - return t; - }; + /// Restituisce l'n-simo vettore riga + Point3 GetRow(const int n) const { + assert( (n>=0) && (n<3) ); + Point3 t; + int m=n*3; + t[0]=a[m]; t[1]=a[m+1]; t[2]=a[m+2]; + return t; + }; - /// Funzione per il calcolo del determinante - S Determinant() const - { - return a[0]*(a[4]*a[8]-a[5]*a[7]) - - a[1]*(a[3]*a[8]-a[5]*a[6]) + - a[2]*(a[3]*a[7]-a[4]*a[6]) ; - } + /// Funzione per il calcolo del determinante + S Determinant() const + { + return a[0]*(a[4]*a[8]-a[5]*a[7]) - + a[1]*(a[3]*a[8]-a[5]*a[6]) + + a[2]*(a[3]*a[7]-a[4]*a[6]) ; + } // Warning, this Inversion code can be HIGHLY NUMERICALLY UNSTABLE! // In most case you are advised to use the Invert() method based on SVD decomposition. - Matrix33 & FastInvert() - { - // Maple produsse: - S t4 = a[0]*a[4]; - S t6 = a[0]*a[5]; - S t8 = a[1]*a[3]; - S t10 = a[2]*a[3]; - S t12 = a[1]*a[6]; - S t14 = a[2]*a[6]; - S t17 = 1/(t4*a[8]-t6*a[7]-t8*a[8]+t10*a[7]+t12*a[5]-t14*a[4]); - S a0 = a[0]; - S a1 = a[1]; - S a3 = a[3]; - S a4 = a[4]; - a[0] = (a[4]*a[8]-a[5]*a[7])*t17; - a[1] = -(a[1]*a[8]-a[2]*a[7])*t17; - a[2] = (a1 *a[5]-a[2]*a[4])*t17; - a[3] = -(a[3]*a[8]-a[5]*a[6])*t17; - a[4] = (a0 *a[8]-t14 )*t17; - a[5] = -(t6 - t10)*t17; - a[6] = (a3 *a[7]-a[4]*a[6])*t17; - a[7] = -(a[0]*a[7]-t12)*t17; - a[8] = (t4-t8)*t17; + Matrix33 & FastInvert() + { + // Maple produsse: + S t4 = a[0]*a[4]; + S t6 = a[0]*a[5]; + S t8 = a[1]*a[3]; + S t10 = a[2]*a[3]; + S t12 = a[1]*a[6]; + S t14 = a[2]*a[6]; + S t17 = 1/(t4*a[8]-t6*a[7]-t8*a[8]+t10*a[7]+t12*a[5]-t14*a[4]); + S a0 = a[0]; + S a1 = a[1]; + S a3 = a[3]; + S a4 = a[4]; + a[0] = (a[4]*a[8]-a[5]*a[7])*t17; + a[1] = -(a[1]*a[8]-a[2]*a[7])*t17; + a[2] = (a1 *a[5]-a[2]*a[4])*t17; + a[3] = -(a[3]*a[8]-a[5]*a[6])*t17; + a[4] = (a0 *a[8]-t14 )*t17; + a[5] = -(t6 - t10)*t17; + a[6] = (a3 *a[7]-a[4]*a[6])*t17; + a[7] = -(a[0]*a[7]-t12)*t17; + a[8] = (t4-t8)*t17; - return *this; - } + return *this; + } void show(FILE * /*fp*/) - { - for(int i=0;i<3;++i) - printf("| %g \t%g \t%g |\n",a[3*i+0],a[3*i+1],a[3*i+2]); - } + { + for(int i=0;i<3;++i) + printf("| %g \t%g \t%g |\n",a[3*i+0],a[3*i+1],a[3*i+2]); + } // return the Trace of the matrix i.e. the sum of the diagonal elements S Trace() const { - return a[0]+a[4]+a[8]; + return a[0]+a[4]+a[8]; } /* @@ -533,20 +533,20 @@ compute the matrix generated by the product of a * b^T */ void ExternalProduct(const Point3 &a, const Point3 &b) { - for(int i=0;i<3;++i) - for(int j=0;j<3;++j) - (*this)[i][j] = a[i]*b[j]; + for(int i=0;i<3;++i) + for(int j=0;j<3;++j) + (*this)[i][j] = a[i]*b[j]; } /* Compute the Frobenius Norm of the Matrix */ ScalarType Norm() { - ScalarType SQsum=0; - for(int i=0;i<3;++i) - for(int j=0;j<3;++j) - SQsum += a[i]*a[i]; - return (math::Sqrt(SQsum)); + ScalarType SQsum=0; + for(int i=0;i<3;++i) + for(int j=0;j<3;++j) + SQsum += a[i]*a[i]; + return (math::Sqrt(SQsum)); } @@ -555,20 +555,20 @@ It compute the covariance matrix of a set of 3d points. Returns the barycenter */ template void Covariance(const STLPOINTCONTAINER &points, Point3 &bp) { - assert(!points.empty()); - typedef typename STLPOINTCONTAINER::const_iterator PointIte; - // first cycle: compute the barycenter - bp.SetZero(); - for( PointIte pi = points.begin(); pi != points.end(); ++pi) bp+= (*pi); - bp/=points.size(); - // second cycle: compute the covariance matrix - this->SetZero(); - vcg::Matrix33 A; - for( PointIte pi = points.begin(); pi != points.end(); ++pi) { - Point3 p = (*pi)-bp; - A.OuterProduct(p,p); - (*this)+= A; - } + assert(!points.empty()); + typedef typename STLPOINTCONTAINER::const_iterator PointIte; + // first cycle: compute the barycenter + bp.SetZero(); + for( PointIte pi = points.begin(); pi != points.end(); ++pi) bp+= (*pi); + bp/=points.size(); + // second cycle: compute the covariance matrix + this->SetZero(); + vcg::Matrix33 A; + for( PointIte pi = points.begin(); pi != points.end(); ++pi) { + Point3 p = (*pi)-bp; + A.OuterProduct(p,p); + (*this)+= A; + } } @@ -585,194 +585,194 @@ IEEE TPAMI Vol 14, No 2 1992 */ template void CrossCovariance(const STLPOINTCONTAINER &P, const STLPOINTCONTAINER &X, - Point3 &bp, Point3 &bx) + Point3 &bp, Point3 &bx) { - SetZero(); - assert(P.size()==X.size()); - bx.SetZero(); - bp.SetZero(); - Matrix33 tmp; - typename std::vector >::const_iterator pi,xi; - for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){ - bp+=*pi; - bx+=*xi; - tmp.ExternalProduct(*pi,*xi); - (*this)+=tmp; - } - bp/=P.size(); - bx/=X.size(); - (*this)/=P.size(); - tmp.ExternalProduct(bp,bx); - (*this)-=tmp; + SetZero(); + assert(P.size()==X.size()); + bx.SetZero(); + bp.SetZero(); + Matrix33 tmp; + typename std::vector >::const_iterator pi,xi; + for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){ + bp+=*pi; + bx+=*xi; + tmp.ExternalProduct(*pi,*xi); + (*this)+=tmp; + } + bp/=P.size(); + bx/=X.size(); + (*this)/=P.size(); + tmp.ExternalProduct(bp,bx); + (*this)-=tmp; } template void WeightedCrossCovariance(const STLREALCONTAINER & weights, - const STLPOINTCONTAINER &P, - const STLPOINTCONTAINER &X, - Point3 &bp, - Point3 &bx) + const STLPOINTCONTAINER &P, + const STLPOINTCONTAINER &X, + Point3 &bp, + Point3 &bx) { - SetZero(); - assert(P.size()==X.size()); - bx.SetZero(); - bp.SetZero(); - Matrix33 tmp; - typename std::vector >::const_iterator pi,xi; - typename STLREALCONTAINER::const_iterator pw; + SetZero(); + assert(P.size()==X.size()); + bx.SetZero(); + bp.SetZero(); + Matrix33 tmp; + typename std::vector >::const_iterator pi,xi; + typename STLREALCONTAINER::const_iterator pw; - for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){ - bp+=(*pi); - bx+=(*xi); - } - bp/=P.size(); - bx/=X.size(); + for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){ + bp+=(*pi); + bx+=(*xi); + } + bp/=P.size(); + bx/=X.size(); - for(pi=P.begin(),xi=X.begin(),pw = weights.begin();pi!=P.end();++pi,++xi,++pw){ + for(pi=P.begin(),xi=X.begin(),pw = weights.begin();pi!=P.end();++pi,++xi,++pw){ - tmp.ExternalProduct(((*pi)-(bp)),((*xi)-(bp))); + tmp.ExternalProduct(((*pi)-(bp)),((*xi)-(bp))); - (*this)+=tmp*(*pw); - } + (*this)+=tmp*(*pw); + } } private: - S a[9]; + S a[9]; }; -///return the tranformation matrix to transform +///return the tranformation matrix to transform ///to the frame specified by the three vectors template vcg::Matrix33 TransformationMatrix(const vcg::Point3 dirX, - const vcg::Point3 dirY, - const vcg::Point3 dirZ) + const vcg::Point3 dirY, + const vcg::Point3 dirZ) { - vcg::Matrix33 Trans; + vcg::Matrix33 Trans; - ///it must have right orientation cause of normal - Trans[0][0]=dirX[0]; - Trans[0][1]=dirX[1]; - Trans[0][2]=dirX[2]; - Trans[1][0]=dirY[0]; - Trans[1][1]=dirY[1]; - Trans[1][2]=dirY[2]; - Trans[2][0]=dirZ[0]; - Trans[2][1]=dirZ[1]; - Trans[2][2]=dirZ[2]; + ///it must have right orientation cause of normal + Trans[0][0]=dirX[0]; + Trans[0][1]=dirX[1]; + Trans[0][2]=dirX[2]; + Trans[1][0]=dirY[0]; + Trans[1][1]=dirY[1]; + Trans[1][2]=dirY[2]; + Trans[2][0]=dirZ[0]; + Trans[2][1]=dirZ[1]; + Trans[2][2]=dirZ[2]; - /////then find the inverse - return (Trans); + /////then find the inverse + return (Trans); } template void Invert(Matrix33 &m) - { - Matrix33 v; - Point3::ScalarType> e; - SingularValueDecomposition(m,&e[0],v); - e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2]; - m.Transpose(); - m = v * Matrix33Diag(e) * m; - } + { + Matrix33 v; + Point3::ScalarType> e; + SingularValueDecomposition(m,&e[0],v); + e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2]; + m.Transpose(); + m = v * Matrix33Diag(e) * m; + } template Matrix33 Inverse(const Matrix33&m) - { - Matrix33 v,m_copy=m; - Point3 e; - SingularValueDecomposition(m_copy,&e[0],v); - m_copy.Transpose(); - e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2]; - return v * Matrix33Diag(e) * m_copy; - } + { + Matrix33 v,m_copy=m; + Point3 e; + SingularValueDecomposition(m_copy,&e[0],v); + m_copy.Transpose(); + e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2]; + return v * Matrix33Diag(e) * m_copy; + } ///given 2 vector centered into origin calculate the rotation matrix from first to the second template Matrix33 RotationMatrix(vcg::Point3 v0,vcg::Point3 v1,bool normalized=true) - { - typedef typename vcg::Point3 CoordType; - Matrix33 rotM; - const S epsilon=0.00001; - if (!normalized) - { - v0.Normalize(); - v1.Normalize(); - } - S dot=(v0*v1); - ///control if there is no rotation - if (dot>((S)1-epsilon)) - { - rotM.SetIdentity(); - return rotM; - } + { + typedef typename vcg::Point3 CoordType; + Matrix33 rotM; + const S epsilon=0.00001; + if (!normalized) + { + v0.Normalize(); + v1.Normalize(); + } + S dot=(v0*v1); + ///control if there is no rotation + if (dot>((S)1-epsilon)) + { + rotM.SetIdentity(); + return rotM; + } - ///find the axis of rotation - CoordType axis; - axis=v0^v1; - axis.Normalize(); + ///find the axis of rotation + CoordType axis; + axis=v0^v1; + axis.Normalize(); - ///construct rotation matrix - S u=axis.X(); - S v=axis.Y(); - S w=axis.Z(); - S phi=acos(dot); - S rcos = cos(phi); - S rsin = sin(phi); + ///construct rotation matrix + S u=axis.X(); + S v=axis.Y(); + S w=axis.Z(); + S phi=acos(dot); + S rcos = cos(phi); + S rsin = sin(phi); - rotM[0][0] = rcos + u*u*(1-rcos); - rotM[1][0] = w * rsin + v*u*(1-rcos); - rotM[2][0] = -v * rsin + w*u*(1-rcos); - rotM[0][1] = -w * rsin + u*v*(1-rcos); - rotM[1][1] = rcos + v*v*(1-rcos); - rotM[2][1] = u * rsin + w*v*(1-rcos); - rotM[0][2] = v * rsin + u*w*(1-rcos); - rotM[1][2] = -u * rsin + v*w*(1-rcos); - rotM[2][2] = rcos + w*w*(1-rcos); + rotM[0][0] = rcos + u*u*(1-rcos); + rotM[1][0] = w * rsin + v*u*(1-rcos); + rotM[2][0] = -v * rsin + w*u*(1-rcos); + rotM[0][1] = -w * rsin + u*v*(1-rcos); + rotM[1][1] = rcos + v*v*(1-rcos); + rotM[2][1] = u * rsin + w*v*(1-rcos); + rotM[0][2] = v * rsin + u*w*(1-rcos); + rotM[1][2] = -u * rsin + v*w*(1-rcos); + rotM[2][2] = rcos + w*w*(1-rcos); - return rotM; - } + return rotM; + } ///return the rotation matrix along axis template Matrix33 RotationMatrix(const vcg::Point3 &axis, - const float &angleRad) - { - vcg::Matrix44 matr44; - vcg::Matrix33 matr33; - matr44.SetRotate(angleRad,axis); - for (int i=0;i<3;i++) - for (int j=0;j<3;j++) - matr33[i][j]=matr44[i][j]; - return matr33; - } + const float &angleRad) + { + vcg::Matrix44 matr44; + vcg::Matrix33 matr33; + matr44.SetRotate(angleRad,axis); + for (int i=0;i<3;i++) + for (int j=0;j<3;j++) + matr33[i][j]=matr44[i][j]; + return matr33; + } /// return a random rotation matrix, from the paper: /// Fast Random Rotation Matrices, James Arvo /// Graphics Gems III pp. 117-120 template Matrix33 RandomRotation(){ - S x1,x2,x3; - Matrix33 R,H,M,vv; - Point3 v; - R.SetIdentity(); - H.SetIdentity(); - x1 = rand()/S(RAND_MAX); - x2 = rand()/S(RAND_MAX); - x3 = rand()/S(RAND_MAX); + S x1,x2,x3; + Matrix33 R,H,M,vv; + Point3 v; + R.SetIdentity(); + H.SetIdentity(); + x1 = rand()/S(RAND_MAX); + x2 = rand()/S(RAND_MAX); + x3 = rand()/S(RAND_MAX); - R[0][0] = cos(S(2)*M_PI*x1); - R[0][1] = sin(S(2)*M_PI*x1); - R[1][0] = - R[0][1]; - R[1][1] = R[0][0]; + R[0][0] = cos(S(2)*M_PI*x1); + R[0][1] = sin(S(2)*M_PI*x1); + R[1][0] = - R[0][1]; + R[1][1] = R[0][0]; - v[0] = cos(2.0 * M_PI * x2)*sqrt(x3); - v[1] = sin(2.0 * M_PI * x2)*sqrt(x3); - v[2] = sqrt(1-x3); + v[0] = cos(2.0 * M_PI * x2)*sqrt(x3); + v[1] = sin(2.0 * M_PI * x2)*sqrt(x3); + v[2] = sqrt(1-x3); - vv.OuterProduct(v,v); - H -= vv*S(2); - M = H*R*S(-1); - return M; + vv.OuterProduct(v,v); + H -= vv*S(2); + M = H*R*S(-1); + return M; } ///