manolas
/
vcglib
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

remove memset from simple_temporary_data and matrix44, remove old_matrix

This commit is contained in:
alemuntoni 2021-10-19 12:45:05 +02:00
parent 5e17997b37
commit da77800d02
9 changed files with 21 additions and 2419 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
CMakeLists.txt
View File

@ -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

1
vcg/complex/algorithms/create/marching_cubes.h
View File

@ -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
{

23
vcg/container/simple_temporary_data.h
View File

@ -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;

16
vcg/math/matrix44.h
View File

@ -73,7 +73,7 @@ for 'column' vectors.
*/
template <class T> class Matrix44 {
protected:
T _a[16];
std::array<T, 16> _a;
public:
typedef T ScalarType;
@ -258,7 +258,9 @@ typedef Matrix44<double> Matrix44d;
//}
template <class T> Matrix44<T>::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 <class T> T &Matrix44<T>::ElementAt(const int row, const int col) {
@ -284,15 +286,15 @@ template <class T> T Matrix44<T>::ElementAt(const int row, const int col) const
//}
template <class T> T *Matrix44<T>::operator[](const int i) {
assert(i >= 0 && i < 4);
return _a+i*4;
return &_a[i*4];
}
template <class T> const T *Matrix44<T>::operator[](const int i) const {
assert(i >= 0 && i < 4);
return _a+i*4;
return &_a[i*4];
}
template <class T> T *Matrix44<T>::V() { return _a;}
template <class T> const T *Matrix44<T>::V() const { return _a;}
template <class T> T *Matrix44<T>::V() { return _a.data();}
template <class T> const T *Matrix44<T>::V() const { return _a.data();}
template <class T> Matrix44<T> Matrix44<T>::operator+(const Matrix44 &m) const {
@ -421,7 +423,7 @@ void Matrix44<T>::FromEulerAngles(T alpha, T beta, T gamma)
}
template <class T> void Matrix44<T>::SetZero() {
memset((T *)_a, 0, 16 * sizeof(T));
_a.fill(0);
}
template <class T> void Matrix44<T>::SetIdentity() {

786
vcg/math/old_deprecated_matrix.h
View File

@ -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<TYPE>
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 <stdio.h>
#include <math.h>
#include <memory.h>
#include <assert.h>
#include <algorithm>
#include <vcg/space/point.h>
namespace vcg{
namespace ndim{
/** \addtogroup math */
/* @{ */
/*!
* This class represent a diagonal <I>m</I><EFBFBD><I>m</I> matrix.
*/
class MatrixDiagBase{public:
virtual const int & Dimension()const =0;
virtual float operator[](const int & i)const = 0;
};
template<int N, class S>
class MatrixDiag: public Point<N,S>, public MatrixDiagBase{
public:
const int & Dimension() const {return N;}
MatrixDiag(const Point<N,S>&p):Point<N,S>(p){}
};
/*!
* This class represent a generic <I>m</I><EFBFBD><I>n</I> matrix. The class is templated over the scalar type field.
* @param TYPE (Templete Parameter) Specifies the ScalarType field.
*/
template<class TYPE>
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<TYPE> &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<TYPE> &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<TYPE> &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>i</I>-th rows at the <I>j</I>-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 <I>A<SUB>i,j</SUB></I> of the <I>a<SUB>i,j</SUB></I> 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<TYPE> 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>i</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>i</I>-th matrix row
*/
inline const TYPE* operator[](const unsigned int i) const
{
assert(i<_rows);
return _data + i*_columns;
};
/*!
* Get the <I>j</I>-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>i</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>i</I>-th and the <I>j</I>-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>i</I>-th and the <I>j</I>-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<TYPE>& operator=(const Matrix<TYPE> &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 <I>m</I> to this matrix.
* \param m reference to matrix to add to this
* \return the matrix sum.
*/
Matrix<TYPE>& operator+=(const Matrix<TYPE> &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 <I>m</I> to this matrix.
* \param m reference to matrix to subtract
* \return the matrix difference.
*/
Matrix<TYPE>& operator-=(const Matrix<TYPE> &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 <I>k</I>.
* \param k the scalar constant
* \return the modified matrix
*/
Matrix<TYPE>& 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 <I>k</I>.
* \param k the scalar constant
* \return the modified matrix
*/
Matrix<TYPE>& 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 <I>k</I>.
* \param k the scalar constant
* \return the modified matrix
*/
Matrix<TYPE>& 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 <I>k</I>.
* \param k the scalar constant
* \return the modified matrix
*/
Matrix<TYPE>& 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<TYPE> operator*(const Matrix<TYPE> &m) const
{
assert(_columns == m._rows);
Matrix<TYPE> result(_rows, m._columns);
unsigned int i, j, k, p, q, r;
for (i=0, p=0, r=0; i<result._rows; i++, p+=_columns, r+=result._columns)
for (j=0; j<result._columns; j++)
{
ScalarType temp = 0;
for (k=0, q=0; k<_columns; k++, q+=m._columns)
temp+=(_data[p+k]*m._data[q+j]);
result._data[r+j] = temp;
}
return result;
};
/*!
* Matrix-Vector product. Computes the product of the matrix by the vector v.
* \param v reference to the vector to multiply by
* \return the matrix-vector product. This pointer must be deallocated by the caller
*/
ScalarType* operator*(const ScalarType v[]) const
{
ScalarType *result = new ScalarType[_rows];
memset(result, 0, _rows*sizeof(ScalarType));
unsigned int r, c, i;
for (r=0, i=0; r<_rows; r++)
for (c=0; c<_columns; c++, i++)
result[r] += _data[i]*v[c];
return result;
};
/*!
* Matrix multiplication: calculates the cross product.
* \param reference to the matrix to multiply by
* \return the matrix product
*/
template <int N,int M>
void DotProduct(Point<N,TYPE> &m,Point<M,TYPE> &result)
{
unsigned int i, j, p, r;
for (i=0, p=0, r=0; i<M; i++)
{ result[i]=0;
for (j=0; j<N; j++)
result[i]+=(*this)[i][j]*m[j];
}
};
/*!
* Matrix multiplication by a diagonal matrix
*/
Matrix<TYPE> operator*(const MatrixDiagBase &m) const
{
assert(_columns == _rows);
assert(_columns == m.Dimension());
int i,j;
Matrix<TYPE> result(_rows, _columns);
for (i=0; i<result._rows; i++)
for (j=0; j<result._columns; j++)
result[i][j]*= m[j];
return result;
};
/*!
* Matrix from outer product.
*/
template <int N, int M>
void OuterProduct(const Point<N,TYPE> a, const Point< M,TYPE> b)
{
assert(N == _rows);
assert(M == _columns);
Matrix<TYPE> result(_rows,_columns);
unsigned int i, j;
for (i=0; i<result._rows; i++)
for (j=0; j<result._columns; j++)
(*this)[i][j] = a[i] * b[j];
};
/*!
* Matrix-vector multiplication.
* \param reference to the 3-dimensional vector to multiply by
* \return the resulting vector
*/
Point3<TYPE> operator*(Point3<TYPE> &p) const
{
assert(_columns==3 && _rows==3);
vcg::Point3<TYPE> 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<TYPE> operator+(const TYPE k)
{
Matrix<TYPE> 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<TYPE> operator-(const TYPE k)
{
Matrix<TYPE> 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<TYPE> operator-() const
{
Matrix<TYPE> 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<TYPE> operator*(const TYPE k) const
{
Matrix<TYPE> 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<TYPE> operator/(const TYPE k)
{
Matrix<TYPE> 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 <I>j</I>-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>i</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 <I>v<SUB>i,i</SUB></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<m*n; i++)
_data[i] = 0;
};
/*!
* Matrix transposition operation: set the current matrix to its transpose
*/
void Transpose()
{
ScalarType *temp = new ScalarType[_rows*_columns];
unsigned int i, j, p, q;
for (i=0, p=0; i<_rows; i++, p+=_columns)
for (j=0, q=0; j<_columns; j++, q+=_rows)
temp[q+i] = _data[p+j];
std::swap(_columns, _rows);
std::swap(_data, temp);
delete []temp;
};
// for the transistion to eigen
Matrix transpose()
{
Matrix res = *this;
res.Transpose();
return res;
}
void transposeInPlace() { Transpose(); }
// for the transistion to eigen
/*!
* Print all matrix elements
*/
void Dump()
{
unsigned int i, j, p;
for (i=0, p=0; i<_rows; i++, p+=_columns)
{
printf("[\t");
for (j=0; j<_columns; j++)
printf("%f\t", _data[p+j]);
printf("]\n");
}
printf("\n");
};
protected:
/// the number of matrix rows
unsigned int _rows;
/// the number of matrix rows
unsigned int _columns;
/// the matrix elements
ScalarType *_data;
};
typedef vcg::ndim::Matrix<double> MatrixMNd;
typedef vcg::ndim::Matrix<float> MatrixMNf;
/*! @} */
// template <class MatrixType>
// void Invert(MatrixType & m){
// typedef typename MatrixType::ScalarType X;
// X *diag;
// diag = new X [m.ColumnsNumber()];
// MatrixType res(m.RowsNumber(),m.ColumnsNumber());
// vcg::SingularValueDecomposition<MatrixType > (m,&diag[0],res,LeaveUnsorted,50 );
// m.Transpose();
// // prodotto per la diagonale
// unsigned int i,j;
// for (i=0; i<m.RowsNumber(); i++)
// for (j=0; j<m.ColumnsNumber(); j++)
// res[i][j]/= diag[j];
// m = res *m;
// }
}
}; // end of namespace
#endif

634
vcg/math/old_lin_algebra.h
View File

@ -1,634 +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 __VCGLIB_LINALGEBRA_H
#define __VCGLIB_LINALGEBRA_H
#include <vcg/math/base.h>
#include <vcg/math/matrix44.h>
#include <algorithm>
#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 <typename MATRIX_TYPE, typename POINT_TYPE>
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;ip<dimension;++ip) //Initialize b and d to the diagonal of a.
{
b[ip]=d[ip]=w[ip][ip];
z[ip]=ScalarType(0.0); //This vector will accumulate terms of the form tapq as in equation (11.1.14).
}
nrot=0;
for (i=0;i<50;i++)
{
sm=ScalarType(0.0);
for (ip=0;ip<dimension-1;++ip) // Sum off diagonal elements
{
for (iq=ip+1;iq<dimension;++iq)
sm += math::Abs(w[ip][iq]);
}
if (sm == ScalarType(0.0)) //The normal return, which relies on quadratic convergence to machine underflow.
{
return;
}
if (i < 4)
tresh=ScalarType(0.2)*sm/(dimension*dimension); //...on the first three sweeps.
else
tresh=ScalarType(0.0); //...thereafter.
for (ip=0;ip<dimension-1;++ip)
{
for (iq=ip+1;iq<dimension;iq++)
{
g=ScalarType(100.0)*fabs(w[ip][iq]);
//After four sweeps, skip the rotation if the off-diagonal element is small.
if(i>4 && (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<MATRIX_TYPE>(w,s,tau,j,ip,j,iq) ;
}
for (j=ip+1;j<=iq-1;j++) { //Case of rotations p < j < q.
JacobiRotate<MATRIX_TYPE>(w,s,tau,ip,j,j,iq);
}
for (j=iq+1;j<dimension;j++) { //Case of rotations q< j <= n.
JacobiRotate<MATRIX_TYPE>(w,s,tau,ip,j,iq,j);
}
for (j=0;j<dimension;j++) {
JacobiRotate<MATRIX_TYPE>(v,s,tau,j,ip,j,iq);
}
++nrot;
}
}
}
for (ip=0;ip<dimension;ip++)
{
b[ip] += z[ip];
d[ip]=b[ip]; //Update d with the sum of ta_pq ,
z[ip]=0.0; //and reinitialize z.
}
}
};
/*!
* Given the eigenvectors and the eigenvalues as output from JacobiRotate, sorts the eigenvalues
* into descending order, and rearranges the columns of v correspondinlgy.
* \param eigenvalues
* \param eigenvector (in columns)
* \param absComparison sort according to the absolute values of the eigenvalues.
*/
template < typename MATRIX_TYPE, typename POINT_TYPE >
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<dimension-1; i++)
{
if (absComparison)
{
p = fabs(eigenvalues[k=i]);
for (j=i+1; j<dimension; j++)
if ((q=fabs(eigenvalues[j])) >= p)
{
p = q;
k = j;
}
p = eigenvalues[k];
}
else
{
p = eigenvalues[ k=i ];
for (j=i+1; j<dimension; j++)
if (eigenvalues[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<dimension; j++)
{
p = eigenvectors[j][i]; // i.e.
eigenvectors[j][i] = eigenvectors[j][k]; // swaps the eigenvectors stored in the
eigenvectors[j][k] = p; // i-th and the k-th column
}
}
}
};
template <typename TYPE>
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 <typename TYPE>
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 <typename TYPE>
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 <I>A<SUB>mxn</SUB></I>, this routine computes its singular value decomposition,
* i.e. <I>A=UxWxV<SUP>T</SUP></I>. The matrix <I>A</I> will be destroyed!
* (This is the implementation described in <I>Numerical Recipies</I>).
* \param A the matrix to be decomposed
* \param W the diagonal matrix of singular values <I>W</I>, stored as a vector <I>W[1...N]</I>
* \param V the matrix <I>V</I> (not the transpose <I>V<SUP>T</SUP></I>)
* \param max_iters max iteration number (default = 30).
* \return
*/
template <typename MATRIX_TYPE>
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<n; i++)
{
l = i+1;
rv1[i] = scale*g;
g = s = scale = 0.0;
if (i < m)
{
for (k = i; k<m; k++)
scale += fabs(A[k][i]);
if (scale)
{
for (k=i; k<m; k++)
{
A[k][i] /= scale;
s += A[k][i]*A[k][i];
}
f=A[i][i];
g = -sign<ScalarType>( sqrt(s), f );
h = f*g - s;
A[i][i]=f-g;
for (j=l; j<n; j++)
{
for (s=0.0, k=i; k<m; k++)
s += A[k][i]*A[k][j];
f = s/h;
for (k=i; k<m; k++)
A[k][j] += f*A[k][i];
}
for (k=i; k<m; k++)
A[k][i] *= scale;
}
}
W[i] = scale *g;
g = s = scale = 0.0;
if (i < m && i != (n-1))
{
for (k=l; k<n; k++)
scale += fabs(A[i][k]);
if (scale)
{
for (k=l; k<n; k++)
{
A[i][k] /= scale;
s += A[i][k]*A[i][k];
}
f = A[i][l];
g = -sign<ScalarType>(sqrt(s),f);
h = f*g - s;
A[i][l] = f-g;
for (k=l; k<n; k++)
rv1[k] = A[i][k]/h;
for (j=l; j<m; j++)
{
for (s=0.0, k=l; k<n; k++)
s += A[j][k]*A[i][k];
for (k=l; k<n; k++)
A[j][k] += s*rv1[k];
}
for (k=l; k<n; k++)
A[i][k] *= scale;
}
}
anorm=std::max( anorm, (math::Abs(W[i])+math::Abs(rv1[i])) );
}
// Accumulation of right-hand transformations.
for (i=(n-1); i>=0; i--)
{
//Accumulation of right-hand transformations.
if (i < (n-1))
{
if (g)
{
for (j=l; j<n;j++) //Double division to avoid possible underflow.
V[j][i]=(A[i][j]/A[i][l])/g;
for (j=l; j<n; j++)
{
for (s=0.0, k=l; k<n; k++)
s += A[i][k] * V[k][j];
for (k=l; k<n; k++)
V[k][j] += s*V[k][i];
}
}
for (j=l; j<n; j++)
V[i][j] = V[j][i] = 0.0;
}
V[i][i] = 1.0;
g = rv1[i];
l = i;
}
// Accumulation of left-hand transformations.
for (i=std::min(m,n)-1; i>=0; i--)
{
l = i+1;
g = W[i];
for (j=l; j<n; j++)
A[i][j]=0.0;
if (g)
{
g = (ScalarType)1.0/g;
for (j=l; j<n; j++)
{
for (s=0.0, k=l; k<m; k++)
s += A[k][i]*A[k][j];
f = (s/A[i][i])*g;
for (k=i; k<m; k++)
A[k][j] += f*A[k][i];
}
for (j=i; j<m; j++)
A[j][i] *= g;
}
else
for (j=i; j<m; j++)
A[j][i] = 0.0;
++A[i][i];
}
// Diagonalization of the bidiagonal form: Loop over
// singular values, and over allowed iterations.
for (k=(n-1); k>=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<ScalarType>(f,g);
W[i] = h;
h = (ScalarType)1.0/h;
c = g*h;
s = -f*h;
for (j=0; j<m; j++)
{
y = A[j][nm];
z = A[j][i];
A[j][nm] = y*c + z*s;
A[j][i] = z*c - y*s;
}
}
}
z = W[k];
if (l == k) //Convergence.
{
if (z < 0.0) { // Singular value is made nonnegative.
W[k] = -z;
for (j=0; j<n; j++)
V[j][k] = -V[j][k];
}
break;
}
if (its == max_iters)
{
convergence = false;
}
x = W[l]; // Shift from bottom 2-by-2 minor.
nm = k-1;
y = W[nm];
g = rv1[nm];
h = rv1[k];
f = ((y-z)*(y+z) + (g-h)*(g+h))/((ScalarType)2.0*h*y);
g = pythagora<ScalarType>(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<ScalarType>(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<n; jj++)
{
x = V[jj][j];
z = V[jj][i];
V[jj][j] = x*c + z*s;
V[jj][i] = z*c - x*s;
}
z = pythagora<ScalarType>(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<m; jj++)
{
y = A[jj][j];
z = A[jj][i];
A[jj][j] = y*c + z*s;
A[jj][i] = z*c - y*s;
}
}
rv1[l] = 0.0;
rv1[k] = f;
W[k] = x;
}
}
delete []rv1;
if (sorting!=LeaveUnsorted)
Sort<MATRIX_TYPE>(A, W, V, sorting);
return convergence;
};
/*!
* Sort the singular values computed by the <CODE>SingularValueDecomposition</CODE> procedure and
* modify the matrices <I>U</I> and <I>V</I> 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<n; i++)
{
int k = i;
ScalarType p = W[i];
switch (sorting)
{
case SortAscending:
{
for (int j=i+1; j<n; j++)
{
if (W[j] < p)
{
k = j;
p = W[j];
}
}
break;
}
case SortDescending:
{
for (int j=i+1; j<n; j++)
{
if (W[j] > 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 <I>U<SUB>mxn</SUB></I>,
* <I>W<SUB>nx1</SUB></I> and <I>V<SUB>nxn</SUB></I> as returned by <CODE>SingularValueDecomposition</CODE>.
* No input quantities are destroyed, so the routine may be called sequentially with different bxs.
* \param x is the output solution vector (<I>x<SUB>nx1</SUB></I>)
* \param b is the input right-hand side (<I>b<SUB>nx1</SUB></I>)
*/
template <typename MATRIX_TYPE>
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<columns_number; j++) //Calculate U^T * B.
{
s = 0;
if (W[j]!=0) //Nonzero result only if wj is nonzero.
{
for (i=0; i<rows_number; i++)
s += U[i][j]*b[i];
s /= W[j]; //This is the divide by wj .
}
tmp[j]=s;
}
for (j=0;j<columns_number;j++) //Matrix multiply by V to get answer.
{
s = 0;
for (jj=0; jj<columns_number; jj++)
s += V[j][jj]*tmp[jj];
x[j]=s;
}
delete []tmp;
};
/*! @} */
}; // end of namespace
#endif

