diff --git a/vcg/math/matrix33.h b/vcg/math/matrix33.h new file mode 100644 index 00000000..7a6d38b0 --- /dev/null +++ b/vcg/math/matrix33.h @@ -0,0 +1,405 @@ +/**************************************************************************** +* VCGLib o o * +* Visual and Computer Graphics Library o o * +* _ O _ * +* Copyright(C) 2004 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ +/**************************************************************************** + History + +$Log: not supported by cvs2svn $ +Revision 1.1 2004/05/28 13:00:39 ganovelli +created + + +****************************************************************************/ + + +#ifndef __VCGLIB_MATRIX33_H +#define __VCGLIB_MATRIX33_H + +#include +#include +#include + +namespace vcg { + +template +/** @name Matrix33 + Class Matrix33. + This is the class for definition of a matrix 3x3. + @param S (Templete Parameter) Specifies the ScalarType field. +*/ +class Matrix33 +{ +public: + + /// Default constructor + inline Matrix33() {} + + /// 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]; + } + + /// 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; + } + + + /// 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 sottrazione per matrici 3x3 + Matrix33 & operator -= ( const Matrix33 &m ) + { + for(int i=0;i<9;++i) + a[i] -= m.a[i]; + 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; + + 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; + } + + /// 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 ) + { + Matrix33 r; + for(int i=0;i<9;++i) + r.a[i] = a[i]* t; + + return r; + } + + /// Operatore sottrazione per matrici 3x3 + Matrix33 operator - ( const Matrix33 &m ) + { + Matrix33 r; + for(int i=0;i<9;++i) + r.a[i] = a[i] - m.a[i]; + + 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; + + 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 Zero() { + for(int i=0;i<9;++i) a[i] =0; + } + void Identity() { + for(int i=0;i<9;++i) a[i] =0; + a[0]=a[4]=a[8]=1.0; + } + + void Rotate(S angle, const Point3 & axis ) + { + angle = angle*3.14159265358979323846/180; + double c = cos(angle); + double s = sin(angle); + double 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; + } + /// Funzione per eseguire la trasposta della matrice + Matrix33 & Trasp() + { + swap(a[1],a[3]); + swap(a[2],a[6]); + swap(a[5],a[7]); + return *this; + } + + /// Funzione per costruire una matrice diagonale dati i tre elem. + Matrix33 & SetDiag(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 SetCol(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 colonna + void SetCol(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]; + }; + + /// Restituisce l'n-simo vettore colonna + Point3 GetCol(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; + }; + + + + /// Funzione per il calcolo del determinante + S Det() 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]) ; + } + + Matrix33 & invert() + { + // 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; + } + + 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]); + } + +// return the Trace of the matrix i.e. the sum of the diagonal elements +S Trace() const +{ + return a[0]+a[4]+a[8]; +} + +/* +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]; +} + +/* +It compute the cross covariance matrix of two set of 3d points P and X; +it returns also the barycenters of P and X. +fonte: + +Besl, McKay +A method for registration o f 3d Shapes +IEEE TPAMI Vol 14, No 2 1992 + +*/ +template +void CrossCovariance(const STLPOINTCONTAINER &P, const STLPOINTCONTAINER &X, + Point3 &bp, Point3 &bx) +{ + Zero(); + assert(P.size()==X.size()); + bx.Zero(); + bp.Zero(); + 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) +{ + Zero(); + assert(P.size()==X.size()); + bx.Zero(); + bp.Zero(); + 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(),pw = weights.begin();pi!=P.end();++pi,++xi,++pw){ + + tmp.ExternalProduct(((*pi)-(bp)),((*xi)-(bp))); + + (*this)+=tmp*(*pw); + } +} + +private: + S a[9]; +}; + + +/// +typedef Matrix33 Matrix33s; +typedef Matrix33 Matrix33i; +typedef Matrix33 Matrix33f; +typedef Matrix33 Matrix33d; + +} // end of namespace + +#endif