From da77800d0214c9c78ef9f71abe7ce43b42cd9dc3 Mon Sep 17 00:00:00 2001 From: alemuntoni Date: Tue, 19 Oct 2021 12:45:05 +0200 Subject: [PATCH] remove memset from simple_temporary_data and matrix44, remove old_matrix --- CMakeLists.txt | 5 - .../algorithms/create/marching_cubes.h | 1 - vcg/container/simple_temporary_data.h | 23 +- vcg/math/matrix44.h | 16 +- vcg/math/old_deprecated_matrix.h | 786 ------------------ vcg/math/old_lin_algebra.h | 634 -------------- vcg/math/old_matrix.h | 184 ---- vcg/math/old_matrix33.h | 298 ------- vcg/math/old_matrix44.h | 493 ----------- 9 files changed, 21 insertions(+), 2419 deletions(-) delete mode 100644 vcg/math/old_deprecated_matrix.h delete mode 100644 vcg/math/old_lin_algebra.h delete mode 100644 vcg/math/old_matrix.h delete mode 100644 vcg/math/old_matrix33.h delete mode 100644 vcg/math/old_matrix44.h diff --git a/CMakeLists.txt b/CMakeLists.txt index a4f6c76d..e48b6816 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -232,18 +232,13 @@ set(VCG_HEADERS vcg/math/linear.h vcg/math/matrix44.h vcg/math/eigen.h - vcg/math/old_lin_algebra.h vcg/math/similarity2.h vcg/math/gen_normal.h - vcg/math/old_matrix44.h - vcg/math/old_deprecated_matrix.h - vcg/math/old_matrix33.h vcg/math/polar_decomposition.h vcg/math/base.h vcg/math/histogram.h vcg/math/legendre.h vcg/math/matrix33.h - vcg/math/old_matrix.h vcg/simplex/edge/distance.h vcg/simplex/edge/topology.h vcg/simplex/edge/pos.h diff --git a/vcg/complex/algorithms/create/marching_cubes.h b/vcg/complex/algorithms/create/marching_cubes.h index 43e31cba..cbc8f5dd 100644 --- a/vcg/complex/algorithms/create/marching_cubes.h +++ b/vcg/complex/algorithms/create/marching_cubes.h @@ -674,7 +674,6 @@ namespace vcg { vp = NULL; vertices_idx.fill(-1); - //memset(vertices_idx, -1, 3*sizeof(size_t)); for (int vert=0; vert<3; vert++, trig++) //ok { diff --git a/vcg/container/simple_temporary_data.h b/vcg/container/simple_temporary_data.h index 9b175faf..13db925e 100644 --- a/vcg/container/simple_temporary_data.h +++ b/vcg/container/simple_temporary_data.h @@ -79,17 +79,18 @@ public: datareserve = sz; } - void resize(size_t sz) - { - int oldDatasize = datasize; - if ((int)sz <= oldDatasize) - return; - if (sz > datareserve) - reserve(sz); - datasize = sz; - memset(&booldata[oldDatasize], 0, datasize - oldDatasize); - } - void push_back(const bool &v) + void resize(size_t sz) + { + int oldDatasize = datasize; + if ((int) sz <= oldDatasize) + return; + if (sz > datareserve) + reserve(sz); + datasize = sz; + for (unsigned int i = oldDatasize; i < datasize; ++i) + booldata[i] = false; + } + void push_back(const bool &v) { resize(datasize + 1); booldata[datasize] = v; diff --git a/vcg/math/matrix44.h b/vcg/math/matrix44.h index 11d228d9..bd9d3a69 100644 --- a/vcg/math/matrix44.h +++ b/vcg/math/matrix44.h @@ -73,7 +73,7 @@ for 'column' vectors. */ template class Matrix44 { protected: - T _a[16]; + std::array _a; public: typedef T ScalarType; @@ -258,7 +258,9 @@ typedef Matrix44 Matrix44d; //} template Matrix44::Matrix44(const T v[]) { - memcpy((T *)_a, v, 16 * sizeof(T)); +// memcpy((T *)_a, v, 16 * sizeof(T)); + for (unsigned int i = 0; i < 16; ++i) + _a[i] = v[i]; } template T &Matrix44::ElementAt(const int row, const int col) { @@ -284,15 +286,15 @@ template T Matrix44::ElementAt(const int row, const int col) const //} template T *Matrix44::operator[](const int i) { assert(i >= 0 && i < 4); - return _a+i*4; + return &_a[i*4]; } template const T *Matrix44::operator[](const int i) const { assert(i >= 0 && i < 4); - return _a+i*4; + return &_a[i*4]; } -template T *Matrix44::V() { return _a;} -template const T *Matrix44::V() const { return _a;} +template T *Matrix44::V() { return _a.data();} +template const T *Matrix44::V() const { return _a.data();} template Matrix44 Matrix44::operator+(const Matrix44 &m) const { @@ -421,7 +423,7 @@ void Matrix44::FromEulerAngles(T alpha, T beta, T gamma) } template void Matrix44::SetZero() { - memset((T *)_a, 0, 16 * sizeof(T)); + _a.fill(0); } template void Matrix44::SetIdentity() { diff --git a/vcg/math/old_deprecated_matrix.h b/vcg/math/old_deprecated_matrix.h deleted file mode 100644 index b0f232e1..00000000 --- a/vcg/math/old_deprecated_matrix.h +++ /dev/null @@ -1,786 +0,0 @@ -/**************************************************************************** -* VCGLib o o * -* Visual and Computer Graphics Library o o * -* _ O _ * -* Copyright(C) 2004-2016 \/)\/ * -* 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 - -namespace vcg{ - namespace ndim{ - - /** \addtogroup math */ - /* @{ */ - - /*! - * This class represent a diagonal m�m matrix. - */ - - class MatrixDiagBase{public: - virtual const int & Dimension()const =0; - virtual 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(TYPE(-1.0), TYPE(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<_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<_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 -#include -#include -#ifndef _YES_I_WANT_TO_USE_DANGEROUS_STUFF -#error "Please do not never user this file. Use EIGEN!!!!" -#endif -namespace vcg -{ - /** \addtogroup math */ - /* @{ */ - - /*! - * - */ - template< typename MATRIX_TYPE > - static void JacobiRotate(MATRIX_TYPE &A, typename MATRIX_TYPE::ScalarType s, typename MATRIX_TYPE::ScalarType tau, int i,int j,int k,int l) - { - typename MATRIX_TYPE::ScalarType g=A[i][j]; - typename MATRIX_TYPE::ScalarType h=A[k][l]; - A[i][j]=g-s*(h+g*tau); - A[k][l]=h+s*(g-h*tau); - }; - - /*! - * Computes all eigenvalues and eigenvectors of a real symmetric matrix . - * On output, elements of the input matrix above the diagonal are destroyed. - * \param d returns the eigenvalues of a. - * \param v is a matrix whose columns contain, the normalized eigenvectors - * \param nrot returns the number of Jacobi rotations that were required. - */ - template - static void Jacobi(MATRIX_TYPE &w, POINT_TYPE &d, MATRIX_TYPE &v, int &nrot) - { - typedef typename MATRIX_TYPE::ScalarType ScalarType; - assert(w.RowsNumber()==w.ColumnsNumber()); - int dimension = w.RowsNumber(); - - int j,iq,ip,i; - //assert(w.IsSymmetric()); - typename MATRIX_TYPE::ScalarType tresh, theta, tau, t, sm, s, h, g, c; - POINT_TYPE b, z; - - v.SetIdentity(); - - for (ip=0;ip4 && (float)(fabs(d[ip])+g) == (float)fabs(d[ip]) && (float)(fabs(d[iq])+g) == (float)fabs(d[iq])) - w[ip][iq]=ScalarType(0.0); - else if (fabs(w[ip][iq]) > tresh) - { - h=d[iq]-d[ip]; - if ((float)(fabs(h)+g) == (float)fabs(h)) - t=(w[ip][iq])/h; //t =1/(2#) - else - { - theta=ScalarType(0.5)*h/(w[ip][iq]); //Equation (11.1.10). - t=ScalarType(1.0)/(fabs(theta)+sqrt(ScalarType(1.0)+theta*theta)); - if (theta < ScalarType(0.0)) t = -t; - } - c=ScalarType(1.0)/sqrt(ScalarType(1.0)+t*t); - s=t*c; - tau=s/(ScalarType(1.0)+c); - h=t*w[ip][iq]; - z[ip] -= h; - z[iq] += h; - d[ip] -= h; - d[iq] += h; - w[ip][iq]=ScalarType(0.0); - for (j=0;j<=ip-1;j++) { //Case of rotations 1 <= j < p. - JacobiRotate(w,s,tau,j,ip,j,iq) ; - } - for (j=ip+1;j<=iq-1;j++) { //Case of rotations p < j < q. - JacobiRotate(w,s,tau,ip,j,j,iq); - } - for (j=iq+1;j(w,s,tau,ip,j,iq,j); - } - for (j=0;j(v,s,tau,j,ip,j,iq); - } - ++nrot; - } - } - } - for (ip=0;ip - void SortEigenvaluesAndEigenvectors(POINT_TYPE &eigenvalues, MATRIX_TYPE &eigenvectors, bool absComparison = false) - { - assert(eigenvectors.ColumnsNumber()==eigenvectors.RowsNumber()); - int dimension = eigenvectors.ColumnsNumber(); - int i, j, k; - float p,q; - for (i=0; i= p) - { - p = q; - k = j; - } - p = eigenvalues[k]; - } - else - { - p = eigenvalues[ k=i ]; - for (j=i+1; j= p) - p = eigenvalues[ k=j ]; - } - - if (k != i) - { - eigenvalues[k] = eigenvalues[i]; // i.e. - eigenvalues[i] = p; // swaps the value of the elements i-th and k-th - - for (j=0; j - inline static TYPE sqr(TYPE a) - { - TYPE sqr_arg = a; - return (sqr_arg == 0 ? 0 : sqr_arg*sqr_arg); - } - - // Computes (a^2 + b^2)^(1/2) without destructive underflow or overflow. - template - inline static TYPE pythagora(TYPE a, TYPE b) - { - TYPE abs_a = fabs(a); - TYPE abs_b = fabs(b); - if (abs_a > abs_b) - return abs_a*sqrt((TYPE)1.0+sqr(abs_b/abs_a)); - else - return (abs_b == (TYPE)0.0 ? (TYPE)0.0 : abs_b*sqrt((TYPE)1.0+sqr(abs_a/abs_b))); - } - - template - inline static TYPE sign(TYPE a, TYPE b) - { - return (b >= 0.0 ? fabs(a) : -fabs(a)); - } - - /*! - * - */ - enum SortingStrategy {LeaveUnsorted=0, SortAscending=1, SortDescending=2}; - template< typename MATRIX_TYPE > - void Sort(MATRIX_TYPE &U, typename MATRIX_TYPE::ScalarType W[], MATRIX_TYPE &V, const SortingStrategy sorting) ; - - - /*! - * Given a matrix Amxn, this routine computes its singular value decomposition, - * i.e. A=UxWxVT. The matrix A will be destroyed! - * (This is the implementation described in Numerical Recipies). - * \param A the matrix to be decomposed - * \param W the diagonal matrix of singular values W, stored as a vector W[1...N] - * \param V the matrix V (not the transpose VT) - * \param max_iters max iteration number (default = 30). - * \return - */ - template - static bool SingularValueDecomposition(MATRIX_TYPE &A, typename MATRIX_TYPE::ScalarType *W, MATRIX_TYPE &V, const SortingStrategy sorting=LeaveUnsorted, const int max_iters=30) - { - typedef typename MATRIX_TYPE::ScalarType ScalarType; - int m = (int) A.RowsNumber(); - int n = (int) A.ColumnsNumber(); - int flag,i,its,j,jj,k,l,nm; - ScalarType anorm, c, f, g, h, s, scale, x, y, z, *rv1; - bool convergence = true; - - rv1 = new ScalarType[n]; - g = scale = anorm = 0; - // Householder reduction to bidiagonal form. - for (i=0; i( sqrt(s), f ); - h = f*g - s; - A[i][i]=f-g; - for (j=l; j(sqrt(s),f); - h = f*g - s; - A[i][l] = f-g; - for (k=l; k=0; i--) - { - //Accumulation of right-hand transformations. - if (i < (n-1)) - { - if (g) - { - for (j=l; j=0; i--) - { - l = i+1; - g = W[i]; - for (j=l; j=0; k--) - { - for (its=1; its<=max_iters; its++) - { - flag=1; - for (l=k; l>=0; l--) - { - // Test for splitting. - nm=l-1; - // Note that rv1[1] is always zero. - if ((double)(fabs(rv1[l])+anorm) == anorm) - { - flag=0; - break; - } - if ((double)(fabs(W[nm])+anorm) == anorm) - break; - } - if (flag) - { - c=0.0; //Cancellation of rv1[l], if l > 1. - s=1.0; - for (i=l ;i<=k; i++) - { - f = s*rv1[i]; - rv1[i] = c*rv1[i]; - if ((double)(fabs(f)+anorm) == anorm) - break; - g = W[i]; - h = pythagora(f,g); - W[i] = h; - h = (ScalarType)1.0/h; - c = g*h; - s = -f*h; - for (j=0; j(f,1.0); - f=((x-z)*(x+z) + h*((y/(f+sign(g,f)))-h))/x; - c=s=1.0; - //Next QR transformation: - for (j=l; j<= nm;j++) - { - i = j+1; - g = rv1[i]; - y = W[i]; - h = s*g; - g = c*g; - z = pythagora(f,h); - rv1[j] = z; - c = f/z; - s = h/z; - f = x*c + g*s; - g = g*c - x*s; - h = y*s; - y *= c; - for (jj=0; jj(f,h); - W[j] = z; - // Rotation can be arbitrary if z = 0. - if (z) - { - z = (ScalarType)1.0/z; - c = f*z; - s = h*z; - } - f = c*g + s*y; - x = c*y - s*g; - for (jj=0; jj(A, W, V, sorting); - - return convergence; - }; - - - /*! - * Sort the singular values computed by the SingularValueDecomposition procedure and - * modify the matrices U and V accordingly. - */ - // TODO modify the last parameter type - template< typename MATRIX_TYPE > - void Sort(MATRIX_TYPE &U, typename MATRIX_TYPE::ScalarType W[], MATRIX_TYPE &V, const SortingStrategy sorting) - { - typedef typename MATRIX_TYPE::ScalarType ScalarType; - - assert(U.ColumnsNumber()==V.ColumnsNumber()); - - int mu = U.RowsNumber(); - int mv = V.RowsNumber(); - int n = U.ColumnsNumber(); - - //ScalarType* u = &U[0][0]; - //ScalarType* v = &V[0][0]; - - for (int i=0; i p) - { - k = j; - p = W[j]; - } - } - break; - } - case LeaveUnsorted: break; // nothing to do. - } - if (k != i) - { - W[k] = W[i]; // i.e. - W[i] = p; // swaps the i-th and the k-th elements - - int j = mu; - //ScalarType* uji = u + i; // uji = &U[0][i] - //ScalarType* ujk = u + k; // ujk = &U[0][k] - //ScalarType* vji = v + i; // vji = &V[0][i] - //ScalarType* vjk = v + k; // vjk = &V[0][k] - //if (j) - //{ - // for(;;) for( ; j!=0; --j, uji+=n, ujk+=n) - // { { - // p = *uji; p = *uji; // i.e. - // *uji = *ujk; *uji = *ujk; // swap( U[s][i], U[s][k] ) - // *ujk = p; *ujk = p; // - // if (!(--j)) } - // break; - // uji += n; - // ujk += n; - // } - //} - for(int s=0; j!=0; ++s, --j) - std::swap(U[s][i], U[s][k]); - - j = mv; - //if (j!=0) - //{ - // for(;;) for ( ; j!=0; --j, vji+=n, ujk+=n) - // { { - // p = *vji; p = *vji; // i.e. - // *vji = *vjk; *vji = *vjk; // swap( V[s][i], V[s][k] ) - // *vjk = p; *vjk = p; // - // if (!(--j)) } - // break; - // vji += n; - // vjk += n; - // } - //} - for (int s=0; j!=0; ++s, --j) - std::swap(V[s][i], V[s][k]); - } - } - } - - - /*! - * Solves AxX = B for a vector X, where A is specified by the matrices Umxn, - * Wnx1 and Vnxn as returned by SingularValueDecomposition. - * No input quantities are destroyed, so the routine may be called sequentially with different bxs. - * \param x is the output solution vector (xnx1) - * \param b is the input right-hand side (bnx1) - */ - template - static void SingularValueBacksubstitution(const MATRIX_TYPE &U, - const typename MATRIX_TYPE::ScalarType *W, - const MATRIX_TYPE &V, - typename MATRIX_TYPE::ScalarType *x, - const typename MATRIX_TYPE::ScalarType *b) - { - typedef typename MATRIX_TYPE::ScalarType ScalarType; - unsigned int jj, j, i; - unsigned int columns_number = U.ColumnsNumber(); - unsigned int rows_number = U.RowsNumber(); - ScalarType s; - ScalarType *tmp = new ScalarType[columns_number]; - for (j=0; j - -namespace vcg{ -namespace ndim{ -template class Matrix; -} -} - -namespace Eigen{ -template -struct ei_traits > : ei_traits > {}; -template struct ei_to_vcgtype -{ typedef vcg::ndim::Matrix type; }; -} - -namespace vcg{ -namespace ndim{ - -/** \addtogroup math */ -/* @{ */ - -/*! - * \deprecated use Matrix or Matrix or any typedef - * 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,Eigen::RowMajor> // FIXME col or row major ? -{ - typedef Eigen::Matrix<_Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> _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(int m, int n) - : Base(m,n) - { - memset(Base::data(), 0, m*n*sizeof(Scalar)); - } - - /*! - * 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(int m, 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 - */ - // FIXME what the hell is that ! - /*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)); - }; -}; - -typedef vcg::ndim::Matrix MatrixMNd; -typedef vcg::ndim::Matrix MatrixMNf; - -/*! @} */ - -template -void Invert(MatrixType & m) -{ - m = m.inverse(); -} - -} -} // end of namespace - -#endif - -#endif - diff --git a/vcg/math/old_matrix33.h b/vcg/math/old_matrix33.h deleted file mode 100644 index 0aebd8b0..00000000 --- a/vcg/math/old_matrix33.h +++ /dev/null @@ -1,298 +0,0 @@ -/**************************************************************************** -* VCGLib o o * -* Visual and Computer Graphics Library o o * -* _ O _ * -* Copyright(C) 2004-2016 \/)\/ * -* 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 VCG_USE_EIGEN -#include "deprecated_matrix33.h" -#else - -#ifndef __VCGLIB_MATRIX33_H -#define __VCGLIB_MATRIX33_H - -#include "eigen.h" -#include "matrix44.h" - -namespace vcg{ -template class Matrix33; -} - -namespace Eigen{ -template -struct ei_traits > : ei_traits > {}; -template struct ei_to_vcgtype -{ typedef vcg::Matrix33 type; }; -} - -namespace vcg { - -/** \deprecated use Matrix - @name Matrix33 - Class Matrix33. - This is the class for definition of a matrix 3x3. - @param S (Templete Parameter) Specifies the ScalarType field. -*/ -template -class Matrix33 : public Eigen::Matrix<_Scalar,3,3,Eigen::RowMajor> // FIXME col or row major ? -{ - - typedef Eigen::Matrix<_Scalar,3,3,Eigen::RowMajor> _Base; -public: - - using _Base::coeff; - using _Base::coeffRef; - using _Base::setZero; - - _EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix33,_Base); - typedef _Scalar ScalarType; - - VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Matrix33) - - /// Default constructor - inline Matrix33() : Base() {} - - /// Copy constructor - Matrix33(const Matrix33& m ) : Base(m) {} - - /// create from a \b row-major array - Matrix33(const Scalar * v ) : Base(Eigen::Map >(v)) {} - - /// create from Matrix44 excluding row and column k - Matrix33(const Matrix44 & m, const int & k) : Base(m.minor(k,k)) {} - - template - Matrix33(const Eigen::MatrixBase& other) : Base(other) {} - - /*! \deprecated use *this.row(i) */ - inline typename Base::RowXpr operator[](const unsigned int i) - { return Base::row(i); } - - /*! \deprecated use *this.row(i) */ - inline const typename Base::RowXpr operator[](const unsigned int i) const - { return Base::row(i); } - - /** \deprecated */ - Matrix33 & SetRotateRad(Scalar angle, const Point3 & axis ) - { - *this = Eigen::AngleAxis(angle,axis).toRotationMatrix(); - return (*this); - } - /** \deprecated */ - Matrix33 & SetRotateDeg(Scalar angle, const Point3 & axis ){ - return SetRotateRad(math::ToRad(angle),axis); - } - - // Warning, this Inversion code can be HIGHLY NUMERICALLY UNSTABLE! - // In most case you are advised to use the Invert() method based on SVD decomposition. - /** \deprecated */ - Matrix33 & FastInvert() { return *this = Base::inverse(); } - - void show(FILE * fp) - { - for(int i=0;i<3;++i) - printf("| %g \t%g \t%g |\n",coeff(i,0),coeff(i,1),coeff(i,2)); - } - - /** \deprecated use a * b.transpose() - compute the matrix generated by the product of a * b^T - */ - // hm.... this is the outer product - void ExternalProduct(const Point3 &a, const Point3 &b) { *this = a * b.transpose(); } - - /** Compute the Frobenius Norm of the Matrix */ - Scalar Norm() { return Base::cwise().abs2().sum(); } - - /** Computes the covariance matrix of a set of 3d points. Returns the barycenter. - */ - // FIXME should be outside Matrix - - - /** - 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; - } - - 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) { m = m.lu().inverse(); } - -template -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 -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.dot(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 - -#endif diff --git a/vcg/math/old_matrix44.h b/vcg/math/old_matrix44.h deleted file mode 100644 index d4a7ae9a..00000000 --- a/vcg/math/old_matrix44.h +++ /dev/null @@ -1,493 +0,0 @@ -/**************************************************************************** -* VCGLib o o * -* Visual and Computer Graphics Library o o * -* _ O _ * -* Copyright(C) 2004-2016 \/)\/ * -* 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 VCG_USE_EIGEN -#include "deprecated_matrix44.h" -#else - -#ifndef __VCGLIB_MATRIX44 -#define __VCGLIB_MATRIX44 - -#include "eigen.h" -#include -#include -#include -#include - -namespace vcg{ -template class Matrix44; -} - -namespace Eigen{ -template -struct ei_traits > : ei_traits > {}; -template struct ei_to_vcgtype -{ typedef vcg::Matrix44 type; }; -} - -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. - -*/ - -// Note that we have to pass Dim and HDim because it is not allowed to use a template -// parameter to define a template specialization. To be more precise, in the following -// specializations, it is not allowed to use Dim+1 instead of HDim. -template< typename Other, - int OtherRows=Eigen::ei_traits::RowsAtCompileTime, - int OtherCols=Eigen::ei_traits::ColsAtCompileTime> -struct ei_matrix44_product_impl; - -/** \deprecated use Eigen::Matrix (or the typedef) you want a real 4x4 matrix, or use Eigen::Transform if you want a transformation matrix for a 3D space (a Eigen::Transform is internally a 4x4 col-major matrix) - * - * This class represents a 4x4 matrix. T is the kind of element in the matrix. - */ -template -class Matrix44 : public Eigen::Matrix<_Scalar,4,4,Eigen::RowMajor> // FIXME col or row major ! -{ - - typedef Eigen::Matrix<_Scalar,4,4,Eigen::RowMajor> _Base; -public: - - using _Base::coeff; - using _Base::coeffRef; - using _Base::ElementAt; - using _Base::setZero; - - _EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix44,_Base); - typedef _Scalar ScalarType; - VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Matrix44) - - Matrix44() : Base() {} - ~Matrix44() {} - Matrix44(const Matrix44 &m) : Base(m) {} - Matrix44(const Scalar * v ) : Base(Eigen::Map >(v)) {} - template - Matrix44(const Eigen::MatrixBase& other) : Base(other) {} - - const typename Base::RowXpr operator[](int i) const { return Base::row(i); } - typename Base::RowXpr operator[](int i) { return Base::row(i); } - - typename Base::ColXpr GetColumn4(const int& i) const { return Base::col(i); } - const Eigen::Block GetColumn3(const int& i) const { return this->template block<3,1>(0,i); } - - typename Base::RowXpr GetRow4(const int& i) const { return Base::row(i); } - Eigen::Block GetRow3(const int& i) const { return this->template block<1,3>(i,0); } - - template - void ToMatrix(Matrix44Type & m) const { m = (*this).template cast(); } - - void ToEulerAngles(Scalar &alpha, Scalar &beta, Scalar &gamma); - - template - void FromMatrix(const Matrix44Type & m) { for(int i = 0; i < 16; i++) Base::data()[i] = m.data()[i]; } - - void FromEulerAngles(Scalar alpha, Scalar beta, Scalar gamma); - void SetDiagonal(const Scalar k); - Matrix44 &SetScale(const Scalar sx, const Scalar sy, const Scalar sz); - Matrix44 &SetScale(const Point3 &t); - Matrix44 &SetTranslate(const Point3 &t); - Matrix44 &SetTranslate(const Scalar sx, const Scalar sy, const Scalar sz); - Matrix44 &SetShearXY(const Scalar sz); - Matrix44 &SetShearXZ(const Scalar sy); - Matrix44 &SetShearYZ(const Scalar sx); - - ///use radiants for angle. - Matrix44 &SetRotateDeg(Scalar AngleDeg, const Point3 & axis); - Matrix44 &SetRotateRad(Scalar AngleRad, const Point3 & axis); - - /** taken from Eigen::Transform - * \returns the product between the transform \c *this and a matrix expression \a other - * - * The right hand side \a other might be either: - * \li a matrix expression with 4 rows - * \li a 3D vector/point - */ - template - inline const typename ei_matrix44_product_impl::ResultType - operator * (const Eigen::MatrixBase &other) const - { return ei_matrix44_product_impl::run(*this,other.derived()); } - - void print() {std::cout << *this << "\n\n";} - -}; - -//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 < 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::ToEulerAngles(Scalar &alpha, Scalar &beta, Scalar &gamma) -{ - alpha = atan2(coeff(1,2), coeff(2,2)); - beta = asin(-coeff(0,2)); - gamma = atan2(coeff(0,1), coeff(1,1)); -} - -template -void Matrix44::FromEulerAngles(Scalar alpha, Scalar beta, Scalar 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::SetDiagonal(const Scalar 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 Scalar sx, const Scalar sy, const Scalar 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 Scalar tx, const Scalar ty, const Scalar tz) { - Base::setIdentity(); - ElementAt(0, 3) = tx; - ElementAt(1, 3) = ty; - ElementAt(2, 3) = tz; - return *this; -} - -template Matrix44 &Matrix44::SetRotateDeg(Scalar AngleDeg, const Point3 & axis) { - return SetRotateRad(math::ToRad(AngleDeg),axis); -} - -template Matrix44 &Matrix44::SetRotateRad(Scalar 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 Scalar sh) {// shear the X coordinate as the Y coordinate change - Base::setIdentity(); - ElementAt(0,1) = sh; - return *this; - } - - template Matrix44 & Matrix44::SetShearXZ( const Scalar sh) {// shear the X coordinate as the Z coordinate change - Base::setIdentity(); - ElementAt(0,2) = sh; - return *this; - } - - template Matrix44 &Matrix44::SetShearYZ( const Scalar sh) {// shear the Y coordinate as the Z coordinate change - Base::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].dot(M.GetColumn3(1)); // xy shearing - R[1]= M.GetColumn3(1)-R[0]*ShearV[0]; - assert(math::Abs(R[1].dot(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].dot(M.GetColumn3(2)); // xz shearing - R[2]= M.GetColumn3(2)-R[0]*ShearV[1]; - assert(math::Abs(R[2].dot(R[0]))<1e-10); - - R[2] = R[2]-R[1]*(R[2].dot(R[1])); - assert(math::Abs(R[2].dot(R[1]))<1e-10); - assert(math::Abs(R[2].dot(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].dot(R[1]))<1e-10); - assert(math::Abs(R[2].dot(R[0]))<1e-10); - - ShearV[2]=R[1].dot(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; -} - -/* -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) { - return m = m.lu().inverse(); -} - -template Matrix44 Inverse(const Matrix44 &m) { - return m.lu().inverse(); -} - -template -struct ei_matrix44_product_impl -{ - typedef typename Other::Scalar Scalar; - typedef typename Eigen::ProductReturnType::Base,Other>::Type ResultType; - static ResultType run(const Matrix44& tr, const Other& other) - { return (static_cast::Base&>(tr)) * other; } -}; - -template -struct ei_matrix44_product_impl -{ - typedef typename Other::Scalar Scalar; - typedef Eigen::Matrix ResultType; - static ResultType run(const Matrix44& tr, const Other& p) - { - Scalar w; - Eigen::Matrix s; - s[0] = tr.ElementAt(0, 0)*p[0] + tr.ElementAt(0, 1)*p[1] + tr.ElementAt(0, 2)*p[2] + tr.ElementAt(0, 3); - s[1] = tr.ElementAt(1, 0)*p[0] + tr.ElementAt(1, 1)*p[1] + tr.ElementAt(1, 2)*p[2] + tr.ElementAt(1, 3); - s[2] = tr.ElementAt(2, 0)*p[0] + tr.ElementAt(2, 1)*p[1] + tr.ElementAt(2, 2)*p[2] + tr.ElementAt(2, 3); - w = tr.ElementAt(3, 0)*p[0] + tr.ElementAt(3, 1)*p[1] + tr.ElementAt(3, 2)*p[2] + tr.ElementAt(3, 3); - if(w!= 0) s /= w; - return s; - } -}; - -} //namespace -#endif - -#endif