184
vcg/math/old_matrix.h
View File

@ -1,184 +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_matrix.h"
#else
#ifndef MATRIX_VCGLIB
#define MATRIX_VCGLIB
#include "eigen.h"
#include <vcg/space/point.h>
namespace vcg{
namespace ndim{
template<class Scalar> class Matrix;
}
}
namespace Eigen{
template<typename Scalar>
struct ei_traits<vcg::ndim::Matrix<Scalar> > : ei_traits<Eigen::Matrix<Scalar,Dynamic,Dynamic> > {};
template<typename XprType> struct ei_to_vcgtype<XprType,Dynamic,Dynamic,RowMajor,Dynamic,Dynamic>
{ typedef vcg::ndim::Matrix<typename XprType::Scalar> type; };
}
namespace vcg{
namespace ndim{
/** \addtogroup math */
/* @{ */
/*!
* \deprecated use Matrix<Scalar,Rows,Cols> or Matrix<Scalar,Dynamic,Dynamic> or any typedef
* This class represent a generic <I>m</I><EFBFBD><I>n</I> matrix. The class is templated over the scalar type field.
* @param Scalar (Templete Parameter) Specifies the ScalarType field.
*/
template<class _Scalar>
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<Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(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<Scalar> &m) : Base(m) {}
template<typename OtherDerived>
Matrix(const Eigen::MatrixBase<OtherDerived> &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>i</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>i</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 <int N,int M>
void DotProduct(Point<N,Scalar> &m,Point<M,Scalar> &result)
{
unsigned int i, j, p, r;
for (i=0, p=0, r=0; i<M; i++)
{ result[i]=0;
for (j=0; j<N; j++)
result[i]+=(*this)[i][j]*m[j];
}
};*/
/*!
* \deprecated use *this.resize(); *this.setZero();
* 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);
Base::resize(m,n);
memset(Base::data(), 0, m*n*sizeof(Scalar));
};
};
typedef vcg::ndim::Matrix<double> MatrixMNd;
typedef vcg::ndim::Matrix<float> MatrixMNf;
/*! @} */
template <class MatrixType>
void Invert(MatrixType & m)
{
m = m.inverse();
}
}
} // end of namespace
#endif
#endif

298
vcg/math/old_matrix33.h
View File

@ -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 Scalar> class Matrix33;
}
namespace Eigen{
template<typename Scalar>
struct ei_traits<vcg::Matrix33<Scalar> > : ei_traits<Eigen::Matrix<Scalar,3,3,RowMajor> > {};
template<typename XprType> struct ei_to_vcgtype<XprType,3,3,RowMajor,3,3>
{ typedef vcg::Matrix33<typename XprType::Scalar> type; };
}
namespace vcg {
/** \deprecated use Matrix<Scalar,3,3>
@name Matrix33
Class Matrix33.
This is the class for definition of a matrix 3x3.
@param S (Templete Parameter) Specifies the ScalarType field.
*/
template<class _Scalar>
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<Eigen::Matrix<Scalar,3,3,Eigen::RowMajor> >(v)) {}
/// create from Matrix44 excluding row and column k
Matrix33(const Matrix44<Scalar> & m, const int & k) : Base(m.minor(k,k)) {}
template<typename OtherDerived>
Matrix33(const Eigen::MatrixBase<OtherDerived>& 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<Scalar> & axis )
{
*this = Eigen::AngleAxis<Scalar>(angle,axis).toRotationMatrix();
return (*this);
}
/** \deprecated */
Matrix33 & SetRotateDeg(Scalar angle, const Point3<Scalar> & 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<Scalar> &a, const Point3<Scalar> &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 <class STLPOINTCONTAINER >
void CrossCovariance(const STLPOINTCONTAINER &P, const STLPOINTCONTAINER &X,
Point3<Scalar> &bp, Point3<Scalar> &bx)
{
setZero();
assert(P.size()==X.size());
bx.setZero();
bp.setZero();
Matrix33<Scalar> tmp;
typename std::vector <Point3<Scalar> >::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 <class STLPOINTCONTAINER, class STLREALCONTAINER>
void WeightedCrossCovariance(const STLREALCONTAINER & weights,
const STLPOINTCONTAINER &P,
const STLPOINTCONTAINER &X,
Point3<Scalar> &bp,
Point3<Scalar> &bx)
{
setZero();
assert(P.size()==X.size());
bx.SetZero();
bp.SetZero();
Matrix33<Scalar> tmp;
typename std::vector <Point3<Scalar> >::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 <class S>
void Invert(Matrix33<S> &m) { m = m.lu().inverse(); }
template <class S>
Matrix33<S> Inverse(const Matrix33<S>&m) { return m.lu().inverse(); }
///given 2 vector centered into origin calculate the rotation matrix from first to the second
template <class S>
Matrix33<S> RotationMatrix(vcg::Point3<S> v0,vcg::Point3<S> v1,bool normalized=true)
{
typedef typename vcg::Point3<S> CoordType;
Matrix33<S> 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 <class S>
Matrix33<S> RotationMatrix(const vcg::Point3<S> &axis,
const float &angleRad)
{
vcg::Matrix44<S> matr44;
vcg::Matrix33<S> 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 <class S>
Matrix33<S> RandomRotation(){
S x1,x2,x3;
Matrix33<S> R,H,M,vv;
Point3<S> 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<short> Matrix33s;
typedef Matrix33<int> Matrix33i;
typedef Matrix33<float> Matrix33f;
typedef Matrix33<double> Matrix33d;
} // end of namespace
#endif
#endif

493
vcg/math/old_matrix44.h
View File

@ -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 <vcg/space/point3.h>
#include <vcg/space/point4.h>
#include <memory.h>
#include <vector>
namespace vcg{
template<class Scalar> class Matrix44;
}
namespace Eigen{
template<typename Scalar>
struct ei_traits<vcg::Matrix44<Scalar> > : ei_traits<Eigen::Matrix<Scalar,4,4,RowMajor> > {};
template<typename XprType> struct ei_to_vcgtype<XprType,4,4,RowMajor,4,4>
{ typedef vcg::Matrix44<typename XprType::Scalar> 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<Other>::RowsAtCompileTime,
int OtherCols=Eigen::ei_traits<Other>::ColsAtCompileTime>
struct ei_matrix44_product_impl;
/** \deprecated use Eigen::Matrix<Scalar,4,4> (or the typedef) you want a real 4x4 matrix, or use Eigen::Transform<Scalar,3> if you want a transformation matrix for a 3D space (a Eigen::Transform<Scalar,3> is internally a 4x4 col-major matrix)
*
* This class represents a 4x4 matrix. T is the kind of element in the matrix.
*/
template<typename _Scalar>
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<Eigen::Matrix<Scalar,4,4,Eigen::RowMajor> >(v)) {}
template<typename OtherDerived>
Matrix44(const Eigen::MatrixBase<OtherDerived>& 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<Base,3,1> 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<Base,1,3> GetRow3(const int& i) const { return this->template block<1,3>(i,0); }
template <class Matrix44Type>
void ToMatrix(Matrix44Type & m) const { m = (*this).template cast<typename Matrix44Type::Scalar>(); }
void ToEulerAngles(Scalar &alpha, Scalar &beta, Scalar &gamma);
template <class Matrix44Type>
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<Scalar> &t);
Matrix44 &SetTranslate(const Point3<Scalar> &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<Scalar> & axis);
Matrix44 &SetRotateRad(Scalar AngleRad, const Point3<Scalar> & 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<typename OtherDerived>
inline const typename ei_matrix44_product_impl<OtherDerived>::ResultType
operator * (const Eigen::MatrixBase<OtherDerived> &other) const
{ return ei_matrix44_product_impl<OtherDerived>::run(*this,other.derived()); }
void print() {std::cout << *this << "\n\n";}
};
//return NULL matrix if not invertible
template <class T> Matrix44<T> &Invert(Matrix44<T> &m);
template <class T> Matrix44<T> Inverse(const Matrix44<T> &m);
typedef Matrix44<short> Matrix44s;
typedef Matrix44<int> Matrix44i;
typedef Matrix44<float> Matrix44f;
typedef Matrix44<double> Matrix44d;
template < class PointType , class T > void operator*=( std::vector<PointType> &vert, const Matrix44<T> & m ) {
typename std::vector<PointType>::iterator ii;
for(ii=vert.begin();ii!=vert.end();++ii)
(*ii).P()=m * (*ii).P();
}
template <class T>
void Matrix44<T>::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 <class T>
void Matrix44<T>::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 <class T> void Matrix44<T>::SetDiagonal(const Scalar k) {
setZero();
ElementAt(0, 0) = k;
ElementAt(1, 1) = k;
ElementAt(2, 2) = k;
ElementAt(3, 3) = 1;
}
template <class T> Matrix44<T> &Matrix44<T>::SetScale(const Point3<Scalar> &t) {
SetScale(t[0], t[1], t[2]);
return *this;
}
template <class T> Matrix44<T> &Matrix44<T>::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 <class T> Matrix44<T> &Matrix44<T>::SetTranslate(const Point3<Scalar> &t) {
SetTranslate(t[0], t[1], t[2]);
return *this;
}
template <class T> Matrix44<T> &Matrix44<T>::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 <class T> Matrix44<T> &Matrix44<T>::SetRotateDeg(Scalar AngleDeg, const Point3<Scalar> & axis) {
return SetRotateRad(math::ToRad(AngleDeg),axis);
}
template <class T> Matrix44<T> &Matrix44<T>::SetRotateRad(Scalar AngleRad, const Point3<Scalar> & 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> 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 <class T> Matrix44<T> & Matrix44<T>::SetShearXY( const Scalar sh) {// shear the X coordinate as the Y coordinate change
Base::setIdentity();
ElementAt(0,1) = sh;
return *this;
}
template <class T> Matrix44<T> & Matrix44<T>::SetShearXZ( const Scalar sh) {// shear the X coordinate as the Z coordinate change
Base::setIdentity();
ElementAt(0,2) = sh;
return *this;
}
template <class T> Matrix44<T> &Matrix44<T>::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 <class T>
bool Decompose(Matrix44<T> &M, Point3<T> &ScaleV, Point3<T> &ShearV, Point3<T> &RotV,Point3<T> &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<T> 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 <class T> Matrix44<T> & Invert(Matrix44<T> &m) {
return m = m.lu().inverse();
}
template <class T> Matrix44<T> Inverse(const Matrix44<T> &m) {
return m.lu().inverse();
}
template<typename Other,int OtherCols>
struct ei_matrix44_product_impl<Other, 4,OtherCols>
{
typedef typename Other::Scalar Scalar;
typedef typename Eigen::ProductReturnType<typename Matrix44<Scalar>::Base,Other>::Type ResultType;
static ResultType run(const Matrix44<Scalar>& tr, const Other& other)
{ return (static_cast<const typename Matrix44<Scalar>::Base&>(tr)) * other; }
};
template<typename Other>
struct ei_matrix44_product_impl<Other, 3,1>
{
typedef typename Other::Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1> ResultType;
static ResultType run(const Matrix44<Scalar>& tr, const Other& p)
{
Scalar w;
Eigen::Matrix<Scalar,3,1> 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