diff --git a/apps/test/extractors/extractor/ImplicitSphere.h b/apps/test/extractors/extractor/ImplicitSphere.h index f8435e32..ba84be8c 100644 --- a/apps/test/extractors/extractor/ImplicitSphere.h +++ b/apps/test/extractors/extractor/ImplicitSphere.h @@ -6,7 +6,7 @@ class ImplicitSphere public: ImplicitSphere() { - _center.Zero(); + _center.SetZero(); _radius = _sqr_radius = 0.0; }; diff --git a/vcg/complex/local_optimization/tetra_edge_collapse.h b/vcg/complex/local_optimization/tetra_edge_collapse.h index a35b3855..abc0e1de 100644 --- a/vcg/complex/local_optimization/tetra_edge_collapse.h +++ b/vcg/complex/local_optimization/tetra_edge_collapse.h @@ -144,7 +144,7 @@ ScalarType _VolumePreservingError(PosType &pos,CoordType &new_point,int nsteps) else if ((!ext_v0)&&(!ext_v1)) {/*CoordType g; - g.Zero(); + g.SetZero(); g+=ve0->cP(); g+=ve1->cP(); g/=2;*/ @@ -160,7 +160,7 @@ ScalarType _VolumePreservingError(PosType &pos,CoordType &new_point,int nsteps) best_error=1000000.f; ScalarType alfatemp=step*((ScalarType)i); //CoordType g; - // g.Zero(); + // g.SetZero(); //g+=ve0->cP()*alfatemp; //g+=ve1->cP()*(1-alfatemp); //CoordType newPTemp=g; diff --git a/vcg/complex/local_optimization/tri_edge_collapse_quadric.h b/vcg/complex/local_optimization/tri_edge_collapse_quadric.h index d750edf1..f333a91b 100644 --- a/vcg/complex/local_optimization/tri_edge_collapse_quadric.h +++ b/vcg/complex/local_optimization/tri_edge_collapse_quadric.h @@ -526,7 +526,7 @@ static void InitQuadric(TriMeshType &m) // m.ClearFlags(); for(pv=m.vert.begin();pv!=m.vert.end();++pv) // Azzero le quadriche if( ! (*pv).IsD() && (*pv).IsW()) - QH::Qd(*pv).Zero(); + QH::Qd(*pv).SetZero(); for(pf=m.face.begin();pf!=m.face.end();++pf) diff --git a/vcg/complex/trimesh/create/extended_marching_cubes.h b/vcg/complex/trimesh/create/extended_marching_cubes.h index 2f9df7a8..e8924196 100644 --- a/vcg/complex/trimesh/create/extended_marching_cubes.h +++ b/vcg/complex/trimesh/create/extended_marching_cubes.h @@ -379,7 +379,7 @@ namespace vcg VertexPointer mean_point = &*AllocatorType::AddVertices( *_mesh, 1); mean_point->SetUserBit(_featureFlag); mean_point->P() = point; - mean_point->N().Zero(); + mean_point->N().SetZero(); delete []x; delete []points; delete []normals; diff --git a/vcg/complex/trimesh/inertia.h b/vcg/complex/trimesh/inertia.h index 6ddebd33..4907cee7 100644 --- a/vcg/complex/trimesh/inertia.h +++ b/vcg/complex/trimesh/inertia.h @@ -313,7 +313,7 @@ static void Covariance(const MeshType & m, vcg::Point3 & bary, vcg:: ConstFaceIterator fi; ScalarType area = 0.0; - bary.Zero(); + bary.SetZero(); for(fi = m.face.begin(); fi != m.face.end(); ++fi) if(!(*fi).IsD()) { diff --git a/vcg/complex/trimesh/textcoord_optimization.h b/vcg/complex/trimesh/textcoord_optimization.h index a9f013b4..b0cc89d0 100644 --- a/vcg/complex/trimesh/textcoord_optimization.h +++ b/vcg/complex/trimesh/textcoord_optimization.h @@ -240,7 +240,7 @@ public: #define v1 (f->V1(i)->T().P()) #define v2 (f->V2(i)->T().P()) for (VertexIterator v=Super::m.vert.begin(); v!=Super::m.vert.end(); v++) { - sum[v].Zero(); + sum[v].SetZero(); } ScalarType tot_proj_area=0; @@ -424,7 +424,7 @@ void SmoothTextureCoords(MESH_TYPE &m){ sum.Start(); for (typename MESH_TYPE::VertexIterator v=m.vert.begin(); v!=m.vert.end(); v++) { - sum[v].Zero(); + sum[v].SetZero(); div[v]=0; } diff --git a/vcg/complex/trimesh/update/curvature.h b/vcg/complex/trimesh/update/curvature.h index ce52575a..d1ceb397 100644 --- a/vcg/complex/trimesh/update/curvature.h +++ b/vcg/complex/trimesh/update/curvature.h @@ -278,10 +278,7 @@ public: S[0][1] = s; S[1][0] = -1.0f * s; - vcg::ndim::MatrixMNf St (S); - St.Transpose(); - - vcg::ndim::MatrixMNf StMS(St * minor2x2 * S); + vcg::ndim::MatrixMNf StMS(S.transpose() * minor2x2 * S); // compute curvatures and curvature directions float Principal_Curvature1 = (3.0f * StMS[0][0]) - StMS[1][1]; diff --git a/vcg/math/deprecated_matrix.h b/vcg/math/deprecated_matrix.h new file mode 100644 index 00000000..2113cdd9 --- /dev/null +++ b/vcg/math/deprecated_matrix.h @@ -0,0 +1,785 @@ +/**************************************************************************** +* 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. * +* * +****************************************************************************/ +/*************************************************************************** +$Log: not supported by cvs2svn $ +Revision 1.9 2006/09/11 16:11:39 marfr960 +Added const to declarations of the overloaded (operators *). +Otherwise the * operator would always attempt to convert any type of data passed as an argument to Point3 + +Revision 1.8 2006/08/23 15:24:45 marfr960 +Copy constructor : faster memcpy instead of slow 'for' cycle +empty constructor + +Revision 1.7 2006/04/29 10:26:04 fiorin +Added some utility methods (swapping of columns and rows, matrix-vector multiplication) + +Revision 1.6 2006/04/11 08:09:35 zifnab1974 +changes necessary for gcc 3.4.5 on linux 64bit. Please take note of case-sensitivity of filenames + +Revision 1.5 2005/12/12 11:25:00 ganovelli +added diagonal matrix, outer produce and namespace + +***************************************************************************/ + +#ifndef MATRIX_VCGLIB +#define MATRIX_VCGLIB + +#include +#include +#include +#include +#include +#include +#include + +namespace vcg{ + namespace ndim{ + + /** \addtogroup math */ + /* @{ */ + + /*! + * This class represent a diagonal m�m matrix. + */ + + class MatrixDiagBase{public: + virtual const int & Dimension()const =0; + virtual const float operator[](const int & i)const = 0; + }; + template + class MatrixDiag: public Point, public MatrixDiagBase{ + public: + const int & Dimension() const {return N;} + MatrixDiag(const Point&p):Point(p){} + }; + +/*! + * This class represent a generic m�n matrix. The class is templated over the scalar type field. + * @param TYPE (Templete Parameter) Specifies the ScalarType field. + */ + template + class Matrix + { + + public: + typedef TYPE ScalarType; + + /*! + * Default constructor + * All the elements are initialized to zero. + * \param m the number of matrix rows + * \param n the number of matrix columns + */ + Matrix(unsigned int m, unsigned int n) + { + _rows = m; + _columns = n; + _data = new ScalarType[m*n]; + memset(_data, 0, m*n*sizeof(ScalarType)); + }; + + /*! + * Constructor + * The matrix elements are initialized with the values of the elements in \i values. + * \param m the number of matrix rows + * \param n the number of matrix columns + * \param values the values of the matrix elements + */ + Matrix(unsigned int m, unsigned int n, TYPE *values) + { + _rows = m; + _columns = n; + unsigned int dim = m*n; + _data = new ScalarType[dim]; + memcpy(_data, values, dim*sizeof(ScalarType)); + //unsigned int i; + //for (i=0; i<_rows*_columns; i++) + // _data[i] = values[i]; + }; + + /*! + * Empty constructor + * Just create the object + */ + Matrix() + { + _rows = 0; + _columns = 0; + _data = NULL; + }; + + /*! + * Copy constructor + * The matrix elements are initialized with the value of the corresponding element in \i m + * \param m the matrix to be copied + */ + Matrix(const Matrix &m) + { + _rows = m._rows; + _columns = m._columns; + _data = new ScalarType[_rows*_columns]; + + unsigned int dim = _rows * _columns; + memcpy(_data, m._data, dim * sizeof(ScalarType)); + +// for (unsigned int i=0; i<_rows*_columns; i++) +// _data[i] = m._data[i]; + }; + + /*! + * Default destructor + */ + ~Matrix() + { + delete []_data; + }; + + /*! + * Number of columns + */ + inline unsigned int ColumnsNumber() const + { + return _columns; + }; + + + /*! + * Number of rows + */ + inline unsigned int RowsNumber() const + { + return _rows; + }; + + /*! + * Equality operator. + * \param m + * \return true iff the matrices have same size and its elements have same values. + */ + bool operator==(const Matrix &m) const + { + if (_rows==m._rows && _columns==m._columns) + { + bool result = true; + for (unsigned int i=0; i<_rows*_columns && result; i++) + result = (_data[i]==m._data[i]); + return result; + } + return false; + }; + + /*! + * Inequality operator + * \param m + * \return true iff the matrices have different size or if their elements have different values. + */ + bool operator!=(const Matrix &m) const + { + if (_rows==m._rows && _columns==m._columns) + { + bool result = false; + for (unsigned int i=0; i<_rows*_columns && !result; i++) + result = (_data[i]!=m._data[i]); + return result; + } + return true; + }; + + /*! + * Return the element stored in the i-th rows at the j-th column + * \param i the row index + * \param j the column index + * \return the element + */ + inline TYPE ElementAt(unsigned int i, unsigned int j) + { + assert(i>=0 && i<_rows); + assert(j>=0 && j<_columns); + return _data[i*_columns+j]; + }; + + /*! + * Calculate and return the matrix determinant (Laplace) + * \return the matrix determinant + */ + TYPE Determinant() const + { + assert(_rows == _columns); + switch (_rows) + { + case 2: + { + return _data[0]*_data[3]-_data[1]*_data[2]; + break; + }; + case 3: + { + return _data[0]*(_data[4]*_data[8]-_data[5]*_data[7]) - + _data[1]*(_data[3]*_data[8]-_data[5]*_data[6]) + + _data[2]*(_data[3]*_data[7]-_data[4]*_data[6]) ; + break; + }; + default: + { + // da migliorare: si puo' cercare la riga/colonna con maggior numero di zeri + ScalarType det = 0; + for (unsigned int j=0; j<_columns; j++) + if (_data[j]!=0) + det += _data[j]*this->Cofactor(0, j); + + return det; + } + }; + }; + + /*! + * Return the cofactor Ai,j of the ai,j element + * \return ... + */ + TYPE Cofactor(unsigned int i, unsigned int j) const + { + assert(_rows == _columns); + assert(_rows>2); + TYPE *values = new TYPE[(_rows-1)*(_columns-1)]; + unsigned int u, v, p, q, s, t; + for (u=0, p=0, s=0, t=0; u<_rows; u++, t+=_rows) + { + if (i==u) + continue; + + for (v=0, q=0; v<_columns; v++) + { + if (j==v) + continue; + values[s+q] = _data[t+v]; + q++; + } + p++; + s+=(_rows-1); + } + Matrix temp(_rows-1, _columns-1, values); + return (pow(-1, i+j)*temp.Determinant()); + }; + + /*! + * Subscript operator: + * \param i the index of the row + * \return a reference to the i-th matrix row + */ + inline TYPE* operator[](const unsigned int i) + { + assert(i>=0 && i<_rows); + return _data + i*_columns; + }; + + /*! + * Const subscript operator + * \param i the index of the row + * \return a reference to the i-th matrix row + */ + inline const TYPE* operator[](const unsigned int i) const + { + assert(i>=0 && i<_rows); + return _data + i*_columns; + }; + + /*! + * Get the j-th column on the matrix. + * \param j the column index. + * \return the reference to the column elements. This pointer must be deallocated by the caller. + */ + TYPE* GetColumn(const unsigned int j) + { + assert(j>=0 && j<_columns); + ScalarType *v = new ScalarType[_columns]; + unsigned int i, p; + for (i=0, p=j; i<_rows; i++, p+=_columns) + v[i] = _data[p]; + return v; + }; + + /*! + * Get the i-th row on the matrix. + * \param i the column index. + * \return the reference to the row elements. This pointer must be deallocated by the caller. + */ + TYPE* GetRow(const unsigned int i) + { + assert(i>=0 && i<_rows); + ScalarType *v = new ScalarType[_rows]; + unsigned int j, p; + for (j=0, p=i*_columns; j<_columns; j++, p++) + v[j] = _data[p]; + return v; + }; + + /*! + * Swaps the values of the elements between the i-th and the j-th column. + * \param i the index of the first column + * \param j the index of the second column + */ + void SwapColumns(const unsigned int i, const unsigned int j) + { + assert(0<=i && i<_columns); + assert(0<=j && j<_columns); + if (i==j) + return; + + unsigned int r, e0, e1; + for (r=0, e0=i, e1=j; r<_rows; r++, e0+=_columns, e1+=_columns) + std::swap(_data[e0], _data[e1]); + }; + + /*! + * Swaps the values of the elements between the i-th and the j-th row. + * \param i the index of the first row + * \param j the index of the second row + */ + void SwapRows(const unsigned int i, const unsigned int j) + { + assert(0<=i && i<_rows); + assert(0<=j && j<_rows); + if (i==j) + return; + + unsigned int r, e0, e1; + for (r=0, e0=i*_columns, e1=j*_columns; r<_columns; r++, e0++, e1++) + std::swap(_data[e0], _data[e1]); + }; + + /*! + * Assignment operator + * \param m ... + */ + Matrix& operator=(const Matrix &m) + { + if (this != &m) + { + assert(_rows == m._rows); + assert(_columns == m._columns); + for (unsigned int i=0; i<_rows*_columns; i++) + _data[i] = m._data[i]; + } + return *this; + }; + + /*! + * Adds a matrix m to this matrix. + * \param m reference to matrix to add to this + * \return the matrix sum. + */ + Matrix& operator+=(const Matrix &m) + { + assert(_rows == m._rows); + assert(_columns == m._columns); + for (unsigned int i=0; i<_rows*_columns; i++) + _data[i] += m._data[i]; + return *this; + }; + + /*! + * Subtracts a matrix m to this matrix. + * \param m reference to matrix to subtract + * \return the matrix difference. + */ + Matrix& operator-=(const Matrix &m) + { + assert(_rows == m._rows); + assert(_columns == m._columns); + for (unsigned int i=0; i<_rows*_columns; i++) + _data[i] -= m._data[i]; + return *this; + }; + + /*! + * (Modifier) Add to each element of this matrix the scalar constant k. + * \param k the scalar constant + * \return the modified matrix + */ + Matrix& operator+=(const TYPE k) + { + for (unsigned int i=0; i<_rows*_columns; i++) + _data[i] += k; + return *this; + }; + + /*! + * (Modifier) Subtract from each element of this matrix the scalar constant k. + * \param k the scalar constant + * \return the modified matrix + */ + Matrix& operator-=(const TYPE k) + { + for (unsigned int i=0; i<_rows*_columns; i++) + _data[i] -= k; + return *this; + }; + + /*! + * (Modifier) Multiplies each element of this matrix by the scalar constant k. + * \param k the scalar constant + * \return the modified matrix + */ + Matrix& operator*=(const TYPE k) + { + for (unsigned int i=0; i<_rows*_columns; i++) + _data[i] *= k; + return *this; + }; + + /*! + * (Modifier) Divides each element of this matrix by the scalar constant k. + * \param k the scalar constant + * \return the modified matrix + */ + Matrix& operator/=(const TYPE k) + { + assert(k!=0); + for (unsigned int i=0; i<_rows*_columns; i++) + _data[i] /= k; + return *this; + }; + + /*! + * Matrix multiplication: calculates the cross product. + * \param m reference to the matrix to multiply by + * \return the matrix product + */ + Matrix operator*(const Matrix &m) const + { + assert(_columns == m._rows); + Matrix result(_rows, m._columns); + unsigned int i, j, k, p, q, r; + for (i=0, p=0, r=0; i + void DotProduct(Point &m,Point &result) + { + unsigned int i, j, p, r; + for (i=0, p=0, r=0; i operator*(const MatrixDiagBase &m) const + { + assert(_columns == _rows); + assert(_columns == m.Dimension()); + int i,j; + Matrix result(_rows, _columns); + + for (i=0; i + void OuterProduct(const Point a, const Point< M,TYPE> b) + { + assert(N == _rows); + assert(M == _columns); + Matrix result(_rows,_columns); + unsigned int i, j; + + for (i=0; i operator*(Point3 &p) const + { + assert(_columns==3 && _rows==3); + vcg::Point3 result; + result[0] = _data[0]*p[0]+_data[1]*p[1]+_data[2]*p[2]; + result[1] = _data[3]*p[0]+_data[4]*p[1]+_data[5]*p[2]; + result[2] = _data[6]*p[0]+_data[7]*p[1]+_data[8]*p[2]; + return result; + }; + + + /*! + * Scalar sum. + * \param k + * \return the resultant matrix + */ + Matrix operator+(const TYPE k) + { + Matrix result(_rows, _columns); + for (unsigned int i=0; i<_rows*_columns; i++) + result._data[i] = _data[i]+k; + return result; + }; + + /*! + * Scalar difference. + * \param k + * \return the resultant matrix + */ + Matrix operator-(const TYPE k) + { + Matrix result(_rows, _columns); + for (unsigned int i=0; i<_rows*_columns; i++) + result._data[i] = _data[i]-k; + return result; + }; + + /*! + * Negate all matrix elements + * \return the modified matrix + */ + Matrix operator-() const + { + Matrix result(_rows, _columns, _data); + for (unsigned int i=0; i<_columns*_rows; i++) + result._data[i] = -1*_data[i]; + return result; + }; + + /*! + * Scalar multiplication. + * \param k value to multiply every member by + * \return the resultant matrix + */ + Matrix operator*(const TYPE k) const + { + Matrix result(_rows, _columns); + for (unsigned int i=0; i<_rows*_columns; i++) + result._data[i] = _data[i]*k; + return result; + }; + + /*! + * Scalar division. + * \param k value to divide every member by + * \return the resultant matrix + */ + Matrix operator/(const TYPE k) + { + Matrix result(_rows, _columns); + for (unsigned int i=0; i<_rows*_columns; i++) + result._data[i] = _data[i]/k; + return result; + }; + + + /*! + * Set all the matrix elements to zero. + */ + void SetZero() + { + for (unsigned int i=0; i<_rows*_columns; i++) + _data[i] = ScalarType(0.0); + }; + + /*! + * Set the matrix to identity. + */ + void SetIdentity() + { + assert(_rows==_columns); + for (unsigned int i=0; i<_rows; i++) + for (unsigned int j=0; j<_columns; j++) + _data[i] = (i==j) ? ScalarType(1.0) : ScalarType(0.0f); + }; + + /*! + * Set the values of j-th column to v[j] + * \param j the column index + * \param v ... + */ + void SetColumn(const unsigned int j, TYPE* v) + { + assert(j>=0 && j<_columns); + unsigned int i, p; + for (i=0, p=j; i<_rows; i++, p+=_columns) + _data[p] = v[i]; + }; + + /*! + * Set the elements of the i-th row to v[j] + * \param i the row index + * \param v ... + */ + void SetRow(const unsigned int i, TYPE* v) + { + assert(i>=0 && i<_rows); + unsigned int j, p; + for (j=0, p=i*_rows; j<_columns; j++, p++) + _data[p] = v[j]; + }; + + /*! + * Set the diagonal elements vi,i to v[i] + * \param v + */ + void SetDiagonal(TYPE *v) + { + assert(_rows == _columns); + for (unsigned int i=0, p=0; i<_rows; i++, p+=_rows) + _data[p+i] = v[i]; + }; + + /*! + * Resize the current matrix. + * \param m the number of matrix rows. + * \param n the number of matrix columns. + */ + void Resize(const unsigned int m, const unsigned int n) + { + assert(m>=2); + assert(n>=2); + _rows = m; + _columns = n; + delete []_data; + _data = new ScalarType[m*n]; + for (unsigned int i=0; i MatrixMNd; + typedef vcg::ndim::Matrix MatrixMNf; + + /*! @} */ + + template + void Invert(MatrixType & m){ + typedef typename MatrixType::ScalarType X; + X *diag; + diag = new X [m.ColumnsNumber()]; + + MatrixType res(m.RowsNumber(),m.ColumnsNumber()); + vcg::SingularValueDecomposition (m,&diag[0],res,LeaveUnsorted,50 ); + m.Transpose(); + // prodotto per la diagonale + unsigned int i,j; + for (i=0; i &p ) + Matrix33 operator - ( const Matrix33Diag &p ) + Matrix33 operator + ( const Matrix33 &m ) + Matrix33 operator + ( const Matrix33Diag &p ) + +Revision 1.12 2005/11/14 10:28:25 cignoni +Changed Invert -> FastInvert for the function based on the maple expansion + +Revision 1.11 2005/10/13 15:45:23 ponchio +Changed a Zero in SetZero in WeightedCrossCovariance() (again) + +Revision 1.10 2005/10/05 17:06:12 pietroni +corrected sintax error on singular value decomposition + +Revision 1.9 2005/09/29 09:53:58 ganovelli +added inverse by SVD + +Revision 1.8 2005/06/10 14:51:54 cignoni +Changed a Zero in SetZero in WeightedCrossCovariance() + +Revision 1.7 2005/06/10 11:46:49 pietroni +Added Norm Function + +Revision 1.6 2005/06/07 14:29:56 ganovelli +changed from Matrix33Ide to MatrixeeDiag + +Revision 1.5 2005/05/23 15:05:26 ganovelli +Matrix33Diag Added: it implements diagonal matrix. Added only operator += in Matrix33 + +Revision 1.4 2005/04/11 14:11:22 pietroni +changed swap to math::Swap in Traspose Function + +Revision 1.3 2004/10/18 15:03:02 fiorin +Updated interface: all Matrix classes have now the same interface + +Revision 1.2 2004/07/13 06:48:26 cignoni +removed uppercase references in include + +Revision 1.1 2004/05/28 13:09:05 ganovelli +created + +Revision 1.1 2004/05/28 13:00:39 ganovelli +created + + +****************************************************************************/ + + +#ifndef __VCGLIB_MATRIX33_H +#define __VCGLIB_MATRIX33_H + +#include +#include +#include +#include +#include + +namespace vcg { + +template +class Matrix33Diag:public Point3{ +public: + + /** @name Matrix33 + Class Matrix33Diag. + This is the class for definition of a diagonal matrix 3x3. + @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){}; +}; + +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: + typedef S ScalarType; + + /// 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]; + } + + /// 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; + } + } + + /// Number of columns + inline unsigned int ColumnsNumber() 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; + } + + + + /// 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 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 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 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 matrice + void operator *=( const Matrix33< S> & t ) + { + int i,j; + for(i=0;i<3;++i) + for(j=0;j<3;++j) + (*this)[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j]; + + } + + /// 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]; + + return r; + } + + /// Dot product modifier with a diagonal matrix + void operator *=( const Matrix33Diag< S> & t ) + { + int i,j; + for(i=0;i<3;++i) + for(j=0;j<3;++j) + (*this)[i][j] = (*this)[i][j]*t[j]; + } + + /// 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; + + 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]; + + 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]; + + 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; + + 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]; + } + + 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); + } + + /// 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() + { + Matrix33 res = *this; + res.Transpose(); + return res; + } + + /// 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 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 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 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]) ; + } + + // 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; + + 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]; +} + +/* 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)); +} + + +/* +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; + } +} + + + +/* +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) +{ + 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) +{ + 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(),pw = weights.begin();pi!=P.end();++pi,++xi,++pw){ + + tmp.ExternalProduct(((*pi)-(bp)),((*xi)-(bp))); + + (*this)+=tmp*(*pw); + } +} + +private: + S a[9]; +}; + +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; + } + +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; + } + +///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; + } + + ///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); + + 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 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; + } + +/// 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); + + 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); + + vv.OuterProduct(v,v); + H -= vv*S(2); + M = H*R*S(-1); + return M; +} + +/// +typedef Matrix33 Matrix33s; +typedef Matrix33 Matrix33i; +typedef Matrix33 Matrix33f; +typedef Matrix33 Matrix33d; + +} // end of namespace + +#endif diff --git a/vcg/math/deprecated_matrix44.h b/vcg/math/deprecated_matrix44.h new file mode 100644 index 00000000..037846f8 --- /dev/null +++ b/vcg/math/deprecated_matrix44.h @@ -0,0 +1,984 @@ +/**************************************************************************** +* 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.34 2007/07/12 06:42:01 cignoni +added the missing static Construct() member + +Revision 1.33 2007/07/03 16:06:48 corsini +add DCM to Euler Angles conversion + +Revision 1.32 2007/03/08 14:39:27 corsini +final fix to euler angles transformation + +Revision 1.31 2007/02/06 09:57:40 corsini +fix euler angles computation + +Revision 1.30 2007/02/05 14:16:33 corsini +add from euler angles to rotation matrix conversion + +Revision 1.29 2005/12/02 09:46:49 croccia +Corrected bug in == and != Matrix44 operators + +Revision 1.28 2005/06/28 17:42:47 ganovelli +added Matrix44Diag + +Revision 1.27 2005/06/17 05:28:47 cignoni +Completed Shear Matrix code and comments, +Added use of swap inside Transpose +Added more complete comments on the usage of Decompose + +Revision 1.26 2005/06/10 15:04:12 cignoni +Added Various missing functions: SetShearXY, SetShearXZ, SetShearYZ, SetScale for point3 and Decompose +Completed *=(scalar); made uniform GetRow and GetColumn + +Revision 1.25 2005/04/14 11:35:09 ponchio +*** empty log message *** + +Revision 1.24 2005/03/18 00:14:39 cignoni +removed small gcc compiling issues + +Revision 1.23 2005/03/15 11:40:56 cignoni +Added operator*=( std::vector ...) to apply a matrix to a vector of vertexes (replacement of the old style mesh.Apply(tr). + +Revision 1.22 2004/12/15 18:45:50 tommyfranken +*** empty log message *** + +Revision 1.21 2004/10/22 14:41:30 ponchio +return in operator+ added. + +Revision 1.20 2004/10/18 15:03:14 fiorin +Updated interface: all Matrix classes have now the same interface + +Revision 1.19 2004/10/07 14:23:57 ganovelli +added function to take rows and comlumns. Added toMatrix and fromMatrix to comply +RotationTYpe prototype in Similarity.h + +Revision 1.18 2004/05/28 13:01:50 ganovelli +changed scalar to ScalarType + +Revision 1.17 2004/05/26 15:09:32 cignoni +better comments in set rotate + +Revision 1.16 2004/05/07 10:05:50 cignoni +Corrected abuse of for index variable scope + +Revision 1.15 2004/05/04 23:19:41 cignoni +Clarified initial comment, removed vector*matrix operator (confusing!) +Corrected translate and Rotate, removed gl stuff. + +Revision 1.14 2004/05/04 02:34:03 ganovelli +wrong use of operator [] corrected + +Revision 1.13 2004/04/07 10:45:54 cignoni +Added: [i][j] access, V() for the raw float values, constructor from T[16] + +Revision 1.12 2004/03/25 14:57:49 ponchio + +****************************************************************************/ + +#ifndef __VCGLIB_MATRIX44 +#define __VCGLIB_MATRIX44 + +#include +#include +#include +#include +#include + + +namespace vcg { + + /* + Annotations: +Opengl stores matrix in column-major order. That is, the matrix is stored as: + + a0 a4 a8 a12 + a1 a5 a9 a13 + a2 a6 a10 a14 + a3 a7 a11 a15 + + Usually in opengl (see opengl specs) vectors are 'column' vectors + so usually matrix are PRE-multiplied for a vector. + So the command glTranslate generate a matrix that + is ready to be premultipled for a vector: + + 1 0 0 tx + 0 1 0 ty + 0 0 1 tz + 0 0 0 1 + +Matrix44 stores matrix in row-major order i.e. + + a0 a1 a2 a3 + a4 a5 a6 a7 + a8 a9 a10 a11 + a12 a13 a14 a15 + +So for the use of that matrix in opengl with their supposed meaning you have to transpose them before feeding to glMultMatrix. +This mechanism is hidden by the templated function defined in wrap/gl/math.h; +If your machine has the ARB_transpose_matrix extension it will use the appropriate; +The various gl-like command SetRotate, SetTranslate assume that you are making matrix +for 'column' vectors. + +*/ + + template +class Matrix44Diag:public Point4{ +public: + /** @name Matrix33 + Class Matrix33Diag. + This is the class for definition of a diagonal matrix 4x4. + @param S (Templete Parameter) Specifies the ScalarType field. +*/ + Matrix44Diag(const S & p0,const S & p1,const S & p2,const S & p3):Point4(p0,p1,p2,p3){}; + Matrix44Diag( const Point4 & p ):Point4(p){}; +}; + + + /** This class represent a 4x4 matrix. T is the kind of element in the matrix. + */ +template class Matrix44 { +protected: + T _a[16]; + +public: + typedef T ScalarType; + +///@{ + + /** $name Constructors + * No automatic casting and default constructor is empty + */ + Matrix44() {}; + ~Matrix44() {}; + Matrix44(const Matrix44 &m); + Matrix44(const T v[]); + + /// Number of columns + inline unsigned int ColumnsNumber() const + { + return 4; + }; + + /// Number of rows + inline unsigned int RowsNumber() const + { + return 4; + }; + + T &ElementAt(const int row, const int col); + T ElementAt(const int row, const int col) const; + //T &operator[](const int i); + //const T &operator[](const int i) const; + T *V(); + const T *V() const ; + + T *operator[](const int i); + const T *operator[](const int i) const; + + // return a copy of the i-th column + Point4 GetColumn4(const int& i)const{ + assert(i>=0 && i<4); + return Point4(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i),ElementAt(3,i)); + //return Point4(_a[i],_a[i+4],_a[i+8],_a[i+12]); + } + + Point3 GetColumn3(const int& i)const{ + assert(i>=0 && i<4); + return Point3(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i)); + } + + Point4 GetRow4(const int& i)const{ + assert(i>=0 && i<4); + return Point4(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2),ElementAt(i,3)); + // return *((Point4*)(&_a[i<<2])); alternativa forse + efficiente + } + + Point3 GetRow3(const int& i)const{ + assert(i>=0 && i<4); + return Point3(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2)); + // return *((Point4*)(&_a[i<<2])); alternativa forse + efficiente + } + + Matrix44 operator+(const Matrix44 &m) const; + Matrix44 operator-(const Matrix44 &m) const; + Matrix44 operator*(const Matrix44 &m) const; + Matrix44 operator*(const Matrix44Diag &m) const; + Point4 operator*(const Point4 &v) const; + + bool operator==(const Matrix44 &m) const; + bool operator!= (const Matrix44 &m) const; + + Matrix44 operator-() const; + Matrix44 operator*(const T k) const; + void operator+=(const Matrix44 &m); + void operator-=(const Matrix44 &m); + void operator*=( const Matrix44 & m ); + void operator*=( const T k ); + + template + void ToMatrix(Matrix44Type & m) const {for(int i = 0; i < 16; i++) m.V()[i]=V()[i];} + + void ToEulerAngles(T &alpha, T &beta, T &gamma); + + template + void FromMatrix(const Matrix44Type & m){for(int i = 0; i < 16; i++) V()[i]=m.V()[i];} + void FromEulerAngles(T alpha, T beta, T gamma); + void SetZero(); + void SetIdentity(); + void SetDiagonal(const T k); + Matrix44 &SetScale(const T sx, const T sy, const T sz); + Matrix44 &SetScale(const Point3 &t); + Matrix44 &SetTranslate(const Point3 &t); + Matrix44 &SetTranslate(const T sx, const T sy, const T sz); + Matrix44 &SetShearXY(const T sz); + Matrix44 &SetShearXZ(const T sy); + Matrix44 &SetShearYZ(const T sx); + + ///use radiants for angle. + Matrix44 &SetRotateDeg(T AngleDeg, const Point3 & axis); + Matrix44 &SetRotateRad(T AngleRad, const Point3 & axis); + + T Determinant() const; + + template void Import(const Matrix44 &m) { + for(int i = 0; i < 16; i++) + _a[i] = (T)(m.V()[i]); + } + template + static inline Matrix44 Construct( const Matrix44 & b ) + { + Matrix44 tmp; tmp.FromMatrix(b); + return tmp; + } + + static inline const Matrix44 &Identity( ) + { + static Matrix44 tmp; tmp.SetIdentity(); + return tmp; + } + + +}; + + +/** Class for solving A * x = b. */ +template class LinearSolve: public Matrix44 { +public: + LinearSolve(const Matrix44 &m); + Point4 Solve(const Point4 &b); // solve A · x = b + ///If you need to solve some equation you can use this function instead of Matrix44 one for speed. + T Determinant() const; +protected: + ///Holds row permutation. + int index[4]; //hold permutation + ///Hold sign of row permutation (used for determinant sign) + T d; + bool Decompose(); +}; + +/*** Postmultiply */ +//template Point3 operator*(const Point3 &p, const Matrix44 &m); + +///Premultiply +template Point3 operator*(const Matrix44 &m, const Point3 &p); + +template Matrix44 &Transpose(Matrix44 &m); +//return NULL matrix if not invertible +template Matrix44 &Invert(Matrix44 &m); +template Matrix44 Inverse(const Matrix44 &m); + +typedef Matrix44 Matrix44s; +typedef Matrix44 Matrix44i; +typedef Matrix44 Matrix44f; +typedef Matrix44 Matrix44d; + + + +template Matrix44::Matrix44(const Matrix44 &m) { + memcpy((T *)_a, (T *)m._a, 16 * sizeof(T)); +} + +template Matrix44::Matrix44(const T v[]) { + memcpy((T *)_a, v, 16 * sizeof(T)); +} + +template T &Matrix44::ElementAt(const int row, const int col) { + assert(row >= 0 && row < 4); + assert(col >= 0 && col < 4); + return _a[(row<<2) + col]; +} + +template T Matrix44::ElementAt(const int row, const int col) const { + assert(row >= 0 && row < 4); + assert(col >= 0 && col < 4); + return _a[(row<<2) + col]; +} + +//template T &Matrix44::operator[](const int i) { +// assert(i >= 0 && i < 16); +// return ((T *)_a)[i]; +//} +// +//template const T &Matrix44::operator[](const int i) const { +// assert(i >= 0 && i < 16); +// return ((T *)_a)[i]; +//} +template T *Matrix44::operator[](const int i) { + assert(i >= 0 && i < 4); + return _a+i*4; +} + +template const T *Matrix44::operator[](const int i) const { + assert(i >= 0 && i < 4); + return _a+i*4; +} +template T *Matrix44::V() { return _a;} +template const T *Matrix44::V() const { return _a;} + + +template Matrix44 Matrix44::operator+(const Matrix44 &m) const { + Matrix44 ret; + for(int i = 0; i < 16; i++) + ret.V()[i] = V()[i] + m.V()[i]; + return ret; +} + +template Matrix44 Matrix44::operator-(const Matrix44 &m) const { + Matrix44 ret; + for(int i = 0; i < 16; i++) + ret.V()[i] = V()[i] - m.V()[i]; + return ret; +} + +template Matrix44 Matrix44::operator*(const Matrix44 &m) const { + Matrix44 ret; + for(int i = 0; i < 4; i++) + for(int j = 0; j < 4; j++) { + T t = 0.0; + for(int k = 0; k < 4; k++) + t += ElementAt(i, k) * m.ElementAt(k, j); + ret.ElementAt(i, j) = t; + } + return ret; +} + +template Matrix44 Matrix44::operator*(const Matrix44Diag &m) const { + Matrix44 ret = (*this); + for(int i = 0; i < 4; ++i) + for(int j = 0; j < 4; ++j) + ret[i][j]*=m[i]; + return ret; +} + +template Point4 Matrix44::operator*(const Point4 &v) const { + Point4 ret; + for(int i = 0; i < 4; i++){ + T t = 0.0; + for(int k = 0; k < 4; k++) + t += ElementAt(i,k) * v[k]; + ret[i] = t; + } + return ret; +} + + +template bool Matrix44::operator==(const Matrix44 &m) const { + for(int i = 0; i < 4; ++i) + for(int j = 0; j < 4; ++j) + if(ElementAt(i,j) != m.ElementAt(i,j)) + return false; + return true; +} +template bool Matrix44::operator!=(const Matrix44 &m) const { + for(int i = 0; i < 4; ++i) + for(int j = 0; j < 4; ++j) + if(ElementAt(i,j) != m.ElementAt(i,j)) + return true; + return false; +} + +template Matrix44 Matrix44::operator-() const { + Matrix44 res; + for(int i = 0; i < 16; i++) + res.V()[i] = -V()[i]; + return res; +} + +template Matrix44 Matrix44::operator*(const T k) const { + Matrix44 res; + for(int i = 0; i < 16; i++) + res.V()[i] =V()[i] * k; + return res; +} + +template void Matrix44::operator+=(const Matrix44 &m) { + for(int i = 0; i < 16; i++) + V()[i] += m.V()[i]; +} +template void Matrix44::operator-=(const Matrix44 &m) { + for(int i = 0; i < 16; i++) + V()[i] -= m.V()[i]; +} +template void Matrix44::operator*=( const Matrix44 & m ) { + *this = *this *m; + + /*for(int i = 0; i < 4; i++) { //sbagliato + Point4 t(0, 0, 0, 0); + for(int k = 0; k < 4; k++) { + for(int j = 0; j < 4; j++) { + t[k] += ElementAt(i, k) * m.ElementAt(k, j); + } + } + for(int l = 0; l < 4; l++) + ElementAt(i, l) = t[l]; + } */ +} + +template < class PointType , class T > void operator*=( std::vector &vert, const Matrix44 & m ) { + typename std::vector::iterator ii; + for(ii=vert.begin();ii!=vert.end();++ii) + (*ii).P()=m * (*ii).P(); +} + +template void Matrix44::operator*=( const T k ) { + for(int i = 0; i < 16; i++) + _a[i] *= k; +} + +template +void Matrix44::ToEulerAngles(T &alpha, T &beta, T &gamma) +{ + alpha = atan2(ElementAt(1,2), ElementAt(2,2)); + beta = asin(-ElementAt(0,2)); + gamma = atan2(ElementAt(0,1), ElementAt(1,1)); +} + +template +void Matrix44::FromEulerAngles(T alpha, T beta, T gamma) +{ + this->SetZero(); + + T cosalpha = cos(alpha); + T cosbeta = cos(beta); + T cosgamma = cos(gamma); + T sinalpha = sin(alpha); + T sinbeta = sin(beta); + T singamma = sin(gamma); + + ElementAt(0,0) = cosbeta * cosgamma; + ElementAt(1,0) = -cosalpha * singamma + sinalpha * sinbeta * cosgamma; + ElementAt(2,0) = sinalpha * singamma + cosalpha * sinbeta * cosgamma; + + ElementAt(0,1) = cosbeta * singamma; + ElementAt(1,1) = cosalpha * cosgamma + sinalpha * sinbeta * singamma; + ElementAt(2,1) = -sinalpha * cosgamma + cosalpha * sinbeta * singamma; + + ElementAt(0,2) = -sinbeta; + ElementAt(1,2) = sinalpha * cosbeta; + ElementAt(2,2) = cosalpha * cosbeta; + + ElementAt(3,3) = 1; +} + +template void Matrix44::SetZero() { + memset((T *)_a, 0, 16 * sizeof(T)); +} + +template void Matrix44::SetIdentity() { + SetDiagonal(1); +} + +template void Matrix44::SetDiagonal(const T k) { + SetZero(); + ElementAt(0, 0) = k; + ElementAt(1, 1) = k; + ElementAt(2, 2) = k; + ElementAt(3, 3) = 1; +} + +template Matrix44 &Matrix44::SetScale(const Point3 &t) { + SetScale(t[0], t[1], t[2]); + return *this; +} +template Matrix44 &Matrix44::SetScale(const T sx, const T sy, const T sz) { + SetZero(); + ElementAt(0, 0) = sx; + ElementAt(1, 1) = sy; + ElementAt(2, 2) = sz; + ElementAt(3, 3) = 1; + return *this; +} + +template Matrix44 &Matrix44::SetTranslate(const Point3 &t) { + SetTranslate(t[0], t[1], t[2]); + return *this; +} +template Matrix44 &Matrix44::SetTranslate(const T tx, const T ty, const T tz) { + SetIdentity(); + ElementAt(0, 3) = tx; + ElementAt(1, 3) = ty; + ElementAt(2, 3) = tz; + return *this; +} + +template Matrix44 &Matrix44::SetRotateDeg(T AngleDeg, const Point3 & axis) { + return SetRotateRad(math::ToRad(AngleDeg),axis); +} + +template Matrix44 &Matrix44::SetRotateRad(T AngleRad, const Point3 & axis) { + //angle = angle*(T)3.14159265358979323846/180; e' in radianti! + T c = math::Cos(AngleRad); + T s = math::Sin(AngleRad); + T q = 1-c; + Point3 t = axis; + t.Normalize(); + ElementAt(0,0) = t[0]*t[0]*q + c; + ElementAt(0,1) = t[0]*t[1]*q - t[2]*s; + ElementAt(0,2) = t[0]*t[2]*q + t[1]*s; + ElementAt(0,3) = 0; + ElementAt(1,0) = t[1]*t[0]*q + t[2]*s; + ElementAt(1,1) = t[1]*t[1]*q + c; + ElementAt(1,2) = t[1]*t[2]*q - t[0]*s; + ElementAt(1,3) = 0; + ElementAt(2,0) = t[2]*t[0]*q -t[1]*s; + ElementAt(2,1) = t[2]*t[1]*q +t[0]*s; + ElementAt(2,2) = t[2]*t[2]*q +c; + ElementAt(2,3) = 0; + ElementAt(3,0) = 0; + ElementAt(3,1) = 0; + ElementAt(3,2) = 0; + ElementAt(3,3) = 1; + return *this; +} + + /* Shear Matrixes + XY + 1 k 0 0 x x+ky + 0 1 0 0 y y + 0 0 1 0 z z + 0 0 0 1 1 1 + + 1 0 k 0 x x+kz + 0 1 0 0 y y + 0 0 1 0 z z + 0 0 0 1 1 1 + + 1 1 0 0 x x + 0 1 k 0 y y+kz + 0 0 1 0 z z + 0 0 0 1 1 1 + + */ + + template Matrix44 & Matrix44:: SetShearXY( const T sh) {// shear the X coordinate as the Y coordinate change + SetIdentity(); + ElementAt(0,1) = sh; + return *this; + } + + template Matrix44 & Matrix44:: SetShearXZ( const T sh) {// shear the X coordinate as the Z coordinate change + SetIdentity(); + ElementAt(0,2) = sh; + return *this; + } + + template Matrix44 &Matrix44:: SetShearYZ( const T sh) {// shear the Y coordinate as the Z coordinate change + SetIdentity(); + ElementAt(1,2) = sh; + return *this; + } + + +/* +Given a non singular, non projective matrix (e.g. with the last row equal to [0,0,0,1] ) +This procedure decompose it in a sequence of + Scale,Shear,Rotation e Translation + +- ScaleV and Tranv are obiviously scaling and translation. +- ShearV contains three scalars with, respectively + ShearXY, ShearXZ e ShearYZ +- RotateV contains the rotations (in degree!) around the x,y,z axis + The input matrix is modified leaving inside it a simple roto translation. + + To obtain the original matrix the above transformation have to be applied in the strict following way: + + OriginalMatrix = Trn * Rtx*Rty*Rtz * ShearYZ*ShearXZ*ShearXY * Scl + +Example Code: +double srv() { return (double(rand()%40)-20)/2.0; } // small random value + + srand(time(0)); + Point3d ScV(10+srv(),10+srv(),10+srv()),ScVOut(-1,-1,-1); + Point3d ShV(srv(),srv(),srv()),ShVOut(-1,-1,-1); + Point3d RtV(10+srv(),srv(),srv()),RtVOut(-1,-1,-1); + Point3d TrV(srv(),srv(),srv()),TrVOut(-1,-1,-1); + + Matrix44d Scl; Scl.SetScale(ScV); + Matrix44d Sxy; Sxy.SetShearXY(ShV[0]); + Matrix44d Sxz; Sxz.SetShearXZ(ShV[1]); + Matrix44d Syz; Syz.SetShearYZ(ShV[2]); + Matrix44d Rtx; Rtx.SetRotate(math::ToRad(RtV[0]),Point3d(1,0,0)); + Matrix44d Rty; Rty.SetRotate(math::ToRad(RtV[1]),Point3d(0,1,0)); + Matrix44d Rtz; Rtz.SetRotate(math::ToRad(RtV[2]),Point3d(0,0,1)); + Matrix44d Trn; Trn.SetTranslate(TrV); + + Matrix44d StartM = Trn * Rtx*Rty*Rtz * Syz*Sxz*Sxy *Scl; + Matrix44d ResultM=StartM; + Decompose(ResultM,ScVOut,ShVOut,RtVOut,TrVOut); + + Scl.SetScale(ScVOut); + Sxy.SetShearXY(ShVOut[0]); + Sxz.SetShearXZ(ShVOut[1]); + Syz.SetShearYZ(ShVOut[2]); + Rtx.SetRotate(math::ToRad(RtVOut[0]),Point3d(1,0,0)); + Rty.SetRotate(math::ToRad(RtVOut[1]),Point3d(0,1,0)); + Rtz.SetRotate(math::ToRad(RtVOut[2]),Point3d(0,0,1)); + Trn.SetTranslate(TrVOut); + + // Now Rebuild is equal to StartM + Matrix44d RebuildM = Trn * Rtx*Rty*Rtz * Syz*Sxz*Sxy * Scl ; +*/ +template +bool Decompose(Matrix44 &M, Point3 &ScaleV, Point3 &ShearV, Point3 &RotV,Point3 &TranV) +{ + if(!(M[3][0]==0 && M[3][1]==0 && M[3][2]==0 && M[3][3]==1) ) // the matrix is projective + return false; + if(math::Abs(M.Determinant())<1e-10) return false; // matrix should be at least invertible... + + // First Step recover the traslation + TranV=M.GetColumn3(3); + + + // Second Step Recover Scale and Shearing interleaved + ScaleV[0]=Norm(M.GetColumn3(0)); + Point3 R[3]; + R[0]=M.GetColumn3(0); + R[0].Normalize(); + + ShearV[0]=R[0]*M.GetColumn3(1); // xy shearing + R[1]= M.GetColumn3(1)-R[0]*ShearV[0]; + assert(math::Abs(R[1]*R[0])<1e-10); + ScaleV[1]=Norm(R[1]); // y scaling + R[1]=R[1]/ScaleV[1]; + ShearV[0]=ShearV[0]/ScaleV[1]; + + ShearV[1]=R[0]*M.GetColumn3(2); // xz shearing + R[2]= M.GetColumn3(2)-R[0]*ShearV[1]; + assert(math::Abs(R[2]*R[0])<1e-10); + + R[2] = R[2]-R[1]*(R[2]*R[1]); + assert(math::Abs(R[2]*R[1])<1e-10); + assert(math::Abs(R[2]*R[0])<1e-10); + + ScaleV[2]=Norm(R[2]); + ShearV[1]=ShearV[1]/ScaleV[2]; + R[2]=R[2]/ScaleV[2]; + assert(math::Abs(R[2]*R[1])<1e-10); + assert(math::Abs(R[2]*R[0])<1e-10); + + ShearV[2]=R[1]*M.GetColumn3(2); // yz shearing + ShearV[2]=ShearV[2]/ScaleV[2]; + int i,j; + for(i=0;i<3;++i) + for(j=0;j<3;++j) + M[i][j]=R[j][i]; + + // Third and last step: Recover the rotation + //now the matrix should be a pure rotation matrix so its determinant is +-1 + double det=M.Determinant(); + if(math::Abs(det)<1e-10) return false; // matrix should be at least invertible... + assert(math::Abs(math::Abs(det)-1.0)<1e-10); // it should be +-1... + if(det<0) { + ScaleV *= -1; + M *= -1; + } + + double alpha,beta,gamma; // rotations around the x,y and z axis + beta=asin( M[0][2]); + double cosbeta=cos(beta); + if(math::Abs(cosbeta) > 1e-5) + { + alpha=asin(-M[1][2]/cosbeta); + if((M[2][2]/cosbeta) < 0 ) alpha=M_PI-alpha; + gamma=asin(-M[0][1]/cosbeta); + if((M[0][0]/cosbeta)<0) gamma = M_PI-gamma; + } + else + { + alpha=asin(-M[1][0]); + if(M[1][1]<0) alpha=M_PI-alpha; + gamma=0; + } + + RotV[0]=math::ToDeg(alpha); + RotV[1]=math::ToDeg(beta); + RotV[2]=math::ToDeg(gamma); + + return true; +} + + + + +template T Matrix44::Determinant() const { + LinearSolve solve(*this); + return solve.Determinant(); +} + + +template Point3 operator*(const Matrix44 &m, const Point3 &p) { + T w; + Point3 s; + s[0] = m.ElementAt(0, 0)*p[0] + m.ElementAt(0, 1)*p[1] + m.ElementAt(0, 2)*p[2] + m.ElementAt(0, 3); + s[1] = m.ElementAt(1, 0)*p[0] + m.ElementAt(1, 1)*p[1] + m.ElementAt(1, 2)*p[2] + m.ElementAt(1, 3); + s[2] = m.ElementAt(2, 0)*p[0] + m.ElementAt(2, 1)*p[1] + m.ElementAt(2, 2)*p[2] + m.ElementAt(2, 3); + w = m.ElementAt(3, 0)*p[0] + m.ElementAt(3, 1)*p[1] + m.ElementAt(3, 2)*p[2] + m.ElementAt(3, 3); + if(w!= 0) s /= w; + return s; +} + +//template Point3 operator*(const Point3 &p, const Matrix44 &m) { +// T w; +// Point3 s; +// s[0] = m.ElementAt(0, 0)*p[0] + m.ElementAt(1, 0)*p[1] + m.ElementAt(2, 0)*p[2] + m.ElementAt(3, 0); +// s[1] = m.ElementAt(0, 1)*p[0] + m.ElementAt(1, 1)*p[1] + m.ElementAt(2, 1)*p[2] + m.ElementAt(3, 1); +// s[2] = m.ElementAt(0, 2)*p[0] + m.ElementAt(1, 2)*p[1] + m.ElementAt(2, 2)*p[2] + m.ElementAt(3, 2); +// w = m.ElementAt(0, 3)*p[0] + m.ElementAt(1, 3)*p[1] + m.ElementAt(2, 3)*p[2] + m.ElementAt(3, 3); +// if(w != 0) s /= w; +// return s; +//} + +template Matrix44 &Transpose(Matrix44 &m) { + for(int i = 1; i < 4; i++) + for(int j = 0; j < i; j++) { + math::Swap(m.ElementAt(i, j), m.ElementAt(j, i)); + } + return m; +} + +/* + To invert a matrix you can + either invert the matrix inplace calling + + vcg::Invert(yourMatrix); + + or get the inverse matrix of a given matrix without touching it: + + invertedMatrix = vcg::Inverse(untouchedMatrix); + +*/ +template Matrix44 & Invert(Matrix44 &m) { + LinearSolve solve(m); + + for(int j = 0; j < 4; j++) { //Find inverse by columns. + Point4 col(0, 0, 0, 0); + col[j] = 1.0; + col = solve.Solve(col); + for(int i = 0; i < 4; i++) + m.ElementAt(i, j) = col[i]; + } + return m; +} + +template Matrix44 Inverse(const Matrix44 &m) { + LinearSolve solve(m); + Matrix44 res; + for(int j = 0; j < 4; j++) { //Find inverse by columns. + Point4 col(0, 0, 0, 0); + col[j] = 1.0; + col = solve.Solve(col); + for(int i = 0; i < 4; i++) + res.ElementAt(i, j) = col[i]; + } + return res; +} + + + +/* LINEAR SOLVE IMPLEMENTATION */ + +template LinearSolve::LinearSolve(const Matrix44 &m): Matrix44(m) { + if(!Decompose()) { + for(int i = 0; i < 4; i++) + index[i] = i; + Matrix44::SetZero(); + } +} + + +template T LinearSolve::Determinant() const { + T det = d; + for(int j = 0; j < 4; j++) + det *= this-> ElementAt(j, j); + return det; +} + + +/*replaces a matrix by its LU decomposition of a rowwise permutation. +d is +or -1 depeneing of row permutation even or odd.*/ +#define TINY 1e-100 + +template bool LinearSolve::Decompose() { + + /* Matrix44 A; + for(int i = 0; i < 16; i++) + A[i] = operator[](i); + SetIdentity(); + Point4 scale; + // Set scale factor, scale(i) = max( |a(i,j)| ), for each row + for(int i = 0; i < 4; i++ ) { + index[i] = i; // Initialize row index list + T scalemax = (T)0.0; + for(int j = 0; j < 4; j++) + scalemax = (scalemax > math::Abs(A.ElementAt(i,j))) ? scalemax : math::Abs(A.ElementAt(i,j)); + scale[i] = scalemax; + } + + // Loop over rows k = 1, ..., (N-1) + int signDet = 1; + for(int k = 0; k < 3; k++ ) { + // Select pivot row from max( |a(j,k)/s(j)| ) + T ratiomax = (T)0.0; + int jPivot = k; + for(int i = k; i < 4; i++ ) { + T ratio = math::Abs(A.ElementAt(index[i], k))/scale[index[i]]; + if(ratio > ratiomax) { + jPivot = i; + ratiomax = ratio; + } + } + // Perform pivoting using row index list + int indexJ = index[k]; + if( jPivot != k ) { // Pivot + indexJ = index[jPivot]; + index[jPivot] = index[k]; // Swap index jPivot and k + index[k] = indexJ; + signDet *= -1; // Flip sign of determinant + } + // Perform forward elimination + for(int i=k+1; i < 4; i++ ) { + T coeff = A.ElementAt(index[i],k)/A.ElementAt(indexJ,k); + for(int j=k+1; j < 4; j++ ) + A.ElementAt(index[i],j) -= coeff*A.ElementAt(indexJ,j); + A.ElementAt(index[i],k) = coeff; + //for( j=1; j< 4; j++ ) + // ElementAt(index[i],j) -= A.ElementAt(index[i], k)*ElementAt(indexJ, j); + } + } + for(int i = 0; i < 16; i++) + operator[](i) = A[i]; + + d = signDet; + // this = A; + return true; */ + + d = 1; //no permutation still + + T scaling[4]; + int i,j,k; + //Saving the scvaling information per row + for(i = 0; i < 4; i++) { + T largest = 0.0; + for(j = 0; j < 4; j++) { + T t = math::Abs(this->ElementAt(i, j)); + if (t > largest) largest = t; + } + + if (largest == 0.0) { //oooppps there is a zero row! + return false; + } + scaling[i] = (T)1.0 / largest; + } + + int imax = 0; + for(j = 0; j < 4; j++) { + for(i = 0; i < j; i++) { + T sum = this->ElementAt(i,j); + for(int k = 0; k < i; k++) + sum -= this->ElementAt(i,k)*this->ElementAt(k,j); + this->ElementAt(i,j) = sum; + } + T largest = 0.0; + for(i = j; i < 4; i++) { + T sum = this->ElementAt(i,j); + for(k = 0; k < j; k++) + sum -= this->ElementAt(i,k)*this->ElementAt(k,j); + this->ElementAt(i,j) = sum; + T t = scaling[i] * math::Abs(sum); + if(t >= largest) { + largest = t; + imax = i; + } + } + if (j != imax) { + for (int k = 0; k < 4; k++) { + T dum = this->ElementAt(imax,k); + this->ElementAt(imax,k) = this->ElementAt(j,k); + this->ElementAt(j,k) = dum; + } + d = -(d); + scaling[imax] = scaling[j]; + } + index[j]=imax; + if (this->ElementAt(j,j) == 0.0) this->ElementAt(j,j) = (T)TINY; + if (j != 3) { + T dum = (T)1.0 / (this->ElementAt(j,j)); + for (i = j+1; i < 4; i++) + this->ElementAt(i,j) *= dum; + } + } + return true; +} + + +template Point4 LinearSolve::Solve(const Point4 &b) { + Point4 x(b); + int first = -1, ip; + for(int i = 0; i < 4; i++) { + ip = index[i]; + T sum = x[ip]; + x[ip] = x[i]; + if(first!= -1) + for(int j = first; j <= i-1; j++) + sum -= this->ElementAt(i,j) * x[j]; + else + if(sum) first = i; + x[i] = sum; + } + for (int i = 3; i >= 0; i--) { + T sum = x[i]; + for (int j = i+1; j < 4; j++) + sum -= this->ElementAt(i, j) * x[j]; + x[i] = sum / this->ElementAt(i, i); + } + return x; +} + +} //namespace +#endif + + diff --git a/vcg/math/eigen.h b/vcg/math/eigen.h new file mode 100644 index 00000000..23d47fd2 --- /dev/null +++ b/vcg/math/eigen.h @@ -0,0 +1,57 @@ +/**************************************************************************** +* 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. * +* * +****************************************************************************/ + +#ifndef EIGEN_VCGLIB +#define EIGEN_VCGLIB + +#define EIGEN_MATRIXBASE_PLUGIN + +#include "../Eigen/Array" +#include "../Eigen/Core" + +#define VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, Op) \ + template \ + Derived& operator Op(const Eigen::MatrixBase& other) \ + { \ + Base::operator Op(other.derived()); return *this;\ + } \ + Derived& operator Op(const Derived& other) \ + { \ + Base::operator Op(other); return *this;\ + } + + #define VCG_EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, Op) \ + template \ + Derived& operator Op(const Other& scalar) \ + { \ + Base::operator Op(scalar); return *this;\ + } + +#define VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ + VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, =) \ + VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, +=) \ + VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, -=) \ + VCG_EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, *=) \ + VCG_EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, /=) + +#endif diff --git a/vcg/math/eigen_vcgaddons.h b/vcg/math/eigen_vcgaddons.h new file mode 100644 index 00000000..3aa2cffe --- /dev/null +++ b/vcg/math/eigen_vcgaddons.h @@ -0,0 +1,319 @@ +/**************************************************************************** +* 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. * +* * +****************************************************************************/ + +#warning You are including deprecated math stuff +/*! +* \deprecated use cols() +* Number of columns +*/ +EIGEN_DEPRECATED inline unsigned int ColumnsNumber() const +{ + return cols(); +}; + + +/*! +* \deprecated use rows() +* Number of rows +*/ +EIGEN_DEPRECATED inline unsigned int RowsNumber() const +{ + return rows(); +}; + +/* +* \deprecated use *this.isApprox(m) or *this.cwise() == m +* Equality operator. +* \param m +* \return true iff the matrices have same size and its elements have same values. +*/ +// template +// EIGEN_DEPRECATED bool operator==(const MatrixBase &m) const +// { +// return (this->cwise() == m); +// } + +/* +* \deprecated use !*this.isApprox(m) or *this.cwise() != m +* Inequality operator +* \param m +* \return true iff the matrices have different size or if their elements have different values. +*/ +// template +// EIGEN_DEPRECATED bool operator!=(const MatrixBase &m) const +// { +// return (this->cwise() != m); +// }; + +/*! +* \deprecated use *this(i,j) (or *this.coeff(i,j)) +* Return the element stored in the i-th rows at the j-th column +* \param i the row index +* \param j the column index +* \return the element +*/ +EIGEN_DEPRECATED inline Scalar ElementAt(unsigned int i, unsigned int j) +{ + return (*this)(i,j); +}; + +/*! +* \deprecated use *this.determinant() (or *this.lu().determinant() for large matrices) +* Calculate and return the matrix determinant (Laplace) +* \return the matrix determinant +*/ +EIGEN_DEPRECATED Scalar Determinant() const +{ + return determinant(); +}; + +/*! +* Return the cofactor Ai,j of the ai,j element +* \return ... +*/ +EIGEN_DEPRECATED Scalar Cofactor(unsigned int i, unsigned int j) const +{ + assert(rows() == cols()); + assert(rows()>2); + return (((i+j)%2==0) ? 1. : -1.) * minor(i,j).determinant(); +}; + +/*! +* \deprecated use *this.col(j) +* Get the j-th column on the matrix. +* \param j the column index. +* \return the reference to the column elements. This pointer must be deallocated by the caller. +*/ +EIGEN_DEPRECATED ColXpr GetColumn(const unsigned int j) +{ + return col(j); +}; + +/*! +* \deprecated use *this.row(i) +* Get the i-th row on the matrix. +* \param i the column index. +* \return the reference to the row elements. This pointer must be deallocated by the caller. +*/ +EIGEN_DEPRECATED RowXpr GetRow(const unsigned int i) +{ + return row(i); +}; + +/*! +* \deprecated use m1.col(i).swap(m1.col(j)); +* Swaps the values of the elements between the i-th and the j-th column. +* \param i the index of the first column +* \param j the index of the second column +*/ +EIGEN_DEPRECATED void SwapColumns(const unsigned int i, const unsigned int j) +{ + if (i==j) + return; + + col(i).swap(col(j)); +}; + +/*! +* \deprecated use m1.col(i).swap(m1.col(j)) +* Swaps the values of the elements between the i-th and the j-th row. +* \param i the index of the first row +* \param j the index of the second row +*/ +EIGEN_DEPRECATED void SwapRows(const unsigned int i, const unsigned int j) +{ + if (i==j) + return; + + row(i).swap(row(j)); +}; + + +/*! +* \deprecated use *this.cwise() += k +* (Modifier) Add to each element of this matrix the scalar constant k. +* \param k the scalar constant +* \return the modified matrix +*/ +EIGEN_DEPRECATED Derived& operator+=(const Scalar k) +{ + cwise() += k; + return *this; +}; + +/*! +* \deprecated use *this.cwise() -= k +* (Modifier) Subtract from each element of this matrix the scalar constant k. +* \param k the scalar constant +* \return the modified matrix +*/ +EIGEN_DEPRECATED Derived& operator-=(const Scalar k) +{ + cwise() -= k; + return *this; +}; + +/*! +* \deprecated use *this.dot +* Matrix multiplication: calculates the cross product. +* \param reference to the matrix to multiply by +* \return the matrix product +*/ +// template +// EIGEN_DEPRECATED void DotProduct(Point &m,Point &result) +// { +// unsigned int i, j; +// for (i=0; i() +* Matrix multiplication by a diagonal matrix +*/ +// EIGEN_DEPRECATED Matrix operator*(const MatrixDiagBase &m) const +// { +// assert(_columns == _rows); +// assert(_columns == m.Dimension()); +// int i,j; +// Matrix result(_rows, _columns); +// +// for (i=0; i +EIGEN_DEPRECATED void OuterProduct(const MatrixBase& a, const MatrixBase& b) +{ + *this = a * b.transpose(); +} + +typedef CwiseUnaryOp, Derived> ScalarAddReturnType; + +/*! +* \deprecated use *this.cwise() + k +* Scalar sum. +* \param k +* \return the resultant matrix +*/ +EIGEN_DEPRECATED const ScalarAddReturnType operator+(const Scalar k) { return cwise() + k; } + +/*! +* \deprecated use *this.cwise() - k +* Scalar difference. +* \param k +* \return the resultant matrix +*/ +EIGEN_DEPRECATED const ScalarAddReturnType operator-(const Scalar k) { return cwise() - k; } + + +/*! +* \deprecated use *this.setZero() or *this = MatrixType::Zero(rows,cols), etc. +* Set all the matrix elements to zero. +*/ +EIGEN_DEPRECATED void SetZero() +{ + setZero(); +}; + +/*! +* \deprecated use *this.setIdentity() or *this = MatrixType::Identity(rows,cols), etc. +* Set the matrix to identity. +*/ +EIGEN_DEPRECATED void SetIdentity() +{ + setIdentity(); +}; + +/*! +* \deprecated use *this.col(j) = expression +* Set the values of j-th column to v[j] +* \param j the column index +* \param v ... +*/ +EIGEN_DEPRECATED void SetColumn(const unsigned int j, Scalar* v) +{ + col(j) = Map >(v,cols(),1); +}; + +/*! +* \deprecated use *this.row(i) = expression +* Set the elements of the i-th row to v[j] +* \param i the row index +* \param v ... +*/ +EIGEN_DEPRECATED void SetRow(const unsigned int i, Scalar* v) +{ + row(i) = Map >(v,1,rows()); +}; + +/*! +* \deprecated use *this.diagonal() = expression +* Set the diagonal elements vi,i to v[i] +* \param v +*/ +EIGEN_DEPRECATED void SetDiagonal(Scalar *v) +{ + assert(rows() == cols()); + diagonal() = Map >(v,cols(),1); +} + + +/*! +* \deprecated use *this = *this.transpose() +*/ +// Transpose already exist +// EIGEN_DEPRECATED void Transpose() +// { +// assert(0 && "dangerous use of deprecated Transpose function, please use: m = m.transpose();"); +// }; + + +/*! +* \deprecated use ostream << *this or ostream << *this.withFormat(...) +* Print all matrix elements +*/ +EIGEN_DEPRECATED void Dump() +{ + unsigned int i, j; + for (i=0; i -Revision 1.8 2006/08/23 15:24:45 marfr960 -Copy constructor : faster memcpy instead of slow 'for' cycle -empty constructor +#ifndef VCG_USE_EIGEN +#include "deprecated_matrix.h" +#endif -Revision 1.7 2006/04/29 10:26:04 fiorin -Added some utility methods (swapping of columns and rows, matrix-vector multiplication) - -Revision 1.6 2006/04/11 08:09:35 zifnab1974 -changes necessary for gcc 3.4.5 on linux 64bit. Please take note of case-sensitivity of filenames - -Revision 1.5 2005/12/12 11:25:00 ganovelli -added diagonal matrix, outer produce and namespace - -***************************************************************************/ #ifndef MATRIX_VCGLIB #define MATRIX_VCGLIB -#include -#include -#include -#include -#include +#include "eigen.h" #include -#include namespace vcg{ - namespace ndim{ +namespace ndim{ +template class Matrix; +} +} - /** \addtogroup math */ - /* @{ */ +namespace Eigen{ +template +struct ei_traits > : ei_traits > {}; +} - /*! - * This class represent a diagonal m×m matrix. - */ - - class MatrixDiagBase{public: - virtual const int & Dimension()const =0; - virtual const float operator[](const int & i)const = 0; - }; - template - class MatrixDiag: public Point, public MatrixDiagBase{ - public: - const int & Dimension() const {return N;} - MatrixDiag(const Point&p):Point(p){} - }; +namespace vcg{ +namespace ndim{ + +/** \addtogroup math */ +/* @{ */ /*! - * This class represent a generic m×n matrix. The class is templated over the scalar type field. - * @param TYPE (Templete Parameter) Specifies the ScalarType field. + * This class represent a diagonal m�m matrix. */ - template - class Matrix - { - - public: - typedef TYPE ScalarType; - - /*! - * Default constructor - * All the elements are initialized to zero. - * \param m the number of matrix rows - * \param n the number of matrix columns - */ - Matrix(unsigned int m, unsigned int n) - { - _rows = m; - _columns = n; - _data = new ScalarType[m*n]; - memset(_data, 0, m*n*sizeof(ScalarType)); - }; - - /*! - * Constructor - * The matrix elements are initialized with the values of the elements in \i values. - * \param m the number of matrix rows - * \param n the number of matrix columns - * \param values the values of the matrix elements - */ - Matrix(unsigned int m, unsigned int n, TYPE *values) - { - _rows = m; - _columns = n; - unsigned int dim = m*n; - _data = new ScalarType[dim]; - memcpy(_data, values, dim*sizeof(ScalarType)); - //unsigned int i; - //for (i=0; i<_rows*_columns; i++) - // _data[i] = values[i]; - }; - - /*! - * Empty constructor - * Just create the object - */ - Matrix() - { - _rows = 0; - _columns = 0; - _data = NULL; - }; - - /*! - * Copy constructor - * The matrix elements are initialized with the value of the corresponding element in \i m - * \param m the matrix to be copied - */ - Matrix(const Matrix &m) - { - _rows = m._rows; - _columns = m._columns; - _data = new ScalarType[_rows*_columns]; - - unsigned int dim = _rows * _columns; - memcpy(_data, m._data, dim * sizeof(ScalarType)); - -// for (unsigned int i=0; i<_rows*_columns; i++) -// _data[i] = m._data[i]; - }; - - /*! - * Default destructor - */ - ~Matrix() - { - delete []_data; - }; - - /*! - * Number of columns - */ - inline unsigned int ColumnsNumber() const - { - return _columns; - }; - - - /*! - * Number of rows - */ - inline unsigned int RowsNumber() const - { - return _rows; - }; - - /*! - * Equality operator. - * \param m - * \return true iff the matrices have same size and its elements have same values. - */ - bool operator==(const Matrix &m) const - { - if (_rows==m._rows && _columns==m._columns) - { - bool result = true; - for (unsigned int i=0; i<_rows*_columns && result; i++) - result = (_data[i]==m._data[i]); - return result; - } - return false; - }; - - /*! - * Inequality operator - * \param m - * \return true iff the matrices have different size or if their elements have different values. - */ - bool operator!=(const Matrix &m) const - { - if (_rows==m._rows && _columns==m._columns) - { - bool result = false; - for (unsigned int i=0; i<_rows*_columns && !result; i++) - result = (_data[i]!=m._data[i]); - return result; - } - return true; - }; - - /*! - * Return the element stored in the i-th rows at the j-th column - * \param i the row index - * \param j the column index - * \return the element - */ - inline TYPE ElementAt(unsigned int i, unsigned int j) - { - assert(i>=0 && i<_rows); - assert(j>=0 && j<_columns); - return _data[i*_columns+j]; - }; - - /*! - * Calculate and return the matrix determinant (Laplace) - * \return the matrix determinant - */ - TYPE Determinant() const - { - assert(_rows == _columns); - switch (_rows) - { - case 2: - { - return _data[0]*_data[3]-_data[1]*_data[2]; - break; - }; - case 3: - { - return _data[0]*(_data[4]*_data[8]-_data[5]*_data[7]) - - _data[1]*(_data[3]*_data[8]-_data[5]*_data[6]) + - _data[2]*(_data[3]*_data[7]-_data[4]*_data[6]) ; - break; - }; - default: - { - // da migliorare: si puo' cercare la riga/colonna con maggior numero di zeri - ScalarType det = 0; - for (unsigned int j=0; j<_columns; j++) - if (_data[j]!=0) - det += _data[j]*this->Cofactor(0, j); - - return det; - } - }; - }; - - /*! - * Return the cofactor Ai,j of the ai,j element - * \return ... - */ - TYPE Cofactor(unsigned int i, unsigned int j) const - { - assert(_rows == _columns); - assert(_rows>2); - TYPE *values = new TYPE[(_rows-1)*(_columns-1)]; - unsigned int u, v, p, q, s, t; - for (u=0, p=0, s=0, t=0; u<_rows; u++, t+=_rows) - { - if (i==u) - continue; - - for (v=0, q=0; v<_columns; v++) - { - if (j==v) - continue; - values[s+q] = _data[t+v]; - q++; - } - p++; - s+=(_rows-1); - } - Matrix temp(_rows-1, _columns-1, values); - return (pow(-1, i+j)*temp.Determinant()); - }; - - /*! - * Subscript operator: - * \param i the index of the row - * \return a reference to the i-th matrix row - */ - inline TYPE* operator[](const unsigned int i) - { - assert(i>=0 && i<_rows); - return _data + i*_columns; - }; - - /*! - * Const subscript operator - * \param i the index of the row - * \return a reference to the i-th matrix row - */ - inline const TYPE* operator[](const unsigned int i) const - { - assert(i>=0 && i<_rows); - return _data + i*_columns; - }; - - /*! - * Get the j-th column on the matrix. - * \param j the column index. - * \return the reference to the column elements. This pointer must be deallocated by the caller. - */ - TYPE* GetColumn(const unsigned int j) - { - assert(j>=0 && j<_columns); - ScalarType *v = new ScalarType[_columns]; - unsigned int i, p; - for (i=0, p=j; i<_rows; i++, p+=_columns) - v[i] = _data[p]; - return v; - }; - - /*! - * Get the i-th row on the matrix. - * \param i the column index. - * \return the reference to the row elements. This pointer must be deallocated by the caller. - */ - TYPE* GetRow(const unsigned int i) - { - assert(i>=0 && i<_rows); - ScalarType *v = new ScalarType[_rows]; - unsigned int j, p; - for (j=0, p=i*_columns; j<_columns; j++, p++) - v[j] = _data[p]; - return v; - }; - - /*! - * Swaps the values of the elements between the i-th and the j-th column. - * \param i the index of the first column - * \param j the index of the second column - */ - void SwapColumns(const unsigned int i, const unsigned int j) - { - assert(0<=i && i<_columns); - assert(0<=j && j<_columns); - if (i==j) - return; - - unsigned int r, e0, e1; - for (r=0, e0=i, e1=j; r<_rows; r++, e0+=_columns, e1+=_columns) - std::swap(_data[e0], _data[e1]); - }; - - /*! - * Swaps the values of the elements between the i-th and the j-th row. - * \param i the index of the first row - * \param j the index of the second row - */ - void SwapRows(const unsigned int i, const unsigned int j) - { - assert(0<=i && i<_rows); - assert(0<=j && j<_rows); - if (i==j) - return; - - unsigned int r, e0, e1; - for (r=0, e0=i*_columns, e1=j*_columns; r<_columns; r++, e0++, e1++) - std::swap(_data[e0], _data[e1]); - }; - - /*! - * Assignment operator - * \param m ... - */ - Matrix& operator=(const Matrix &m) - { - if (this != &m) - { - assert(_rows == m._rows); - assert(_columns == m._columns); - for (unsigned int i=0; i<_rows*_columns; i++) - _data[i] = m._data[i]; - } - return *this; - }; - - /*! - * Adds a matrix m to this matrix. - * \param m reference to matrix to add to this - * \return the matrix sum. - */ - Matrix& operator+=(const Matrix &m) - { - assert(_rows == m._rows); - assert(_columns == m._columns); - for (unsigned int i=0; i<_rows*_columns; i++) - _data[i] += m._data[i]; - return *this; - }; - - /*! - * Subtracts a matrix m to this matrix. - * \param m reference to matrix to subtract - * \return the matrix difference. - */ - Matrix& operator-=(const Matrix &m) - { - assert(_rows == m._rows); - assert(_columns == m._columns); - for (unsigned int i=0; i<_rows*_columns; i++) - _data[i] -= m._data[i]; - return *this; - }; - - /*! - * (Modifier) Add to each element of this matrix the scalar constant k. - * \param k the scalar constant - * \return the modified matrix - */ - Matrix& operator+=(const TYPE k) - { - for (unsigned int i=0; i<_rows*_columns; i++) - _data[i] += k; - return *this; - }; - - /*! - * (Modifier) Subtract from each element of this matrix the scalar constant k. - * \param k the scalar constant - * \return the modified matrix - */ - Matrix& operator-=(const TYPE k) - { - for (unsigned int i=0; i<_rows*_columns; i++) - _data[i] -= k; - return *this; - }; - - /*! - * (Modifier) Multiplies each element of this matrix by the scalar constant k. - * \param k the scalar constant - * \return the modified matrix - */ - Matrix& operator*=(const TYPE k) - { - for (unsigned int i=0; i<_rows*_columns; i++) - _data[i] *= k; - return *this; - }; - - /*! - * (Modifier) Divides each element of this matrix by the scalar constant k. - * \param k the scalar constant - * \return the modified matrix - */ - Matrix& operator/=(const TYPE k) - { - assert(k!=0); - for (unsigned int i=0; i<_rows*_columns; i++) - _data[i] /= k; - return *this; - }; - - /*! - * Matrix multiplication: calculates the cross product. - * \param m reference to the matrix to multiply by - * \return the matrix product - */ - Matrix operator*(const Matrix &m) const - { - assert(_columns == m._rows); - Matrix result(_rows, m._columns); - unsigned int i, j, k, p, q, r; - for (i=0, p=0, r=0; i - void DotProduct(Point &m,Point &result) - { - unsigned int i, j, p, r; - for (i=0, p=0, r=0; i operator*(const MatrixDiagBase &m) const - { - assert(_columns == _rows); - assert(_columns == m.Dimension()); - int i,j; - Matrix result(_rows, _columns); - - for (i=0; i - void OuterProduct(const Point a, const Point< M,TYPE> b) - { - assert(N == _rows); - assert(M == _columns); - Matrix result(_rows,_columns); - unsigned int i, j; - - for (i=0; i operator*(Point3 &p) const - { - assert(_columns==3 && _rows==3); - vcg::Point3 result; - result[0] = _data[0]*p[0]+_data[1]*p[1]+_data[2]*p[2]; - result[1] = _data[3]*p[0]+_data[4]*p[1]+_data[5]*p[2]; - result[2] = _data[6]*p[0]+_data[7]*p[1]+_data[8]*p[2]; - return result; - }; - - - /*! - * Scalar sum. - * \param k - * \return the resultant matrix - */ - Matrix operator+(const TYPE k) - { - Matrix result(_rows, _columns); - for (unsigned int i=0; i<_rows*_columns; i++) - result._data[i] = _data[i]+k; - return result; - }; - - /*! - * Scalar difference. - * \param k - * \return the resultant matrix - */ - Matrix operator-(const TYPE k) - { - Matrix result(_rows, _columns); - for (unsigned int i=0; i<_rows*_columns; i++) - result._data[i] = _data[i]-k; - return result; - }; - - /*! - * Negate all matrix elements - * \return the modified matrix - */ - Matrix operator-() const - { - Matrix result(_rows, _columns, _data); - for (unsigned int i=0; i<_columns*_rows; i++) - result._data[i] = -1*_data[i]; - return result; - }; - - /*! - * Scalar multiplication. - * \param k value to multiply every member by - * \return the resultant matrix - */ - Matrix operator*(const TYPE k) const - { - Matrix result(_rows, _columns); - for (unsigned int i=0; i<_rows*_columns; i++) - result._data[i] = _data[i]*k; - return result; - }; - - /*! - * Scalar division. - * \param k value to divide every member by - * \return the resultant matrix - */ - Matrix operator/(const TYPE k) - { - Matrix result(_rows, _columns); - for (unsigned int i=0; i<_rows*_columns; i++) - result._data[i] = _data[i]/k; - return result; - }; - - - /*! - * Set all the matrix elements to zero. - */ - void SetZero() - { - for (unsigned int i=0; i<_rows*_columns; i++) - _data[i] = ScalarType(0.0); - }; - - /*! - * Set the matrix to identity. - */ - void SetIdentity() - { - assert(_rows==_columns); - for (unsigned int i=0; i<_rows; i++) - for (unsigned int j=0; j<_columns; j++) - _data[i] = (i==j) ? ScalarType(1.0) : ScalarType(0.0f); - }; - - /*! - * Set the values of j-th column to v[j] - * \param j the column index - * \param v ... - */ - void SetColumn(const unsigned int j, TYPE* v) - { - assert(j>=0 && j<_columns); - unsigned int i, p; - for (i=0, p=j; i<_rows; i++, p+=_columns) - _data[p] = v[i]; - }; - - /*! - * Set the elements of the i-th row to v[j] - * \param i the row index - * \param v ... - */ - void SetRow(const unsigned int i, TYPE* v) - { - assert(i>=0 && i<_rows); - unsigned int j, p; - for (j=0, p=i*_rows; j<_columns; j++, p++) - _data[p] = v[j]; - }; - - /*! - * Set the diagonal elements vi,i to v[i] - * \param v - */ - void SetDiagonal(TYPE *v) - { - assert(_rows == _columns); - for (unsigned int i=0, p=0; i<_rows; i++, p+=_rows) - _data[p+i] = v[i]; - }; - - /*! - * Resize the current matrix. - * \param m the number of matrix rows. - * \param n the number of matrix columns. - */ - void Resize(const unsigned int m, const unsigned int n) - { - assert(m>=2); - assert(n>=2); - _rows = m; - _columns = n; - delete []_data; - _data = new ScalarType[m*n]; - for (unsigned int i=0; i MatrixMNd; - typedef vcg::ndim::Matrix MatrixMNf; - - /*! @} */ - - template - void Invert(MatrixType & m){ - typedef typename MatrixType::ScalarType X; - X *diag; - diag = new X [m.ColumnsNumber()]; - - MatrixType res(m.RowsNumber(),m.ColumnsNumber()); - vcg::SingularValueDecomposition (m,&diag[0],res,LeaveUnsorted,50 ); - m.Transpose(); - // prodotto per la diagonale - unsigned int i,j; - for (i=0; i +class MatrixDiag: public Point, public MatrixDiagBase{ +public: + const int & Dimension() const {return N;} + MatrixDiag(const Point&p):Point(p){} +}; + +/*! + * This class represent a generic m�n matrix. The class is templated over the scalar type field. + * @param Scalar (Templete Parameter) Specifies the ScalarType field. + */ +template +class Matrix : public Eigen::Matrix<_Scalar,Eigen::Dynamic,Eigen::Dynamic> // FIXME col or row major ? +{ + + typedef Eigen::Matrix<_Scalar,Eigen::Dynamic,Eigen::Dynamic> _Base; + +public: + + _EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix,_Base); + typedef _Scalar ScalarType; + + VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Matrix) + + /*! + * Default constructor + * All the elements are initialized to zero. + * \param m the number of matrix rows + * \param n the number of matrix columns + */ + Matrix(unsigned int m, unsigned int n) + : Base(m,n) + { + memset(Base::data(), 0, m*n*sizeof(Scalar)); } -}; // end of namespace + + /*! + * Constructor + * The matrix elements are initialized with the values of the elements in \i values. + * \param m the number of matrix rows + * \param n the number of matrix columns + * \param values the values of the matrix elements + */ + Matrix(unsigned int m, unsigned int n, Scalar *values) + : Base(m.n) + { + *this = Eigen::Map >(values, m , n); + } + + /*! + * Empty constructor + * Just create the object + */ + Matrix() : Base() {} + + /*! + * Copy constructor + * The matrix elements are initialized with the value of the corresponding element in \i m + * \param m the matrix to be copied + */ + Matrix(const Matrix &m) : Base(m) {} + + template + Matrix(const Eigen::MatrixBase &m) : Base(m) {} + + /*! + * Default destructor + */ + ~Matrix() {} + + /*! + * \deprecated use *this.row(i) + * Subscript operator: + * \param i the index of the row + * \return a reference to the i-th matrix row + */ + inline typename Base::RowXpr operator[](const unsigned int i) + { return Base::row(i); } + + /*! + * \deprecated use *this.row(i) + * Const subscript operator + * \param i the index of the row + * \return a reference to the i-th matrix row + */ + inline const typename Base::RowXpr operator[](const unsigned int i) const + { return Base::row(i); } + + + /*! + * Matrix multiplication: calculates the cross product. + * \param reference to the matrix to multiply by + * \return the matrix product + */ + /*template + void DotProduct(Point &m,Point &result) + { + unsigned int i, j, p, r; + for (i=0, p=0, r=0; i=2); + assert(n>=2); + Base::resize(m,n); + memset(Base::data(), 0, m*n*sizeof(Scalar)); + }; + +// EIGEN_DEPRECATED void Transpose() +// { +// assert(0 && "dangerous use of deprecated Transpose function, please use: m = m.transpose();"); +// } +}; + +typedef vcg::ndim::Matrix MatrixMNd; +typedef vcg::ndim::Matrix MatrixMNf; + +/*! @} */ + +template +void Invert(MatrixType & m) +{ + m = m.inverse(); +} + +} +} // end of namespace #endif diff --git a/vcg/math/matrix33.h b/vcg/math/matrix33.h index a9160ff6..fc319fad 100644 --- a/vcg/math/matrix33.h +++ b/vcg/math/matrix33.h @@ -19,622 +19,212 @@ * GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * * for more details. * * * -****************************************************************************/ -/**************************************************************************** - History - -$Log: not supported by cvs2svn $ -Revision 1.18 2007/04/19 14:30:26 pietroni -added RotationMatrix method to calculate rotation matrix along an axis - -Revision 1.17 2007/04/07 23:06:47 pietroni -Added function RotationMatrix - -Revision 1.16 2007/01/29 00:20:25 pietroni --Used scalar type passed as template argument istead of double to prevent warnings.. in Rotate function - -Revision 1.15 2006/09/25 23:05:29 ganovelli -added constructor from matrix44 excluding a row and colum - -Revision 1.14 2006/06/22 08:00:05 ganovelli -bug in operator + with MatrixxDig - -Revision 1.13 2006/01/20 16:41:44 pietroni -added operators: - operator -= ( const Matrix33Diag &p ) - Matrix33 operator - ( const Matrix33Diag &p ) - Matrix33 operator + ( const Matrix33 &m ) - Matrix33 operator + ( const Matrix33Diag &p ) - -Revision 1.12 2005/11/14 10:28:25 cignoni -Changed Invert -> FastInvert for the function based on the maple expansion - -Revision 1.11 2005/10/13 15:45:23 ponchio -Changed a Zero in SetZero in WeightedCrossCovariance() (again) - -Revision 1.10 2005/10/05 17:06:12 pietroni -corrected sintax error on singular value decomposition - -Revision 1.9 2005/09/29 09:53:58 ganovelli -added inverse by SVD - -Revision 1.8 2005/06/10 14:51:54 cignoni -Changed a Zero in SetZero in WeightedCrossCovariance() - -Revision 1.7 2005/06/10 11:46:49 pietroni -Added Norm Function - -Revision 1.6 2005/06/07 14:29:56 ganovelli -changed from Matrix33Ide to MatrixeeDiag - -Revision 1.5 2005/05/23 15:05:26 ganovelli -Matrix33Diag Added: it implements diagonal matrix. Added only operator += in Matrix33 - -Revision 1.4 2005/04/11 14:11:22 pietroni -changed swap to math::Swap in Traspose Function - -Revision 1.3 2004/10/18 15:03:02 fiorin -Updated interface: all Matrix classes have now the same interface - -Revision 1.2 2004/07/13 06:48:26 cignoni -removed uppercase references in include - -Revision 1.1 2004/05/28 13:09:05 ganovelli -created - -Revision 1.1 2004/05/28 13:00:39 ganovelli -created - - ****************************************************************************/ +// #ifndef VCG_USE_EIGEN +#include "deprecated_matrix33.h" +// #endif #ifndef __VCGLIB_MATRIX33_H #define __VCGLIB_MATRIX33_H -#include -#include -#include -#include -#include +#include "eigen.h" + +namespace vcg{ +template class Matrix33; +} + +namespace Eigen{ +template +struct ei_traits > : ei_traits > {}; +} namespace vcg { -template -class Matrix33Diag:public Point3{ -public: - /** @name Matrix33 - Class Matrix33Diag. - This is the class for definition of a diagonal matrix 3x3. - @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){}; -}; - -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 +template +class Matrix33 : public Eigen::Matrix<_Scalar,3,3,RowMajor> // FIXME col or row major ? { + + typedef Eigen::Matrix<_Scalar,Eigen::Dynamic,Eigen::Dynamic> _Base; + public: - typedef S ScalarType; + + _EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix,_Base); + typedef _Scalar ScalarType; + + VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Matrix) /// Default constructor - inline Matrix33() {} + inline Matrix33() : Base() {} /// Copy constructor - Matrix33( const Matrix33 & m ) - { - for(int i=0;i<9;++i) - a[i] = m.a[i]; - } + Matrix33(const Matrix33& m ) : Base(m) {} - /// create from array - Matrix33( const S * v ) - { - for(int i=0;i<9;++i) a[i] = v[i]; - } + /// create from a \b row-major array + Matrix33(const Scalar * v ) : Base(Map(v)) {} /// 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; - } - } + Matrix33(const Matrix44 & m, const int & k) : Base(m.minor(k,k)) {} - /// Number of columns - inline unsigned int ColumnsNumber() 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; - } - - - - /// 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 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 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 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 matrice - void operator *=( const Matrix33< S> & t ) - { - int i,j; - for(i=0;i<3;++i) - for(j=0;j<3;++j) - (*this)[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j]; - - } - - /// 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]; - - return r; - } - - /// Dot product modifier with a diagonal matrix - void operator *=( const Matrix33Diag< S> & t ) - { - int i,j; - for(i=0;i<3;++i) - for(j=0;j<3;++j) - (*this)[i][j] = (*this)[i][j]*t[j]; - } - - /// 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; - - 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]; - - 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]; - - 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}$ + /*! + * \deprecated use *this.row(i) */ - Point3 operator * ( const Point3 & v ) const - { - Point3 t; + inline typename Base::RowXpr operator[](const unsigned int i) + { return Base::row(i); } - 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; - } + /*! + * \deprecated use *this.row(i) + */ + inline const typename Base::RowXpr operator[](const unsigned int i) const + { return Base::row(i); } - 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); - } + /** \deprecated */ 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; + *this = Eigen::AngleAxis(angle,axis).toRotationMatrix(); return (*this); } + /** \deprecated */ 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 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 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 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 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]) ; - } // 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; - - return *this; - } + /** \deprecated */ + Matrix33 & FastInvert() { return *this = inverse(); } 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]); + printf("| %g \t%g \t%g |\n",coeff(i,0),coeff(i,1),coeff(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]; -} - -/* 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)); -} - - -/* -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.Zero(); - 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; + /* + 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) -{ - SetZero(); - 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; + /* Compute the Frobenius Norm of the Matrix + */ + ScalarType Norm() + { + // FIXME looks like there is a bug: j is not used !!! + 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)); } - 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) -{ - SetZero(); - 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); + /** + It computes the covariance matrix of a set of 3d points. Returns the barycenter + */ + // FIXME should be outside Matrix + 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; + } } - 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); + /** + It computes 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 + + */ + // FIXME should be outside Matrix + template + void CrossCovariance(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; + 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; } -} -private: - S a[9]; + template + void WeightedCrossCovariance(const STLREALCONTAINER & weights, + 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; + + 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); + } + } }; 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; - } +void Invert(Matrix33 &m) { m = m.lu().inverse(); } 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 Inverse(const Matrix33&m) { return m.lu().inverse(); } ///given 2 vector centered into origin calculate the rotation matrix from first to the second template diff --git a/vcg/math/matrix44.h b/vcg/math/matrix44.h index 037846f8..0b6b0e26 100644 --- a/vcg/math/matrix44.h +++ b/vcg/math/matrix44.h @@ -20,94 +20,28 @@ * for more details. * * * ****************************************************************************/ -/**************************************************************************** - History -$Log: not supported by cvs2svn $ -Revision 1.34 2007/07/12 06:42:01 cignoni -added the missing static Construct() member - -Revision 1.33 2007/07/03 16:06:48 corsini -add DCM to Euler Angles conversion - -Revision 1.32 2007/03/08 14:39:27 corsini -final fix to euler angles transformation - -Revision 1.31 2007/02/06 09:57:40 corsini -fix euler angles computation - -Revision 1.30 2007/02/05 14:16:33 corsini -add from euler angles to rotation matrix conversion - -Revision 1.29 2005/12/02 09:46:49 croccia -Corrected bug in == and != Matrix44 operators - -Revision 1.28 2005/06/28 17:42:47 ganovelli -added Matrix44Diag - -Revision 1.27 2005/06/17 05:28:47 cignoni -Completed Shear Matrix code and comments, -Added use of swap inside Transpose -Added more complete comments on the usage of Decompose - -Revision 1.26 2005/06/10 15:04:12 cignoni -Added Various missing functions: SetShearXY, SetShearXZ, SetShearYZ, SetScale for point3 and Decompose -Completed *=(scalar); made uniform GetRow and GetColumn - -Revision 1.25 2005/04/14 11:35:09 ponchio -*** empty log message *** - -Revision 1.24 2005/03/18 00:14:39 cignoni -removed small gcc compiling issues - -Revision 1.23 2005/03/15 11:40:56 cignoni -Added operator*=( std::vector ...) to apply a matrix to a vector of vertexes (replacement of the old style mesh.Apply(tr). - -Revision 1.22 2004/12/15 18:45:50 tommyfranken -*** empty log message *** - -Revision 1.21 2004/10/22 14:41:30 ponchio -return in operator+ added. - -Revision 1.20 2004/10/18 15:03:14 fiorin -Updated interface: all Matrix classes have now the same interface - -Revision 1.19 2004/10/07 14:23:57 ganovelli -added function to take rows and comlumns. Added toMatrix and fromMatrix to comply -RotationTYpe prototype in Similarity.h - -Revision 1.18 2004/05/28 13:01:50 ganovelli -changed scalar to ScalarType - -Revision 1.17 2004/05/26 15:09:32 cignoni -better comments in set rotate - -Revision 1.16 2004/05/07 10:05:50 cignoni -Corrected abuse of for index variable scope - -Revision 1.15 2004/05/04 23:19:41 cignoni -Clarified initial comment, removed vector*matrix operator (confusing!) -Corrected translate and Rotate, removed gl stuff. - -Revision 1.14 2004/05/04 02:34:03 ganovelli -wrong use of operator [] corrected - -Revision 1.13 2004/04/07 10:45:54 cignoni -Added: [i][j] access, V() for the raw float values, constructor from T[16] - -Revision 1.12 2004/03/25 14:57:49 ponchio - -****************************************************************************/ +// #ifndef VCG_USE_EIGEN +#include "deprecated_matrix44.h" +// #endif #ifndef __VCGLIB_MATRIX44 #define __VCGLIB_MATRIX44 -#include -#include +#include "eigen.h" #include #include +#include #include +namespace vcg{ +template class Matrix44; +} + +namespace Eigen{ +template +struct ei_traits > : ei_traits > {}; +} namespace vcg { @@ -145,110 +79,49 @@ for 'column' vectors. */ - template -class Matrix44Diag:public Point4{ + +/** This class represents a 4x4 matrix. T is the kind of element in the matrix. + */ +template +class Matrix44 : public Eigen::Matrix<_Scalar,4,4,RowMajor> // FIXME col or row major ! +{ + + typedef Eigen::Matrix<_Scalar,Eigen::Dynamic,Eigen::Dynamic> _Base; + public: - /** @name Matrix33 - Class Matrix33Diag. - This is the class for definition of a diagonal matrix 4x4. - @param S (Templete Parameter) Specifies the ScalarType field. -*/ - Matrix44Diag(const S & p0,const S & p1,const S & p2,const S & p3):Point4(p0,p1,p2,p3){}; - Matrix44Diag( const Point4 & p ):Point4(p){}; -}; + _EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix,_Base); + typedef _Scalar ScalarType; - /** This class represent a 4x4 matrix. T is the kind of element in the matrix. - */ -template class Matrix44 { -protected: - T _a[16]; + VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Matrix) -public: - typedef T ScalarType; - -///@{ - - /** $name Constructors + /** \name Constructors * No automatic casting and default constructor is empty */ - Matrix44() {}; - ~Matrix44() {}; - Matrix44(const Matrix44 &m); - Matrix44(const T v[]); + Matrix44() : Base() {} + ~Matrix44() {} + Matrix44(const Matrix44 &m) : Base(m) {} + Matrix33(const Scalar * v ) : Base(Map(v)) {} - /// Number of columns - inline unsigned int ColumnsNumber() const - { - return 4; - }; - - /// Number of rows - inline unsigned int RowsNumber() const - { - return 4; - }; - - T &ElementAt(const int row, const int col); - T ElementAt(const int row, const int col) const; - //T &operator[](const int i); - //const T &operator[](const int i) const; - T *V(); - const T *V() const ; - - T *operator[](const int i); - const T *operator[](const int i) const; + Scalar *V() { return Base::data(); } + const Scalar *V() const { return Base::data(); } // return a copy of the i-th column - Point4 GetColumn4(const int& i)const{ - assert(i>=0 && i<4); - return Point4(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i),ElementAt(3,i)); - //return Point4(_a[i],_a[i+4],_a[i+8],_a[i+12]); - } + typename Base::ColXpr GetColumn4(const int& i) const { return col(i); } + Block GetColumn3(const int& i) const { return block<3,1>(0,i); } - Point3 GetColumn3(const int& i)const{ - assert(i>=0 && i<4); - return Point3(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i)); - } - - Point4 GetRow4(const int& i)const{ - assert(i>=0 && i<4); - return Point4(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2),ElementAt(i,3)); - // return *((Point4*)(&_a[i<<2])); alternativa forse + efficiente - } - - Point3 GetRow3(const int& i)const{ - assert(i>=0 && i<4); - return Point3(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2)); - // return *((Point4*)(&_a[i<<2])); alternativa forse + efficiente - } - - Matrix44 operator+(const Matrix44 &m) const; - Matrix44 operator-(const Matrix44 &m) const; - Matrix44 operator*(const Matrix44 &m) const; - Matrix44 operator*(const Matrix44Diag &m) const; - Point4 operator*(const Point4 &v) const; - - bool operator==(const Matrix44 &m) const; - bool operator!= (const Matrix44 &m) const; - - Matrix44 operator-() const; - Matrix44 operator*(const T k) const; - void operator+=(const Matrix44 &m); - void operator-=(const Matrix44 &m); - void operator*=( const Matrix44 & m ); - void operator*=( const T k ); + typename Base::RowXpr GetRow4(const int& i) const { return col(i); } + Block GetRow3(const int& i) const { return block<1,3>(i,0); } template - void ToMatrix(Matrix44Type & m) const {for(int i = 0; i < 16; i++) m.V()[i]=V()[i];} + void ToMatrix(Matrix44Type & m) const { m = (*this).cast(); } - void ToEulerAngles(T &alpha, T &beta, T &gamma); + void ToEulerAngles(Scalar &alpha, Scalar &beta, Scalar &gamma); template - void FromMatrix(const Matrix44Type & m){for(int i = 0; i < 16; i++) V()[i]=m.V()[i];} + void FromMatrix(const Matrix44Type & m) { for(int i = 0; i < 16; i++) data()[i] = m.data()[i]; } + void FromEulerAngles(T alpha, T beta, T gamma); - void SetZero(); - void SetIdentity(); void SetDiagonal(const T k); Matrix44 &SetScale(const T sx, const T sy, const T sz); Matrix44 &SetScale(const Point3 &t); @@ -274,12 +147,6 @@ public: Matrix44 tmp; tmp.FromMatrix(b); return tmp; } - - static inline const Matrix44 &Identity( ) - { - static Matrix44 tmp; tmp.SetIdentity(); - return tmp; - } }; @@ -289,7 +156,7 @@ public: template class LinearSolve: public Matrix44 { public: LinearSolve(const Matrix44 &m); - Point4 Solve(const Point4 &b); // solve A · x = b + Point4 Solve(const Point4 &b); // solve A � x = b ///If you need to solve some equation you can use this function instead of Matrix44 one for speed. T Determinant() const; protected: diff --git a/vcg/math/polar_decomposition.h b/vcg/math/polar_decomposition.h index 053cdf30..97b51ab8 100644 --- a/vcg/math/polar_decomposition.h +++ b/vcg/math/polar_decomposition.h @@ -62,7 +62,11 @@ void RotationalPartByPolarDecomposition( const vcg::Matrix33 & m, vcg::Matrix e[0]=math::Sqrt(e[0]); e[1]=math::Sqrt(e[1]); e[2]=math::Sqrt(e[2]); + #ifdef VCG_USE_EIGEN + tmp = tmp*e.asDiagonal()*res; + #else tmp = tmp*Matrix33Diag(e)*res; + #endif bool s1 = SingularValueDecomposition >(tmp,&e[0],res); tmp.Transpose(); @@ -70,7 +74,11 @@ void RotationalPartByPolarDecomposition( const vcg::Matrix33 & m, vcg::Matrix e[1]=1/e[1]; e[2]=1/e[2]; + #ifdef VCG_USE_EIGEN + tmp = res*e.asDiagonal()*tmp; + #else tmp = res*Matrix33Diag(e)*tmp; + #endif r = m*tmp; } diff --git a/vcg/space/box.h b/vcg/space/box.h index 9ee8b1b6..20523d16 100644 --- a/vcg/space/box.h +++ b/vcg/space/box.h @@ -311,8 +311,8 @@ public: /// sets a point to Zero inline void Zero() { - _min.Zero(); - _max.Zero(); + _min.SetZero(); + _max.SetZero(); } inline Box operator + ( Box const & p) const { diff --git a/vcg/space/fitting3.h b/vcg/space/fitting3.h index 23abc1ae..e9c34922 100644 --- a/vcg/space/fitting3.h +++ b/vcg/space/fitting3.h @@ -52,7 +52,7 @@ Point3 PlaneFittingPoints( std::vector< Point3 > & samples,Plane3 &p){ Matrix44 m;m.SetZero(); typename std::vector< Point3 > ::iterator i; - Point3 c; c.Zero(); + Point3 c; c.SetZero(); for(i = samples.begin(); i != samples.end(); ++i) c+=*i; c/=samples.size(); @@ -147,7 +147,7 @@ bool PlaneFittingPointsOld( std::vector< Point3 > & samples, Plane3 & p ) m[i][j]=P[i][j]; -// Point4 s;s.Zero(); +// Point4 s;s.SetZero(); // // s.Normalize(); // printf("\n RES %f %f %f %f \n",s[0],s[1],s[2],s[3]); diff --git a/vcg/space/index/perfect_spatial_hashing.h b/vcg/space/index/perfect_spatial_hashing.h index a7798d5a..a0dad757 100644 --- a/vcg/space/index/perfect_spatial_hashing.h +++ b/vcg/space/index/perfect_spatial_hashing.h @@ -1038,7 +1038,7 @@ namespace vcg { object = NULL; distance = ScalarType(-1.0); - nearest_point.Zero(); + nearest_point.SetZero(); } diff --git a/vcg/space/normal_extrapolation.h b/vcg/space/normal_extrapolation.h index f7f22668..172b5e71 100644 --- a/vcg/space/normal_extrapolation.h +++ b/vcg/space/normal_extrapolation.h @@ -82,7 +82,7 @@ namespace vcg // Plane structure: identify a plain as a pair struct Plane { - Plane() { center.Zero(); normal.Zero();}; + Plane() { center.SetZero(); normal.SetZero();}; // Object functor: return the bounding-box enclosing a given plane inline void GetBBox(BoundingBoxType &bb) { bb.Set(center); }; diff --git a/wrap/gl/camera.h b/wrap/gl/camera.h index 1ecea174..4fa8c719 100644 --- a/wrap/gl/camera.h +++ b/wrap/gl/camera.h @@ -974,7 +974,7 @@ static void SetSubView(vcg::Camera & camera,vcg::Point2 p0,S nearDist, S f // // // prendi la matrice di proiezione // Matrix44d P; -// P.Zero(); +// P.SetZero(); // // if(!IsOrtho())// prospettica // { @@ -1047,7 +1047,7 @@ static void SetSubView(vcg::Camera & camera,vcg::Point2 p0,S nearDist, S f // // // prendi la matrice di proiezione // Matrix44d P; -// P.Zero(); +// P.SetZero(); // // if(!IsOrtho())// prospettica // {