big replacement .Zero => .SetZero, and start of Eigen's compatibilities (currently disabled by default)
This commit is contained in:
parent
c0a5159672
commit
4db69febbe
|
|
@ -6,7 +6,7 @@ class ImplicitSphere
|
|||
public:
|
||||
ImplicitSphere()
|
||||
{
|
||||
_center.Zero();
|
||||
_center.SetZero();
|
||||
_radius = _sqr_radius = 0.0;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -144,7 +144,7 @@ ScalarType _VolumePreservingError(PosType &pos,CoordType &new_point,int nsteps)
|
|||
else
|
||||
if ((!ext_v0)&&(!ext_v1))
|
||||
{/*CoordType g;
|
||||
g.Zero();
|
||||
g.SetZero();
|
||||
g+=ve0->cP();
|
||||
g+=ve1->cP();
|
||||
g/=2;*/
|
||||
|
|
@ -160,7 +160,7 @@ ScalarType _VolumePreservingError(PosType &pos,CoordType &new_point,int nsteps)
|
|||
best_error=1000000.f;
|
||||
ScalarType alfatemp=step*((ScalarType)i);
|
||||
//CoordType g;
|
||||
// g.Zero();
|
||||
// g.SetZero();
|
||||
//g+=ve0->cP()*alfatemp;
|
||||
//g+=ve1->cP()*(1-alfatemp);
|
||||
//CoordType newPTemp=g;
|
||||
|
|
|
|||
|
|
@ -526,7 +526,7 @@ static void InitQuadric(TriMeshType &m)
|
|||
// m.ClearFlags();
|
||||
for(pv=m.vert.begin();pv!=m.vert.end();++pv) // Azzero le quadriche
|
||||
if( ! (*pv).IsD() && (*pv).IsW())
|
||||
QH::Qd(*pv).Zero();
|
||||
QH::Qd(*pv).SetZero();
|
||||
|
||||
|
||||
for(pf=m.face.begin();pf!=m.face.end();++pf)
|
||||
|
|
|
|||
|
|
@ -379,7 +379,7 @@ namespace vcg
|
|||
VertexPointer mean_point = &*AllocatorType::AddVertices( *_mesh, 1);
|
||||
mean_point->SetUserBit(_featureFlag);
|
||||
mean_point->P() = point;
|
||||
mean_point->N().Zero();
|
||||
mean_point->N().SetZero();
|
||||
delete []x;
|
||||
delete []points;
|
||||
delete []normals;
|
||||
|
|
|
|||
|
|
@ -313,7 +313,7 @@ static void Covariance(const MeshType & m, vcg::Point3<ScalarType> & bary, vcg::
|
|||
|
||||
ConstFaceIterator fi;
|
||||
ScalarType area = 0.0;
|
||||
bary.Zero();
|
||||
bary.SetZero();
|
||||
for(fi = m.face.begin(); fi != m.face.end(); ++fi)
|
||||
if(!(*fi).IsD())
|
||||
{
|
||||
|
|
|
|||
|
|
@ -240,7 +240,7 @@ public:
|
|||
#define v1 (f->V1(i)->T().P())
|
||||
#define v2 (f->V2(i)->T().P())
|
||||
for (VertexIterator v=Super::m.vert.begin(); v!=Super::m.vert.end(); v++) {
|
||||
sum[v].Zero();
|
||||
sum[v].SetZero();
|
||||
}
|
||||
|
||||
ScalarType tot_proj_area=0;
|
||||
|
|
@ -424,7 +424,7 @@ void SmoothTextureCoords(MESH_TYPE &m){
|
|||
sum.Start();
|
||||
|
||||
for (typename MESH_TYPE::VertexIterator v=m.vert.begin(); v!=m.vert.end(); v++) {
|
||||
sum[v].Zero();
|
||||
sum[v].SetZero();
|
||||
div[v]=0;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -278,10 +278,7 @@ public:
|
|||
S[0][1] = s;
|
||||
S[1][0] = -1.0f * s;
|
||||
|
||||
vcg::ndim::MatrixMNf St (S);
|
||||
St.Transpose();
|
||||
|
||||
vcg::ndim::MatrixMNf StMS(St * minor2x2 * S);
|
||||
vcg::ndim::MatrixMNf StMS(S.transpose() * minor2x2 * S);
|
||||
|
||||
// compute curvatures and curvature directions
|
||||
float Principal_Curvature1 = (3.0f * StMS[0][0]) - StMS[1][1];
|
||||
|
|
|
|||
|
|
@ -0,0 +1,785 @@
|
|||
/****************************************************************************
|
||||
* VCGLib o o *
|
||||
* Visual and Computer Graphics Library o o *
|
||||
* _ O _ *
|
||||
* Copyright(C) 2004 \/)\/ *
|
||||
* Visual Computing Lab /\/| *
|
||||
* ISTI - Italian National Research Council | *
|
||||
* \ *
|
||||
* All rights reserved. *
|
||||
* *
|
||||
* This program is free software; you can redistribute it and/or modify *
|
||||
* it under the terms of the GNU General Public License as published by *
|
||||
* the Free Software Foundation; either version 2 of the License, or *
|
||||
* (at your option) any later version. *
|
||||
* *
|
||||
* This program is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
||||
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
||||
* for more details. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
/***************************************************************************
|
||||
$Log: not supported by cvs2svn $
|
||||
Revision 1.9 2006/09/11 16:11:39 marfr960
|
||||
Added const to declarations of the overloaded (operators *).
|
||||
Otherwise the * operator would always attempt to convert any type of data passed as an argument to Point3<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>
|
||||
#include <vcg/math/lin_algebra.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 const 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(-1, 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>=0 && 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>=0 && 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;
|
||||
};
|
||||
|
||||
Matrix transpose()
|
||||
{
|
||||
Matrix res = *this;
|
||||
res.Transpose();
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
/*!
|
||||
* 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
|
||||
|
|
@ -0,0 +1,744 @@
|
|||
/****************************************************************************
|
||||
* VCGLib o o *
|
||||
* Visual and Computer Graphics Library o o *
|
||||
* _ O _ *
|
||||
* Copyright(C) 2004 \/)\/ *
|
||||
* Visual Computing Lab /\/| *
|
||||
* ISTI - Italian National Research Council | *
|
||||
* \ *
|
||||
* All rights reserved. *
|
||||
* *
|
||||
* This program is free software; you can redistribute it and/or modify *
|
||||
* it under the terms of the GNU General Public License as published by *
|
||||
* the Free Software Foundation; either version 2 of the License, or *
|
||||
* (at your option) any later version. *
|
||||
* *
|
||||
* This program is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
||||
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
||||
* for more details. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
History
|
||||
|
||||
$Log: not supported by cvs2svn $
|
||||
Revision 1.18 2007/04/19 14:30:26 pietroni
|
||||
added RotationMatrix method to calculate rotation matrix along an axis
|
||||
|
||||
Revision 1.17 2007/04/07 23:06:47 pietroni
|
||||
Added function RotationMatrix
|
||||
|
||||
Revision 1.16 2007/01/29 00:20:25 pietroni
|
||||
-Used scalar type passed as template argument istead of double to prevent warnings.. in Rotate function
|
||||
|
||||
Revision 1.15 2006/09/25 23:05:29 ganovelli
|
||||
added constructor from matrix44 excluding a row and colum
|
||||
|
||||
Revision 1.14 2006/06/22 08:00:05 ganovelli
|
||||
bug in operator + with MatrixxDig
|
||||
|
||||
Revision 1.13 2006/01/20 16:41:44 pietroni
|
||||
added operators:
|
||||
operator -= ( const Matrix33Diag<S> &p )
|
||||
Matrix33 operator - ( const Matrix33Diag<S> &p )
|
||||
Matrix33 operator + ( const Matrix33 &m )
|
||||
Matrix33 operator + ( const Matrix33Diag<S> &p )
|
||||
|
||||
Revision 1.12 2005/11/14 10:28:25 cignoni
|
||||
Changed Invert -> FastInvert for the function based on the maple expansion
|
||||
|
||||
Revision 1.11 2005/10/13 15:45:23 ponchio
|
||||
Changed a Zero in SetZero in WeightedCrossCovariance() (again)
|
||||
|
||||
Revision 1.10 2005/10/05 17:06:12 pietroni
|
||||
corrected sintax error on singular value decomposition
|
||||
|
||||
Revision 1.9 2005/09/29 09:53:58 ganovelli
|
||||
added inverse by SVD
|
||||
|
||||
Revision 1.8 2005/06/10 14:51:54 cignoni
|
||||
Changed a Zero in SetZero in WeightedCrossCovariance()
|
||||
|
||||
Revision 1.7 2005/06/10 11:46:49 pietroni
|
||||
Added Norm Function
|
||||
|
||||
Revision 1.6 2005/06/07 14:29:56 ganovelli
|
||||
changed from Matrix33Ide to MatrixeeDiag
|
||||
|
||||
Revision 1.5 2005/05/23 15:05:26 ganovelli
|
||||
Matrix33Diag Added: it implements diagonal matrix. Added only operator += in Matrix33
|
||||
|
||||
Revision 1.4 2005/04/11 14:11:22 pietroni
|
||||
changed swap to math::Swap in Traspose Function
|
||||
|
||||
Revision 1.3 2004/10/18 15:03:02 fiorin
|
||||
Updated interface: all Matrix classes have now the same interface
|
||||
|
||||
Revision 1.2 2004/07/13 06:48:26 cignoni
|
||||
removed uppercase references in include
|
||||
|
||||
Revision 1.1 2004/05/28 13:09:05 ganovelli
|
||||
created
|
||||
|
||||
Revision 1.1 2004/05/28 13:00:39 ganovelli
|
||||
created
|
||||
|
||||
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#ifndef __VCGLIB_MATRIX33_H
|
||||
#define __VCGLIB_MATRIX33_H
|
||||
|
||||
#include <stdio.h>
|
||||
#include <vcg/math/lin_algebra.h>
|
||||
#include <vcg/math/matrix44.h>
|
||||
#include <vcg/space/point3.h>
|
||||
#include <vector>
|
||||
|
||||
namespace vcg {
|
||||
|
||||
template <class S>
|
||||
class Matrix33Diag:public Point3<S>{
|
||||
public:
|
||||
|
||||
/** @name Matrix33
|
||||
Class Matrix33Diag.
|
||||
This is the class for definition of a diagonal matrix 3x3.
|
||||
@param S (Templete Parameter) Specifies the ScalarType field.
|
||||
*/
|
||||
Matrix33Diag(const S & p0,const S & p1,const S & p2):Point3<S>(p0,p1,p2){};
|
||||
Matrix33Diag(const Point3<S>&p ):Point3<S>(p){};
|
||||
};
|
||||
|
||||
template<class S>
|
||||
/** @name Matrix33
|
||||
Class Matrix33.
|
||||
This is the class for definition of a matrix 3x3.
|
||||
@param S (Templete Parameter) Specifies the ScalarType field.
|
||||
*/
|
||||
class Matrix33
|
||||
{
|
||||
public:
|
||||
typedef S ScalarType;
|
||||
|
||||
/// Default constructor
|
||||
inline Matrix33() {}
|
||||
|
||||
/// Copy constructor
|
||||
Matrix33( const Matrix33 & m )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] = m.a[i];
|
||||
}
|
||||
|
||||
/// create from array
|
||||
Matrix33( const S * v )
|
||||
{
|
||||
for(int i=0;i<9;++i) a[i] = v[i];
|
||||
}
|
||||
|
||||
/// create from Matrix44 excluding row and column k
|
||||
Matrix33 (const Matrix44<S> & m, const int & k)
|
||||
{
|
||||
int i,j, r=0, c=0;
|
||||
for(i = 0; i< 4;++i)if(i!=k){c=0;
|
||||
for(j=0; j < 4;++j)if(j!=k)
|
||||
{ (*this)[r][c] = m[i][j]; ++c;}
|
||||
++r;
|
||||
}
|
||||
}
|
||||
|
||||
/// Number of columns
|
||||
inline unsigned int ColumnsNumber() const
|
||||
{
|
||||
return 3;
|
||||
};
|
||||
|
||||
/// Number of rows
|
||||
inline unsigned int RowsNumber() const
|
||||
{
|
||||
return 3;
|
||||
};
|
||||
|
||||
/// Assignment operator
|
||||
Matrix33 & operator = ( const Matrix33 & m )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] = m.a[i];
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// Operatore di indicizzazione
|
||||
inline S * operator [] ( const int i )
|
||||
{
|
||||
return a+i*3;
|
||||
}
|
||||
/// Operatore const di indicizzazione
|
||||
inline const S * operator [] ( const int i ) const
|
||||
{
|
||||
return a+i*3;
|
||||
}
|
||||
|
||||
|
||||
/// Modificatore somma per matrici 3x3
|
||||
Matrix33 & operator += ( const Matrix33 &m )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] += m.a[i];
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Modificatore somma per matrici 3x3
|
||||
Matrix33 & operator += ( const Matrix33Diag<S> &p )
|
||||
{
|
||||
a[0] += p[0];
|
||||
a[4] += p[1];
|
||||
a[8] += p[2];
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Modificatore sottrazione per matrici 3x3
|
||||
Matrix33 & operator -= ( const Matrix33 &m )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] -= m.a[i];
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Modificatore somma per matrici 3x3
|
||||
Matrix33 & operator -= ( const Matrix33Diag<S> &p )
|
||||
{
|
||||
a[0] -= p[0];
|
||||
a[4] -= p[1];
|
||||
a[8] -= p[2];
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Modificatore divisione per scalare
|
||||
Matrix33 & operator /= ( const S &s )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] /= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/// Modificatore prodotto per matrice
|
||||
Matrix33 operator * ( const Matrix33< S> & t ) const
|
||||
{
|
||||
Matrix33<S> r;
|
||||
|
||||
int i,j;
|
||||
for(i=0;i<3;++i)
|
||||
for(j=0;j<3;++j)
|
||||
r[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j];
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Modificatore prodotto per matrice
|
||||
void operator *=( const Matrix33< S> & t )
|
||||
{
|
||||
int i,j;
|
||||
for(i=0;i<3;++i)
|
||||
for(j=0;j<3;++j)
|
||||
(*this)[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j];
|
||||
|
||||
}
|
||||
|
||||
/// Dot product with a diagonal matrix
|
||||
Matrix33 operator * ( const Matrix33Diag< S> & t ) const
|
||||
{
|
||||
Matrix33<S> r;
|
||||
|
||||
int i,j;
|
||||
for(i=0;i<3;++i)
|
||||
for(j=0;j<3;++j)
|
||||
r[i][j] = (*this)[i][j]*t[j];
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Dot product modifier with a diagonal matrix
|
||||
void operator *=( const Matrix33Diag< S> & t )
|
||||
{
|
||||
int i,j;
|
||||
for(i=0;i<3;++i)
|
||||
for(j=0;j<3;++j)
|
||||
(*this)[i][j] = (*this)[i][j]*t[j];
|
||||
}
|
||||
|
||||
/// Modificatore prodotto per costante
|
||||
Matrix33 & operator *= ( const S t )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] *= t;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Operatore prodotto per costante
|
||||
Matrix33 operator * ( const S t ) const
|
||||
{
|
||||
Matrix33<S> r;
|
||||
for(int i=0;i<9;++i)
|
||||
r.a[i] = a[i]* t;
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Operatore sottrazione per matrici 3x3
|
||||
Matrix33 operator - ( const Matrix33 &m ) const
|
||||
{
|
||||
Matrix33<S> r;
|
||||
for(int i=0;i<9;++i)
|
||||
r.a[i] = a[i] - m.a[i];
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Operatore sottrazione di matrici 3x3 con matrici diagonali
|
||||
Matrix33 operator - ( const Matrix33Diag<S> &p ) const
|
||||
{
|
||||
Matrix33<S> r=a;
|
||||
r[0][0] -= p[0];
|
||||
r[1][1] -= p[1];
|
||||
r[2][2] -= p[2];
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Operatore sottrazione per matrici 3x3
|
||||
Matrix33 operator + ( const Matrix33 &m ) const
|
||||
{
|
||||
Matrix33<S> r;
|
||||
for(int i=0;i<9;++i)
|
||||
r.a[i] = a[i] + m.a[i];
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Operatore addizione di matrici 3x3 con matrici diagonali
|
||||
Matrix33 operator + ( const Matrix33Diag<S> &p ) const
|
||||
{
|
||||
Matrix33<S> r=(*this);
|
||||
r[0][0] += p[0];
|
||||
r[1][1] += p[1];
|
||||
r[2][2] += p[2];
|
||||
return r;
|
||||
}
|
||||
|
||||
/** Operatore per il prodotto matrice-vettore.
|
||||
@param v A point in $R^{3}$
|
||||
@return Il vettore risultante in $R^{3}$
|
||||
*/
|
||||
Point3<S> operator * ( const Point3<S> & v ) const
|
||||
{
|
||||
Point3<S> t;
|
||||
|
||||
t[0] = a[0]*v[0] + a[1]*v[1] + a[2]*v[2];
|
||||
t[1] = a[3]*v[0] + a[4]*v[1] + a[5]*v[2];
|
||||
t[2] = a[6]*v[0] + a[7]*v[1] + a[8]*v[2];
|
||||
return t;
|
||||
}
|
||||
|
||||
void OuterProduct(Point3<S> const &p0, Point3<S> const &p1) {
|
||||
Point3<S> row;
|
||||
row = p1*p0[0];
|
||||
a[0] = row[0];a[1] = row[1];a[2] = row[2];
|
||||
row = p1*p0[1];
|
||||
a[3] = row[0]; a[4] = row[1]; a[5] = row[2];
|
||||
row = p1*p0[2];
|
||||
a[6] = row[0];a[7] = row[1];a[8] = row[2];
|
||||
}
|
||||
|
||||
Matrix33 & SetZero() {
|
||||
for(int i=0;i<9;++i) a[i] =0;
|
||||
return (*this);
|
||||
}
|
||||
Matrix33 & SetIdentity() {
|
||||
for(int i=0;i<9;++i) a[i] =0;
|
||||
a[0]=a[4]=a[8]=1.0;
|
||||
return (*this);
|
||||
}
|
||||
|
||||
Matrix33 & SetRotateRad(S angle, const Point3<S> & axis )
|
||||
{
|
||||
S c = cos(angle);
|
||||
S s = sin(angle);
|
||||
S q = 1-c;
|
||||
Point3<S> t = axis;
|
||||
t.Normalize();
|
||||
a[0] = t[0]*t[0]*q + c;
|
||||
a[1] = t[0]*t[1]*q - t[2]*s;
|
||||
a[2] = t[0]*t[2]*q + t[1]*s;
|
||||
a[3] = t[1]*t[0]*q + t[2]*s;
|
||||
a[4] = t[1]*t[1]*q + c;
|
||||
a[5] = t[1]*t[2]*q - t[0]*s;
|
||||
a[6] = t[2]*t[0]*q -t[1]*s;
|
||||
a[7] = t[2]*t[1]*q +t[0]*s;
|
||||
a[8] = t[2]*t[2]*q +c;
|
||||
return (*this);
|
||||
}
|
||||
Matrix33 & SetRotateDeg(S angle, const Point3<S> & axis ){
|
||||
return SetRotateRad(math::ToRad(angle),axis);
|
||||
}
|
||||
|
||||
/// Funzione per eseguire la trasposta della matrice
|
||||
Matrix33 & Transpose()
|
||||
{
|
||||
math::Swap(a[1],a[3]);
|
||||
math::Swap(a[2],a[6]);
|
||||
math::Swap(a[5],a[7]);
|
||||
return *this;
|
||||
}
|
||||
|
||||
// for the transistion to eigen
|
||||
Matrix33 transpose()
|
||||
{
|
||||
Matrix33 res = *this;
|
||||
res.Transpose();
|
||||
return res;
|
||||
}
|
||||
|
||||
/// Funzione per costruire una matrice diagonale dati i tre elem.
|
||||
Matrix33 & SetDiagonal(S *v)
|
||||
{int i,j;
|
||||
for(i=0;i<3;i++)
|
||||
for(j=0;j<3;j++)
|
||||
if(i==j) (*this)[i][j] = v[i];
|
||||
else (*this)[i][j] = 0;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/// Assegna l'n-simo vettore colonna
|
||||
void SetColumn(const int n, S* v){
|
||||
assert( (n>=0) && (n<3) );
|
||||
a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2];
|
||||
};
|
||||
|
||||
/// Assegna l'n-simo vettore riga
|
||||
void SetRow(const int n, S* v){
|
||||
assert( (n>=0) && (n<3) );
|
||||
int m=n*3;
|
||||
a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2];
|
||||
};
|
||||
|
||||
/// Assegna l'n-simo vettore colonna
|
||||
void SetColumn(const int n, const Point3<S> v){
|
||||
assert( (n>=0) && (n<3) );
|
||||
a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2];
|
||||
};
|
||||
|
||||
/// Assegna l'n-simo vettore riga
|
||||
void SetRow(const int n, const Point3<S> v){
|
||||
assert( (n>=0) && (n<3) );
|
||||
int m=n*3;
|
||||
a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2];
|
||||
};
|
||||
|
||||
/// Restituisce l'n-simo vettore colonna
|
||||
Point3<S> GetColumn(const int n) const {
|
||||
assert( (n>=0) && (n<3) );
|
||||
Point3<S> t;
|
||||
t[0]=a[n]; t[1]=a[n+3]; t[2]=a[n+6];
|
||||
return t;
|
||||
};
|
||||
|
||||
/// Restituisce l'n-simo vettore riga
|
||||
Point3<S> GetRow(const int n) const {
|
||||
assert( (n>=0) && (n<3) );
|
||||
Point3<S> t;
|
||||
int m=n*3;
|
||||
t[0]=a[m]; t[1]=a[m+1]; t[2]=a[m+2];
|
||||
return t;
|
||||
};
|
||||
|
||||
|
||||
|
||||
/// Funzione per il calcolo del determinante
|
||||
S Determinant() const
|
||||
{
|
||||
return a[0]*(a[4]*a[8]-a[5]*a[7]) -
|
||||
a[1]*(a[3]*a[8]-a[5]*a[6]) +
|
||||
a[2]*(a[3]*a[7]-a[4]*a[6]) ;
|
||||
}
|
||||
|
||||
// Warning, this Inversion code can be HIGHLY NUMERICALLY UNSTABLE!
|
||||
// In most case you are advised to use the Invert() method based on SVD decomposition.
|
||||
|
||||
Matrix33 & FastInvert()
|
||||
{
|
||||
// Maple produsse:
|
||||
S t4 = a[0]*a[4];
|
||||
S t6 = a[0]*a[5];
|
||||
S t8 = a[1]*a[3];
|
||||
S t10 = a[2]*a[3];
|
||||
S t12 = a[1]*a[6];
|
||||
S t14 = a[2]*a[6];
|
||||
S t17 = 1/(t4*a[8]-t6*a[7]-t8*a[8]+t10*a[7]+t12*a[5]-t14*a[4]);
|
||||
S a0 = a[0];
|
||||
S a1 = a[1];
|
||||
S a3 = a[3];
|
||||
S a4 = a[4];
|
||||
a[0] = (a[4]*a[8]-a[5]*a[7])*t17;
|
||||
a[1] = -(a[1]*a[8]-a[2]*a[7])*t17;
|
||||
a[2] = (a1 *a[5]-a[2]*a[4])*t17;
|
||||
a[3] = -(a[3]*a[8]-a[5]*a[6])*t17;
|
||||
a[4] = (a0 *a[8]-t14 )*t17;
|
||||
a[5] = -(t6 - t10)*t17;
|
||||
a[6] = (a3 *a[7]-a[4]*a[6])*t17;
|
||||
a[7] = -(a[0]*a[7]-t12)*t17;
|
||||
a[8] = (t4-t8)*t17;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
void show(FILE * fp)
|
||||
{
|
||||
for(int i=0;i<3;++i)
|
||||
printf("| %g \t%g \t%g |\n",a[3*i+0],a[3*i+1],a[3*i+2]);
|
||||
}
|
||||
|
||||
// return the Trace of the matrix i.e. the sum of the diagonal elements
|
||||
S Trace() const
|
||||
{
|
||||
return a[0]+a[4]+a[8];
|
||||
}
|
||||
|
||||
/*
|
||||
compute the matrix generated by the product of a * b^T
|
||||
*/
|
||||
void ExternalProduct(const Point3<S> &a, const Point3<S> &b)
|
||||
{
|
||||
for(int i=0;i<3;++i)
|
||||
for(int j=0;j<3;++j)
|
||||
(*this)[i][j] = a[i]*b[j];
|
||||
}
|
||||
|
||||
/* Compute the Frobenius Norm of the Matrix
|
||||
*/
|
||||
ScalarType Norm()
|
||||
{
|
||||
ScalarType SQsum=0;
|
||||
for(int i=0;i<3;++i)
|
||||
for(int j=0;j<3;++j)
|
||||
SQsum += a[i]*a[i];
|
||||
return (math::Sqrt(SQsum));
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
It compute the covariance matrix of a set of 3d points. Returns the barycenter
|
||||
*/
|
||||
template <class STLPOINTCONTAINER >
|
||||
void Covariance(const STLPOINTCONTAINER &points, Point3<S> &bp) {
|
||||
assert(!points.empty());
|
||||
typedef typename STLPOINTCONTAINER::const_iterator PointIte;
|
||||
// first cycle: compute the barycenter
|
||||
bp.SetZero();
|
||||
for( PointIte pi = points.begin(); pi != points.end(); ++pi) bp+= (*pi);
|
||||
bp/=points.size();
|
||||
// second cycle: compute the covariance matrix
|
||||
this->SetZero();
|
||||
vcg::Matrix33<ScalarType> A;
|
||||
for( PointIte pi = points.begin(); pi != points.end(); ++pi) {
|
||||
Point3<S> p = (*pi)-bp;
|
||||
A.OuterProduct(p,p);
|
||||
(*this)+= A;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
It compute the cross covariance matrix of two set of 3d points P and X;
|
||||
it returns also the barycenters of P and X.
|
||||
fonte:
|
||||
|
||||
Besl, McKay
|
||||
A method for registration o f 3d Shapes
|
||||
IEEE TPAMI Vol 14, No 2 1992
|
||||
|
||||
*/
|
||||
template <class STLPOINTCONTAINER >
|
||||
void CrossCovariance(const STLPOINTCONTAINER &P, const STLPOINTCONTAINER &X,
|
||||
Point3<S> &bp, Point3<S> &bx)
|
||||
{
|
||||
SetZero();
|
||||
assert(P.size()==X.size());
|
||||
bx.SetZero();
|
||||
bp.SetZero();
|
||||
Matrix33<S> tmp;
|
||||
typename std::vector <Point3<S> >::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<S> &bp,
|
||||
Point3<S> &bx)
|
||||
{
|
||||
SetZero();
|
||||
assert(P.size()==X.size());
|
||||
bx.SetZero();
|
||||
bp.SetZero();
|
||||
Matrix33<S> tmp;
|
||||
typename std::vector <Point3<S> >::const_iterator pi,xi;
|
||||
typename STLREALCONTAINER::const_iterator pw;
|
||||
|
||||
for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
|
||||
bp+=(*pi);
|
||||
bx+=(*xi);
|
||||
}
|
||||
bp/=P.size();
|
||||
bx/=X.size();
|
||||
|
||||
for(pi=P.begin(),xi=X.begin(),pw = weights.begin();pi!=P.end();++pi,++xi,++pw){
|
||||
|
||||
tmp.ExternalProduct(((*pi)-(bp)),((*xi)-(bp)));
|
||||
|
||||
(*this)+=tmp*(*pw);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
S a[9];
|
||||
};
|
||||
|
||||
template <class S>
|
||||
void Invert(Matrix33<S> &m)
|
||||
{
|
||||
Matrix33<S> v;
|
||||
Point3<typename Matrix33<S>::ScalarType> e;
|
||||
SingularValueDecomposition(m,&e[0],v);
|
||||
e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2];
|
||||
m.Transpose();
|
||||
m = v * Matrix33Diag<S>(e) * m;
|
||||
}
|
||||
|
||||
template <class S>
|
||||
Matrix33<S> Inverse(const Matrix33<S>&m)
|
||||
{
|
||||
Matrix33<S> v,m_copy=m;
|
||||
Point3<S> e;
|
||||
SingularValueDecomposition(m_copy,&e[0],v);
|
||||
m_copy.Transpose();
|
||||
e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2];
|
||||
return v * Matrix33Diag<S>(e) * m_copy;
|
||||
}
|
||||
|
||||
///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*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
|
||||
|
|
@ -0,0 +1,984 @@
|
|||
/****************************************************************************
|
||||
* VCGLib o o *
|
||||
* Visual and Computer Graphics Library o o *
|
||||
* _ O _ *
|
||||
* Copyright(C) 2004 \/)\/ *
|
||||
* Visual Computing Lab /\/| *
|
||||
* ISTI - Italian National Research Council | *
|
||||
* \ *
|
||||
* All rights reserved. *
|
||||
* *
|
||||
* This program is free software; you can redistribute it and/or modify *
|
||||
* it under the terms of the GNU General Public License as published by *
|
||||
* the Free Software Foundation; either version 2 of the License, or *
|
||||
* (at your option) any later version. *
|
||||
* *
|
||||
* This program is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
||||
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
||||
* for more details. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
History
|
||||
|
||||
$Log: not supported by cvs2svn $
|
||||
Revision 1.34 2007/07/12 06:42:01 cignoni
|
||||
added the missing static Construct() member
|
||||
|
||||
Revision 1.33 2007/07/03 16:06:48 corsini
|
||||
add DCM to Euler Angles conversion
|
||||
|
||||
Revision 1.32 2007/03/08 14:39:27 corsini
|
||||
final fix to euler angles transformation
|
||||
|
||||
Revision 1.31 2007/02/06 09:57:40 corsini
|
||||
fix euler angles computation
|
||||
|
||||
Revision 1.30 2007/02/05 14:16:33 corsini
|
||||
add from euler angles to rotation matrix conversion
|
||||
|
||||
Revision 1.29 2005/12/02 09:46:49 croccia
|
||||
Corrected bug in == and != Matrix44 operators
|
||||
|
||||
Revision 1.28 2005/06/28 17:42:47 ganovelli
|
||||
added Matrix44Diag
|
||||
|
||||
Revision 1.27 2005/06/17 05:28:47 cignoni
|
||||
Completed Shear Matrix code and comments,
|
||||
Added use of swap inside Transpose
|
||||
Added more complete comments on the usage of Decompose
|
||||
|
||||
Revision 1.26 2005/06/10 15:04:12 cignoni
|
||||
Added Various missing functions: SetShearXY, SetShearXZ, SetShearYZ, SetScale for point3 and Decompose
|
||||
Completed *=(scalar); made uniform GetRow and GetColumn
|
||||
|
||||
Revision 1.25 2005/04/14 11:35:09 ponchio
|
||||
*** empty log message ***
|
||||
|
||||
Revision 1.24 2005/03/18 00:14:39 cignoni
|
||||
removed small gcc compiling issues
|
||||
|
||||
Revision 1.23 2005/03/15 11:40:56 cignoni
|
||||
Added operator*=( std::vector<PointType> ...) to apply a matrix to a vector of vertexes (replacement of the old style mesh.Apply(tr).
|
||||
|
||||
Revision 1.22 2004/12/15 18:45:50 tommyfranken
|
||||
*** empty log message ***
|
||||
|
||||
Revision 1.21 2004/10/22 14:41:30 ponchio
|
||||
return in operator+ added.
|
||||
|
||||
Revision 1.20 2004/10/18 15:03:14 fiorin
|
||||
Updated interface: all Matrix classes have now the same interface
|
||||
|
||||
Revision 1.19 2004/10/07 14:23:57 ganovelli
|
||||
added function to take rows and comlumns. Added toMatrix and fromMatrix to comply
|
||||
RotationTYpe prototype in Similarity.h
|
||||
|
||||
Revision 1.18 2004/05/28 13:01:50 ganovelli
|
||||
changed scalar to ScalarType
|
||||
|
||||
Revision 1.17 2004/05/26 15:09:32 cignoni
|
||||
better comments in set rotate
|
||||
|
||||
Revision 1.16 2004/05/07 10:05:50 cignoni
|
||||
Corrected abuse of for index variable scope
|
||||
|
||||
Revision 1.15 2004/05/04 23:19:41 cignoni
|
||||
Clarified initial comment, removed vector*matrix operator (confusing!)
|
||||
Corrected translate and Rotate, removed gl stuff.
|
||||
|
||||
Revision 1.14 2004/05/04 02:34:03 ganovelli
|
||||
wrong use of operator [] corrected
|
||||
|
||||
Revision 1.13 2004/04/07 10:45:54 cignoni
|
||||
Added: [i][j] access, V() for the raw float values, constructor from T[16]
|
||||
|
||||
Revision 1.12 2004/03/25 14:57:49 ponchio
|
||||
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef __VCGLIB_MATRIX44
|
||||
#define __VCGLIB_MATRIX44
|
||||
|
||||
#include <memory.h>
|
||||
#include <vcg/math/base.h>
|
||||
#include <vcg/space/point3.h>
|
||||
#include <vcg/space/point4.h>
|
||||
#include <vector>
|
||||
|
||||
|
||||
namespace vcg {
|
||||
|
||||
/*
|
||||
Annotations:
|
||||
Opengl stores matrix in column-major order. That is, the matrix is stored as:
|
||||
|
||||
a0 a4 a8 a12
|
||||
a1 a5 a9 a13
|
||||
a2 a6 a10 a14
|
||||
a3 a7 a11 a15
|
||||
|
||||
Usually in opengl (see opengl specs) vectors are 'column' vectors
|
||||
so usually matrix are PRE-multiplied for a vector.
|
||||
So the command glTranslate generate a matrix that
|
||||
is ready to be premultipled for a vector:
|
||||
|
||||
1 0 0 tx
|
||||
0 1 0 ty
|
||||
0 0 1 tz
|
||||
0 0 0 1
|
||||
|
||||
Matrix44 stores matrix in row-major order i.e.
|
||||
|
||||
a0 a1 a2 a3
|
||||
a4 a5 a6 a7
|
||||
a8 a9 a10 a11
|
||||
a12 a13 a14 a15
|
||||
|
||||
So for the use of that matrix in opengl with their supposed meaning you have to transpose them before feeding to glMultMatrix.
|
||||
This mechanism is hidden by the templated function defined in wrap/gl/math.h;
|
||||
If your machine has the ARB_transpose_matrix extension it will use the appropriate;
|
||||
The various gl-like command SetRotate, SetTranslate assume that you are making matrix
|
||||
for 'column' vectors.
|
||||
|
||||
*/
|
||||
|
||||
template <class S>
|
||||
class Matrix44Diag:public Point4<S>{
|
||||
public:
|
||||
/** @name Matrix33
|
||||
Class Matrix33Diag.
|
||||
This is the class for definition of a diagonal matrix 4x4.
|
||||
@param S (Templete Parameter) Specifies the ScalarType field.
|
||||
*/
|
||||
Matrix44Diag(const S & p0,const S & p1,const S & p2,const S & p3):Point4<S>(p0,p1,p2,p3){};
|
||||
Matrix44Diag( const Point4<S> & p ):Point4<S>(p){};
|
||||
};
|
||||
|
||||
|
||||
/** This class represent a 4x4 matrix. T is the kind of element in the matrix.
|
||||
*/
|
||||
template <class T> class Matrix44 {
|
||||
protected:
|
||||
T _a[16];
|
||||
|
||||
public:
|
||||
typedef T ScalarType;
|
||||
|
||||
///@{
|
||||
|
||||
/** $name Constructors
|
||||
* No automatic casting and default constructor is empty
|
||||
*/
|
||||
Matrix44() {};
|
||||
~Matrix44() {};
|
||||
Matrix44(const Matrix44 &m);
|
||||
Matrix44(const T v[]);
|
||||
|
||||
/// Number of columns
|
||||
inline unsigned int ColumnsNumber() const
|
||||
{
|
||||
return 4;
|
||||
};
|
||||
|
||||
/// Number of rows
|
||||
inline unsigned int RowsNumber() const
|
||||
{
|
||||
return 4;
|
||||
};
|
||||
|
||||
T &ElementAt(const int row, const int col);
|
||||
T ElementAt(const int row, const int col) const;
|
||||
//T &operator[](const int i);
|
||||
//const T &operator[](const int i) const;
|
||||
T *V();
|
||||
const T *V() const ;
|
||||
|
||||
T *operator[](const int i);
|
||||
const T *operator[](const int i) const;
|
||||
|
||||
// return a copy of the i-th column
|
||||
Point4<T> GetColumn4(const int& i)const{
|
||||
assert(i>=0 && i<4);
|
||||
return Point4<T>(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i),ElementAt(3,i));
|
||||
//return Point4<T>(_a[i],_a[i+4],_a[i+8],_a[i+12]);
|
||||
}
|
||||
|
||||
Point3<T> GetColumn3(const int& i)const{
|
||||
assert(i>=0 && i<4);
|
||||
return Point3<T>(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i));
|
||||
}
|
||||
|
||||
Point4<T> GetRow4(const int& i)const{
|
||||
assert(i>=0 && i<4);
|
||||
return Point4<T>(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2),ElementAt(i,3));
|
||||
// return *((Point4<T>*)(&_a[i<<2])); alternativa forse + efficiente
|
||||
}
|
||||
|
||||
Point3<T> GetRow3(const int& i)const{
|
||||
assert(i>=0 && i<4);
|
||||
return Point3<T>(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2));
|
||||
// return *((Point4<T>*)(&_a[i<<2])); alternativa forse + efficiente
|
||||
}
|
||||
|
||||
Matrix44 operator+(const Matrix44 &m) const;
|
||||
Matrix44 operator-(const Matrix44 &m) const;
|
||||
Matrix44 operator*(const Matrix44 &m) const;
|
||||
Matrix44 operator*(const Matrix44Diag<T> &m) const;
|
||||
Point4<T> operator*(const Point4<T> &v) const;
|
||||
|
||||
bool operator==(const Matrix44 &m) const;
|
||||
bool operator!= (const Matrix44 &m) const;
|
||||
|
||||
Matrix44 operator-() const;
|
||||
Matrix44 operator*(const T k) const;
|
||||
void operator+=(const Matrix44 &m);
|
||||
void operator-=(const Matrix44 &m);
|
||||
void operator*=( const Matrix44 & m );
|
||||
void operator*=( const T k );
|
||||
|
||||
template <class Matrix44Type>
|
||||
void ToMatrix(Matrix44Type & m) const {for(int i = 0; i < 16; i++) m.V()[i]=V()[i];}
|
||||
|
||||
void ToEulerAngles(T &alpha, T &beta, T &gamma);
|
||||
|
||||
template <class Matrix44Type>
|
||||
void FromMatrix(const Matrix44Type & m){for(int i = 0; i < 16; i++) V()[i]=m.V()[i];}
|
||||
void FromEulerAngles(T alpha, T beta, T gamma);
|
||||
void SetZero();
|
||||
void SetIdentity();
|
||||
void SetDiagonal(const T k);
|
||||
Matrix44 &SetScale(const T sx, const T sy, const T sz);
|
||||
Matrix44 &SetScale(const Point3<T> &t);
|
||||
Matrix44 &SetTranslate(const Point3<T> &t);
|
||||
Matrix44 &SetTranslate(const T sx, const T sy, const T sz);
|
||||
Matrix44 &SetShearXY(const T sz);
|
||||
Matrix44 &SetShearXZ(const T sy);
|
||||
Matrix44 &SetShearYZ(const T sx);
|
||||
|
||||
///use radiants for angle.
|
||||
Matrix44 &SetRotateDeg(T AngleDeg, const Point3<T> & axis);
|
||||
Matrix44 &SetRotateRad(T AngleRad, const Point3<T> & axis);
|
||||
|
||||
T Determinant() const;
|
||||
|
||||
template <class Q> void Import(const Matrix44<Q> &m) {
|
||||
for(int i = 0; i < 16; i++)
|
||||
_a[i] = (T)(m.V()[i]);
|
||||
}
|
||||
template <class Q>
|
||||
static inline Matrix44 Construct( const Matrix44<Q> & b )
|
||||
{
|
||||
Matrix44<T> tmp; tmp.FromMatrix(b);
|
||||
return tmp;
|
||||
}
|
||||
|
||||
static inline const Matrix44 &Identity( )
|
||||
{
|
||||
static Matrix44<T> tmp; tmp.SetIdentity();
|
||||
return tmp;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
/** Class for solving A * x = b. */
|
||||
template <class T> class LinearSolve: public Matrix44<T> {
|
||||
public:
|
||||
LinearSolve(const Matrix44<T> &m);
|
||||
Point4<T> Solve(const Point4<T> &b); // solve A · x = b
|
||||
///If you need to solve some equation you can use this function instead of Matrix44 one for speed.
|
||||
T Determinant() const;
|
||||
protected:
|
||||
///Holds row permutation.
|
||||
int index[4]; //hold permutation
|
||||
///Hold sign of row permutation (used for determinant sign)
|
||||
T d;
|
||||
bool Decompose();
|
||||
};
|
||||
|
||||
/*** Postmultiply */
|
||||
//template <class T> Point3<T> operator*(const Point3<T> &p, const Matrix44<T> &m);
|
||||
|
||||
///Premultiply
|
||||
template <class T> Point3<T> operator*(const Matrix44<T> &m, const Point3<T> &p);
|
||||
|
||||
template <class T> Matrix44<T> &Transpose(Matrix44<T> &m);
|
||||
//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 T> Matrix44<T>::Matrix44(const Matrix44<T> &m) {
|
||||
memcpy((T *)_a, (T *)m._a, 16 * sizeof(T));
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T>::Matrix44(const T v[]) {
|
||||
memcpy((T *)_a, v, 16 * sizeof(T));
|
||||
}
|
||||
|
||||
template <class T> T &Matrix44<T>::ElementAt(const int row, const int col) {
|
||||
assert(row >= 0 && row < 4);
|
||||
assert(col >= 0 && col < 4);
|
||||
return _a[(row<<2) + col];
|
||||
}
|
||||
|
||||
template <class T> T Matrix44<T>::ElementAt(const int row, const int col) const {
|
||||
assert(row >= 0 && row < 4);
|
||||
assert(col >= 0 && col < 4);
|
||||
return _a[(row<<2) + col];
|
||||
}
|
||||
|
||||
//template <class T> T &Matrix44<T>::operator[](const int i) {
|
||||
// assert(i >= 0 && i < 16);
|
||||
// return ((T *)_a)[i];
|
||||
//}
|
||||
//
|
||||
//template <class T> const T &Matrix44<T>::operator[](const int i) const {
|
||||
// assert(i >= 0 && i < 16);
|
||||
// return ((T *)_a)[i];
|
||||
//}
|
||||
template <class T> T *Matrix44<T>::operator[](const int i) {
|
||||
assert(i >= 0 && 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;
|
||||
}
|
||||
template <class T> T *Matrix44<T>::V() { return _a;}
|
||||
template <class T> const T *Matrix44<T>::V() const { return _a;}
|
||||
|
||||
|
||||
template <class T> Matrix44<T> Matrix44<T>::operator+(const Matrix44 &m) const {
|
||||
Matrix44<T> ret;
|
||||
for(int i = 0; i < 16; i++)
|
||||
ret.V()[i] = V()[i] + m.V()[i];
|
||||
return ret;
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> Matrix44<T>::operator-(const Matrix44 &m) const {
|
||||
Matrix44<T> ret;
|
||||
for(int i = 0; i < 16; i++)
|
||||
ret.V()[i] = V()[i] - m.V()[i];
|
||||
return ret;
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> Matrix44<T>::operator*(const Matrix44 &m) const {
|
||||
Matrix44 ret;
|
||||
for(int i = 0; i < 4; i++)
|
||||
for(int j = 0; j < 4; j++) {
|
||||
T t = 0.0;
|
||||
for(int k = 0; k < 4; k++)
|
||||
t += ElementAt(i, k) * m.ElementAt(k, j);
|
||||
ret.ElementAt(i, j) = t;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> Matrix44<T>::operator*(const Matrix44Diag<T> &m) const {
|
||||
Matrix44 ret = (*this);
|
||||
for(int i = 0; i < 4; ++i)
|
||||
for(int j = 0; j < 4; ++j)
|
||||
ret[i][j]*=m[i];
|
||||
return ret;
|
||||
}
|
||||
|
||||
template <class T> Point4<T> Matrix44<T>::operator*(const Point4<T> &v) const {
|
||||
Point4<T> ret;
|
||||
for(int i = 0; i < 4; i++){
|
||||
T t = 0.0;
|
||||
for(int k = 0; k < 4; k++)
|
||||
t += ElementAt(i,k) * v[k];
|
||||
ret[i] = t;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
template <class T> bool Matrix44<T>::operator==(const Matrix44 &m) const {
|
||||
for(int i = 0; i < 4; ++i)
|
||||
for(int j = 0; j < 4; ++j)
|
||||
if(ElementAt(i,j) != m.ElementAt(i,j))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
template <class T> bool Matrix44<T>::operator!=(const Matrix44 &m) const {
|
||||
for(int i = 0; i < 4; ++i)
|
||||
for(int j = 0; j < 4; ++j)
|
||||
if(ElementAt(i,j) != m.ElementAt(i,j))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> Matrix44<T>::operator-() const {
|
||||
Matrix44<T> res;
|
||||
for(int i = 0; i < 16; i++)
|
||||
res.V()[i] = -V()[i];
|
||||
return res;
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> Matrix44<T>::operator*(const T k) const {
|
||||
Matrix44<T> res;
|
||||
for(int i = 0; i < 16; i++)
|
||||
res.V()[i] =V()[i] * k;
|
||||
return res;
|
||||
}
|
||||
|
||||
template <class T> void Matrix44<T>::operator+=(const Matrix44 &m) {
|
||||
for(int i = 0; i < 16; i++)
|
||||
V()[i] += m.V()[i];
|
||||
}
|
||||
template <class T> void Matrix44<T>::operator-=(const Matrix44 &m) {
|
||||
for(int i = 0; i < 16; i++)
|
||||
V()[i] -= m.V()[i];
|
||||
}
|
||||
template <class T> void Matrix44<T>::operator*=( const Matrix44 & m ) {
|
||||
*this = *this *m;
|
||||
|
||||
/*for(int i = 0; i < 4; i++) { //sbagliato
|
||||
Point4<T> t(0, 0, 0, 0);
|
||||
for(int k = 0; k < 4; k++) {
|
||||
for(int j = 0; j < 4; j++) {
|
||||
t[k] += ElementAt(i, k) * m.ElementAt(k, j);
|
||||
}
|
||||
}
|
||||
for(int l = 0; l < 4; l++)
|
||||
ElementAt(i, l) = t[l];
|
||||
} */
|
||||
}
|
||||
|
||||
template < class PointType , class T > void operator*=( std::vector<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>::operator*=( const T k ) {
|
||||
for(int i = 0; i < 16; i++)
|
||||
_a[i] *= k;
|
||||
}
|
||||
|
||||
template <class T>
|
||||
void Matrix44<T>::ToEulerAngles(T &alpha, T &beta, T &gamma)
|
||||
{
|
||||
alpha = atan2(ElementAt(1,2), ElementAt(2,2));
|
||||
beta = asin(-ElementAt(0,2));
|
||||
gamma = atan2(ElementAt(0,1), ElementAt(1,1));
|
||||
}
|
||||
|
||||
template <class T>
|
||||
void Matrix44<T>::FromEulerAngles(T alpha, T beta, T gamma)
|
||||
{
|
||||
this->SetZero();
|
||||
|
||||
T cosalpha = cos(alpha);
|
||||
T cosbeta = cos(beta);
|
||||
T cosgamma = cos(gamma);
|
||||
T sinalpha = sin(alpha);
|
||||
T sinbeta = sin(beta);
|
||||
T singamma = sin(gamma);
|
||||
|
||||
ElementAt(0,0) = cosbeta * cosgamma;
|
||||
ElementAt(1,0) = -cosalpha * singamma + sinalpha * sinbeta * cosgamma;
|
||||
ElementAt(2,0) = sinalpha * singamma + cosalpha * sinbeta * cosgamma;
|
||||
|
||||
ElementAt(0,1) = cosbeta * singamma;
|
||||
ElementAt(1,1) = cosalpha * cosgamma + sinalpha * sinbeta * singamma;
|
||||
ElementAt(2,1) = -sinalpha * cosgamma + cosalpha * sinbeta * singamma;
|
||||
|
||||
ElementAt(0,2) = -sinbeta;
|
||||
ElementAt(1,2) = sinalpha * cosbeta;
|
||||
ElementAt(2,2) = cosalpha * cosbeta;
|
||||
|
||||
ElementAt(3,3) = 1;
|
||||
}
|
||||
|
||||
template <class T> void Matrix44<T>::SetZero() {
|
||||
memset((T *)_a, 0, 16 * sizeof(T));
|
||||
}
|
||||
|
||||
template <class T> void Matrix44<T>::SetIdentity() {
|
||||
SetDiagonal(1);
|
||||
}
|
||||
|
||||
template <class T> void Matrix44<T>::SetDiagonal(const T 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<T> &t) {
|
||||
SetScale(t[0], t[1], t[2]);
|
||||
return *this;
|
||||
}
|
||||
template <class T> Matrix44<T> &Matrix44<T>::SetScale(const T sx, const T sy, const T sz) {
|
||||
SetZero();
|
||||
ElementAt(0, 0) = sx;
|
||||
ElementAt(1, 1) = sy;
|
||||
ElementAt(2, 2) = sz;
|
||||
ElementAt(3, 3) = 1;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> &Matrix44<T>::SetTranslate(const Point3<T> &t) {
|
||||
SetTranslate(t[0], t[1], t[2]);
|
||||
return *this;
|
||||
}
|
||||
template <class T> Matrix44<T> &Matrix44<T>::SetTranslate(const T tx, const T ty, const T tz) {
|
||||
SetIdentity();
|
||||
ElementAt(0, 3) = tx;
|
||||
ElementAt(1, 3) = ty;
|
||||
ElementAt(2, 3) = tz;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> &Matrix44<T>::SetRotateDeg(T AngleDeg, const Point3<T> & axis) {
|
||||
return SetRotateRad(math::ToRad(AngleDeg),axis);
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> &Matrix44<T>::SetRotateRad(T AngleRad, const Point3<T> & 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 T sh) {// shear the X coordinate as the Y coordinate change
|
||||
SetIdentity();
|
||||
ElementAt(0,1) = sh;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> & Matrix44<T>:: SetShearXZ( const T sh) {// shear the X coordinate as the Z coordinate change
|
||||
SetIdentity();
|
||||
ElementAt(0,2) = sh;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> &Matrix44<T>:: SetShearYZ( const T sh) {// shear the Y coordinate as the Z coordinate change
|
||||
SetIdentity();
|
||||
ElementAt(1,2) = sh;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
Given a non singular, non projective matrix (e.g. with the last row equal to [0,0,0,1] )
|
||||
This procedure decompose it in a sequence of
|
||||
Scale,Shear,Rotation e Translation
|
||||
|
||||
- ScaleV and Tranv are obiviously scaling and translation.
|
||||
- ShearV contains three scalars with, respectively
|
||||
ShearXY, ShearXZ e ShearYZ
|
||||
- RotateV contains the rotations (in degree!) around the x,y,z axis
|
||||
The input matrix is modified leaving inside it a simple roto translation.
|
||||
|
||||
To obtain the original matrix the above transformation have to be applied in the strict following way:
|
||||
|
||||
OriginalMatrix = Trn * Rtx*Rty*Rtz * ShearYZ*ShearXZ*ShearXY * Scl
|
||||
|
||||
Example Code:
|
||||
double srv() { return (double(rand()%40)-20)/2.0; } // small random value
|
||||
|
||||
srand(time(0));
|
||||
Point3d ScV(10+srv(),10+srv(),10+srv()),ScVOut(-1,-1,-1);
|
||||
Point3d ShV(srv(),srv(),srv()),ShVOut(-1,-1,-1);
|
||||
Point3d RtV(10+srv(),srv(),srv()),RtVOut(-1,-1,-1);
|
||||
Point3d TrV(srv(),srv(),srv()),TrVOut(-1,-1,-1);
|
||||
|
||||
Matrix44d Scl; Scl.SetScale(ScV);
|
||||
Matrix44d Sxy; Sxy.SetShearXY(ShV[0]);
|
||||
Matrix44d Sxz; Sxz.SetShearXZ(ShV[1]);
|
||||
Matrix44d Syz; Syz.SetShearYZ(ShV[2]);
|
||||
Matrix44d Rtx; Rtx.SetRotate(math::ToRad(RtV[0]),Point3d(1,0,0));
|
||||
Matrix44d Rty; Rty.SetRotate(math::ToRad(RtV[1]),Point3d(0,1,0));
|
||||
Matrix44d Rtz; Rtz.SetRotate(math::ToRad(RtV[2]),Point3d(0,0,1));
|
||||
Matrix44d Trn; Trn.SetTranslate(TrV);
|
||||
|
||||
Matrix44d StartM = Trn * Rtx*Rty*Rtz * Syz*Sxz*Sxy *Scl;
|
||||
Matrix44d ResultM=StartM;
|
||||
Decompose(ResultM,ScVOut,ShVOut,RtVOut,TrVOut);
|
||||
|
||||
Scl.SetScale(ScVOut);
|
||||
Sxy.SetShearXY(ShVOut[0]);
|
||||
Sxz.SetShearXZ(ShVOut[1]);
|
||||
Syz.SetShearYZ(ShVOut[2]);
|
||||
Rtx.SetRotate(math::ToRad(RtVOut[0]),Point3d(1,0,0));
|
||||
Rty.SetRotate(math::ToRad(RtVOut[1]),Point3d(0,1,0));
|
||||
Rtz.SetRotate(math::ToRad(RtVOut[2]),Point3d(0,0,1));
|
||||
Trn.SetTranslate(TrVOut);
|
||||
|
||||
// Now Rebuild is equal to StartM
|
||||
Matrix44d RebuildM = Trn * Rtx*Rty*Rtz * Syz*Sxz*Sxy * Scl ;
|
||||
*/
|
||||
template <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]*M.GetColumn3(1); // xy shearing
|
||||
R[1]= M.GetColumn3(1)-R[0]*ShearV[0];
|
||||
assert(math::Abs(R[1]*R[0])<1e-10);
|
||||
ScaleV[1]=Norm(R[1]); // y scaling
|
||||
R[1]=R[1]/ScaleV[1];
|
||||
ShearV[0]=ShearV[0]/ScaleV[1];
|
||||
|
||||
ShearV[1]=R[0]*M.GetColumn3(2); // xz shearing
|
||||
R[2]= M.GetColumn3(2)-R[0]*ShearV[1];
|
||||
assert(math::Abs(R[2]*R[0])<1e-10);
|
||||
|
||||
R[2] = R[2]-R[1]*(R[2]*R[1]);
|
||||
assert(math::Abs(R[2]*R[1])<1e-10);
|
||||
assert(math::Abs(R[2]*R[0])<1e-10);
|
||||
|
||||
ScaleV[2]=Norm(R[2]);
|
||||
ShearV[1]=ShearV[1]/ScaleV[2];
|
||||
R[2]=R[2]/ScaleV[2];
|
||||
assert(math::Abs(R[2]*R[1])<1e-10);
|
||||
assert(math::Abs(R[2]*R[0])<1e-10);
|
||||
|
||||
ShearV[2]=R[1]*M.GetColumn3(2); // yz shearing
|
||||
ShearV[2]=ShearV[2]/ScaleV[2];
|
||||
int i,j;
|
||||
for(i=0;i<3;++i)
|
||||
for(j=0;j<3;++j)
|
||||
M[i][j]=R[j][i];
|
||||
|
||||
// Third and last step: Recover the rotation
|
||||
//now the matrix should be a pure rotation matrix so its determinant is +-1
|
||||
double det=M.Determinant();
|
||||
if(math::Abs(det)<1e-10) return false; // matrix should be at least invertible...
|
||||
assert(math::Abs(math::Abs(det)-1.0)<1e-10); // it should be +-1...
|
||||
if(det<0) {
|
||||
ScaleV *= -1;
|
||||
M *= -1;
|
||||
}
|
||||
|
||||
double alpha,beta,gamma; // rotations around the x,y and z axis
|
||||
beta=asin( M[0][2]);
|
||||
double cosbeta=cos(beta);
|
||||
if(math::Abs(cosbeta) > 1e-5)
|
||||
{
|
||||
alpha=asin(-M[1][2]/cosbeta);
|
||||
if((M[2][2]/cosbeta) < 0 ) alpha=M_PI-alpha;
|
||||
gamma=asin(-M[0][1]/cosbeta);
|
||||
if((M[0][0]/cosbeta)<0) gamma = M_PI-gamma;
|
||||
}
|
||||
else
|
||||
{
|
||||
alpha=asin(-M[1][0]);
|
||||
if(M[1][1]<0) alpha=M_PI-alpha;
|
||||
gamma=0;
|
||||
}
|
||||
|
||||
RotV[0]=math::ToDeg(alpha);
|
||||
RotV[1]=math::ToDeg(beta);
|
||||
RotV[2]=math::ToDeg(gamma);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
template <class T> T Matrix44<T>::Determinant() const {
|
||||
LinearSolve<T> solve(*this);
|
||||
return solve.Determinant();
|
||||
}
|
||||
|
||||
|
||||
template <class T> Point3<T> operator*(const Matrix44<T> &m, const Point3<T> &p) {
|
||||
T w;
|
||||
Point3<T> s;
|
||||
s[0] = m.ElementAt(0, 0)*p[0] + m.ElementAt(0, 1)*p[1] + m.ElementAt(0, 2)*p[2] + m.ElementAt(0, 3);
|
||||
s[1] = m.ElementAt(1, 0)*p[0] + m.ElementAt(1, 1)*p[1] + m.ElementAt(1, 2)*p[2] + m.ElementAt(1, 3);
|
||||
s[2] = m.ElementAt(2, 0)*p[0] + m.ElementAt(2, 1)*p[1] + m.ElementAt(2, 2)*p[2] + m.ElementAt(2, 3);
|
||||
w = m.ElementAt(3, 0)*p[0] + m.ElementAt(3, 1)*p[1] + m.ElementAt(3, 2)*p[2] + m.ElementAt(3, 3);
|
||||
if(w!= 0) s /= w;
|
||||
return s;
|
||||
}
|
||||
|
||||
//template <class T> Point3<T> operator*(const Point3<T> &p, const Matrix44<T> &m) {
|
||||
// T w;
|
||||
// Point3<T> s;
|
||||
// s[0] = m.ElementAt(0, 0)*p[0] + m.ElementAt(1, 0)*p[1] + m.ElementAt(2, 0)*p[2] + m.ElementAt(3, 0);
|
||||
// s[1] = m.ElementAt(0, 1)*p[0] + m.ElementAt(1, 1)*p[1] + m.ElementAt(2, 1)*p[2] + m.ElementAt(3, 1);
|
||||
// s[2] = m.ElementAt(0, 2)*p[0] + m.ElementAt(1, 2)*p[1] + m.ElementAt(2, 2)*p[2] + m.ElementAt(3, 2);
|
||||
// w = m.ElementAt(0, 3)*p[0] + m.ElementAt(1, 3)*p[1] + m.ElementAt(2, 3)*p[2] + m.ElementAt(3, 3);
|
||||
// if(w != 0) s /= w;
|
||||
// return s;
|
||||
//}
|
||||
|
||||
template <class T> Matrix44<T> &Transpose(Matrix44<T> &m) {
|
||||
for(int i = 1; i < 4; i++)
|
||||
for(int j = 0; j < i; j++) {
|
||||
math::Swap(m.ElementAt(i, j), m.ElementAt(j, i));
|
||||
}
|
||||
return m;
|
||||
}
|
||||
|
||||
/*
|
||||
To invert a matrix you can
|
||||
either invert the matrix inplace calling
|
||||
|
||||
vcg::Invert(yourMatrix);
|
||||
|
||||
or get the inverse matrix of a given matrix without touching it:
|
||||
|
||||
invertedMatrix = vcg::Inverse(untouchedMatrix);
|
||||
|
||||
*/
|
||||
template <class T> Matrix44<T> & Invert(Matrix44<T> &m) {
|
||||
LinearSolve<T> solve(m);
|
||||
|
||||
for(int j = 0; j < 4; j++) { //Find inverse by columns.
|
||||
Point4<T> col(0, 0, 0, 0);
|
||||
col[j] = 1.0;
|
||||
col = solve.Solve(col);
|
||||
for(int i = 0; i < 4; i++)
|
||||
m.ElementAt(i, j) = col[i];
|
||||
}
|
||||
return m;
|
||||
}
|
||||
|
||||
template <class T> Matrix44<T> Inverse(const Matrix44<T> &m) {
|
||||
LinearSolve<T> solve(m);
|
||||
Matrix44<T> res;
|
||||
for(int j = 0; j < 4; j++) { //Find inverse by columns.
|
||||
Point4<T> col(0, 0, 0, 0);
|
||||
col[j] = 1.0;
|
||||
col = solve.Solve(col);
|
||||
for(int i = 0; i < 4; i++)
|
||||
res.ElementAt(i, j) = col[i];
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* LINEAR SOLVE IMPLEMENTATION */
|
||||
|
||||
template <class T> LinearSolve<T>::LinearSolve(const Matrix44<T> &m): Matrix44<T>(m) {
|
||||
if(!Decompose()) {
|
||||
for(int i = 0; i < 4; i++)
|
||||
index[i] = i;
|
||||
Matrix44<T>::SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
template <class T> T LinearSolve<T>::Determinant() const {
|
||||
T det = d;
|
||||
for(int j = 0; j < 4; j++)
|
||||
det *= this-> ElementAt(j, j);
|
||||
return det;
|
||||
}
|
||||
|
||||
|
||||
/*replaces a matrix by its LU decomposition of a rowwise permutation.
|
||||
d is +or -1 depeneing of row permutation even or odd.*/
|
||||
#define TINY 1e-100
|
||||
|
||||
template <class T> bool LinearSolve<T>::Decompose() {
|
||||
|
||||
/* Matrix44<T> A;
|
||||
for(int i = 0; i < 16; i++)
|
||||
A[i] = operator[](i);
|
||||
SetIdentity();
|
||||
Point4<T> scale;
|
||||
// Set scale factor, scale(i) = max( |a(i,j)| ), for each row
|
||||
for(int i = 0; i < 4; i++ ) {
|
||||
index[i] = i; // Initialize row index list
|
||||
T scalemax = (T)0.0;
|
||||
for(int j = 0; j < 4; j++)
|
||||
scalemax = (scalemax > math::Abs(A.ElementAt(i,j))) ? scalemax : math::Abs(A.ElementAt(i,j));
|
||||
scale[i] = scalemax;
|
||||
}
|
||||
|
||||
// Loop over rows k = 1, ..., (N-1)
|
||||
int signDet = 1;
|
||||
for(int k = 0; k < 3; k++ ) {
|
||||
// Select pivot row from max( |a(j,k)/s(j)| )
|
||||
T ratiomax = (T)0.0;
|
||||
int jPivot = k;
|
||||
for(int i = k; i < 4; i++ ) {
|
||||
T ratio = math::Abs(A.ElementAt(index[i], k))/scale[index[i]];
|
||||
if(ratio > ratiomax) {
|
||||
jPivot = i;
|
||||
ratiomax = ratio;
|
||||
}
|
||||
}
|
||||
// Perform pivoting using row index list
|
||||
int indexJ = index[k];
|
||||
if( jPivot != k ) { // Pivot
|
||||
indexJ = index[jPivot];
|
||||
index[jPivot] = index[k]; // Swap index jPivot and k
|
||||
index[k] = indexJ;
|
||||
signDet *= -1; // Flip sign of determinant
|
||||
}
|
||||
// Perform forward elimination
|
||||
for(int i=k+1; i < 4; i++ ) {
|
||||
T coeff = A.ElementAt(index[i],k)/A.ElementAt(indexJ,k);
|
||||
for(int j=k+1; j < 4; j++ )
|
||||
A.ElementAt(index[i],j) -= coeff*A.ElementAt(indexJ,j);
|
||||
A.ElementAt(index[i],k) = coeff;
|
||||
//for( j=1; j< 4; j++ )
|
||||
// ElementAt(index[i],j) -= A.ElementAt(index[i], k)*ElementAt(indexJ, j);
|
||||
}
|
||||
}
|
||||
for(int i = 0; i < 16; i++)
|
||||
operator[](i) = A[i];
|
||||
|
||||
d = signDet;
|
||||
// this = A;
|
||||
return true; */
|
||||
|
||||
d = 1; //no permutation still
|
||||
|
||||
T scaling[4];
|
||||
int i,j,k;
|
||||
//Saving the scvaling information per row
|
||||
for(i = 0; i < 4; i++) {
|
||||
T largest = 0.0;
|
||||
for(j = 0; j < 4; j++) {
|
||||
T t = math::Abs(this->ElementAt(i, j));
|
||||
if (t > largest) largest = t;
|
||||
}
|
||||
|
||||
if (largest == 0.0) { //oooppps there is a zero row!
|
||||
return false;
|
||||
}
|
||||
scaling[i] = (T)1.0 / largest;
|
||||
}
|
||||
|
||||
int imax = 0;
|
||||
for(j = 0; j < 4; j++) {
|
||||
for(i = 0; i < j; i++) {
|
||||
T sum = this->ElementAt(i,j);
|
||||
for(int k = 0; k < i; k++)
|
||||
sum -= this->ElementAt(i,k)*this->ElementAt(k,j);
|
||||
this->ElementAt(i,j) = sum;
|
||||
}
|
||||
T largest = 0.0;
|
||||
for(i = j; i < 4; i++) {
|
||||
T sum = this->ElementAt(i,j);
|
||||
for(k = 0; k < j; k++)
|
||||
sum -= this->ElementAt(i,k)*this->ElementAt(k,j);
|
||||
this->ElementAt(i,j) = sum;
|
||||
T t = scaling[i] * math::Abs(sum);
|
||||
if(t >= largest) {
|
||||
largest = t;
|
||||
imax = i;
|
||||
}
|
||||
}
|
||||
if (j != imax) {
|
||||
for (int k = 0; k < 4; k++) {
|
||||
T dum = this->ElementAt(imax,k);
|
||||
this->ElementAt(imax,k) = this->ElementAt(j,k);
|
||||
this->ElementAt(j,k) = dum;
|
||||
}
|
||||
d = -(d);
|
||||
scaling[imax] = scaling[j];
|
||||
}
|
||||
index[j]=imax;
|
||||
if (this->ElementAt(j,j) == 0.0) this->ElementAt(j,j) = (T)TINY;
|
||||
if (j != 3) {
|
||||
T dum = (T)1.0 / (this->ElementAt(j,j));
|
||||
for (i = j+1; i < 4; i++)
|
||||
this->ElementAt(i,j) *= dum;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
template <class T> Point4<T> LinearSolve<T>::Solve(const Point4<T> &b) {
|
||||
Point4<T> x(b);
|
||||
int first = -1, ip;
|
||||
for(int i = 0; i < 4; i++) {
|
||||
ip = index[i];
|
||||
T sum = x[ip];
|
||||
x[ip] = x[i];
|
||||
if(first!= -1)
|
||||
for(int j = first; j <= i-1; j++)
|
||||
sum -= this->ElementAt(i,j) * x[j];
|
||||
else
|
||||
if(sum) first = i;
|
||||
x[i] = sum;
|
||||
}
|
||||
for (int i = 3; i >= 0; i--) {
|
||||
T sum = x[i];
|
||||
for (int j = i+1; j < 4; j++)
|
||||
sum -= this->ElementAt(i, j) * x[j];
|
||||
x[i] = sum / this->ElementAt(i, i);
|
||||
}
|
||||
return x;
|
||||
}
|
||||
|
||||
} //namespace
|
||||
#endif
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
/****************************************************************************
|
||||
* VCGLib o o *
|
||||
* Visual and Computer Graphics Library o o *
|
||||
* _ O _ *
|
||||
* Copyright(C) 2004 \/)\/ *
|
||||
* Visual Computing Lab /\/| *
|
||||
* ISTI - Italian National Research Council | *
|
||||
* \ *
|
||||
* All rights reserved. *
|
||||
* *
|
||||
* This program is free software; you can redistribute it and/or modify *
|
||||
* it under the terms of the GNU General Public License as published by *
|
||||
* the Free Software Foundation; either version 2 of the License, or *
|
||||
* (at your option) any later version. *
|
||||
* *
|
||||
* This program is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
||||
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
||||
* for more details. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef EIGEN_VCGLIB
|
||||
#define EIGEN_VCGLIB
|
||||
|
||||
#define EIGEN_MATRIXBASE_PLUGIN <vcg/math/eigen_vcgaddons.h>
|
||||
|
||||
#include "../Eigen/Array"
|
||||
#include "../Eigen/Core"
|
||||
|
||||
#define VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, Op) \
|
||||
template<typename OtherDerived> \
|
||||
Derived& operator Op(const Eigen::MatrixBase<OtherDerived>& other) \
|
||||
{ \
|
||||
Base::operator Op(other.derived()); return *this;\
|
||||
} \
|
||||
Derived& operator Op(const Derived& other) \
|
||||
{ \
|
||||
Base::operator Op(other); return *this;\
|
||||
}
|
||||
|
||||
#define VCG_EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, Op) \
|
||||
template<typename Other> \
|
||||
Derived& operator Op(const Other& scalar) \
|
||||
{ \
|
||||
Base::operator Op(scalar); return *this;\
|
||||
}
|
||||
|
||||
#define VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
|
||||
VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, =) \
|
||||
VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, +=) \
|
||||
VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, -=) \
|
||||
VCG_EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, *=) \
|
||||
VCG_EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, /=)
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,319 @@
|
|||
/****************************************************************************
|
||||
* VCGLib o o *
|
||||
* Visual and Computer Graphics Library o o *
|
||||
* _ O _ *
|
||||
* Copyright(C) 2004 \/)\/ *
|
||||
* Visual Computing Lab /\/| *
|
||||
* ISTI - Italian National Research Council | *
|
||||
* \ *
|
||||
* All rights reserved. *
|
||||
* *
|
||||
* This program is free software; you can redistribute it and/or modify *
|
||||
* it under the terms of the GNU General Public License as published by *
|
||||
* the Free Software Foundation; either version 2 of the License, or *
|
||||
* (at your option) any later version. *
|
||||
* *
|
||||
* This program is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
||||
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
||||
* for more details. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
|
||||
#warning You are including deprecated math stuff
|
||||
/*!
|
||||
* \deprecated use cols()
|
||||
* Number of columns
|
||||
*/
|
||||
EIGEN_DEPRECATED inline unsigned int ColumnsNumber() const
|
||||
{
|
||||
return cols();
|
||||
};
|
||||
|
||||
|
||||
/*!
|
||||
* \deprecated use rows()
|
||||
* Number of rows
|
||||
*/
|
||||
EIGEN_DEPRECATED inline unsigned int RowsNumber() const
|
||||
{
|
||||
return rows();
|
||||
};
|
||||
|
||||
/*
|
||||
* \deprecated use *this.isApprox(m) or *this.cwise() == m
|
||||
* Equality operator.
|
||||
* \param m
|
||||
* \return true iff the matrices have same size and its elements have same values.
|
||||
*/
|
||||
// template<typename OtherDerived>
|
||||
// EIGEN_DEPRECATED bool operator==(const MatrixBase<OtherDerived> &m) const
|
||||
// {
|
||||
// return (this->cwise() == m);
|
||||
// }
|
||||
|
||||
/*
|
||||
* \deprecated use !*this.isApprox(m) or *this.cwise() != m
|
||||
* Inequality operator
|
||||
* \param m
|
||||
* \return true iff the matrices have different size or if their elements have different values.
|
||||
*/
|
||||
// template<typename OtherDerived>
|
||||
// EIGEN_DEPRECATED bool operator!=(const MatrixBase<OtherDerived> &m) const
|
||||
// {
|
||||
// return (this->cwise() != m);
|
||||
// };
|
||||
|
||||
/*!
|
||||
* \deprecated use *this(i,j) (or *this.coeff(i,j))
|
||||
* 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
|
||||
*/
|
||||
EIGEN_DEPRECATED inline Scalar ElementAt(unsigned int i, unsigned int j)
|
||||
{
|
||||
return (*this)(i,j);
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.determinant() (or *this.lu().determinant() for large matrices)
|
||||
* Calculate and return the matrix determinant (Laplace)
|
||||
* \return the matrix determinant
|
||||
*/
|
||||
EIGEN_DEPRECATED Scalar Determinant() const
|
||||
{
|
||||
return determinant();
|
||||
};
|
||||
|
||||
/*!
|
||||
* Return the cofactor <I>A<SUB>i,j</SUB></I> of the <I>a<SUB>i,j</SUB></I> element
|
||||
* \return ...
|
||||
*/
|
||||
EIGEN_DEPRECATED Scalar Cofactor(unsigned int i, unsigned int j) const
|
||||
{
|
||||
assert(rows() == cols());
|
||||
assert(rows()>2);
|
||||
return (((i+j)%2==0) ? 1. : -1.) * minor(i,j).determinant();
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.col(j)
|
||||
* Get the <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.
|
||||
*/
|
||||
EIGEN_DEPRECATED ColXpr GetColumn(const unsigned int j)
|
||||
{
|
||||
return col(j);
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.row(i)
|
||||
* 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.
|
||||
*/
|
||||
EIGEN_DEPRECATED RowXpr GetRow(const unsigned int i)
|
||||
{
|
||||
return row(i);
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use m1.col(i).swap(m1.col(j));
|
||||
* Swaps the values of the elements between the <I>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
|
||||
*/
|
||||
EIGEN_DEPRECATED void SwapColumns(const unsigned int i, const unsigned int j)
|
||||
{
|
||||
if (i==j)
|
||||
return;
|
||||
|
||||
col(i).swap(col(j));
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use m1.col(i).swap(m1.col(j))
|
||||
* Swaps the values of the elements between the <I>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
|
||||
*/
|
||||
EIGEN_DEPRECATED void SwapRows(const unsigned int i, const unsigned int j)
|
||||
{
|
||||
if (i==j)
|
||||
return;
|
||||
|
||||
row(i).swap(row(j));
|
||||
};
|
||||
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.cwise() += k
|
||||
* (Modifier) Add to each element of this matrix the scalar constant <I>k</I>.
|
||||
* \param k the scalar constant
|
||||
* \return the modified matrix
|
||||
*/
|
||||
EIGEN_DEPRECATED Derived& operator+=(const Scalar k)
|
||||
{
|
||||
cwise() += k;
|
||||
return *this;
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.cwise() -= k
|
||||
* (Modifier) Subtract from each element of this matrix the scalar constant <I>k</I>.
|
||||
* \param k the scalar constant
|
||||
* \return the modified matrix
|
||||
*/
|
||||
EIGEN_DEPRECATED Derived& operator-=(const Scalar k)
|
||||
{
|
||||
cwise() -= k;
|
||||
return *this;
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.dot
|
||||
* Matrix multiplication: calculates the cross product.
|
||||
* \param reference to the matrix to multiply by
|
||||
* \return the matrix product
|
||||
*/
|
||||
// template <int N,int M>
|
||||
// EIGEN_DEPRECATED void DotProduct(Point<N,Scalar> &m,Point<M,Scalar> &result)
|
||||
// {
|
||||
// unsigned int i, j;
|
||||
// for (i=0; i<M; i++)
|
||||
// { result[i]=0;
|
||||
// for (j=0; j<N; j++)
|
||||
// result[i]+=(*this)[i][j]*m[j];
|
||||
// }
|
||||
// };
|
||||
|
||||
/*!
|
||||
* \deprecated use (*this) * vec.asDiagonal() or (*this) * mat.mark<Diagonal>()
|
||||
* Matrix multiplication by a diagonal matrix
|
||||
*/
|
||||
// EIGEN_DEPRECATED Matrix<Scalar> operator*(const MatrixDiagBase &m) const
|
||||
// {
|
||||
// assert(_columns == _rows);
|
||||
// assert(_columns == m.Dimension());
|
||||
// int i,j;
|
||||
// Matrix<Scalar> result(_rows, _columns);
|
||||
//
|
||||
// for (i=0; i<result._rows; i++)
|
||||
// for (j=0; j<result._columns; j++)
|
||||
// result[i][j]*= m[j];
|
||||
//
|
||||
// return result;
|
||||
// };
|
||||
|
||||
/*!
|
||||
* \deprecated use *this = a * b.transpose()
|
||||
* Matrix from outer product.
|
||||
*/
|
||||
template <typename OtherDerived1, typename OtherDerived2>
|
||||
EIGEN_DEPRECATED void OuterProduct(const MatrixBase<OtherDerived1>& a, const MatrixBase<OtherDerived2>& b)
|
||||
{
|
||||
*this = a * b.transpose();
|
||||
}
|
||||
|
||||
typedef CwiseUnaryOp<ei_scalar_add_op<Scalar>, Derived> ScalarAddReturnType;
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.cwise() + k
|
||||
* Scalar sum.
|
||||
* \param k
|
||||
* \return the resultant matrix
|
||||
*/
|
||||
EIGEN_DEPRECATED const ScalarAddReturnType operator+(const Scalar k) { return cwise() + k; }
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.cwise() - k
|
||||
* Scalar difference.
|
||||
* \param k
|
||||
* \return the resultant matrix
|
||||
*/
|
||||
EIGEN_DEPRECATED const ScalarAddReturnType operator-(const Scalar k) { return cwise() - k; }
|
||||
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.setZero() or *this = MatrixType::Zero(rows,cols), etc.
|
||||
* Set all the matrix elements to zero.
|
||||
*/
|
||||
EIGEN_DEPRECATED void SetZero()
|
||||
{
|
||||
setZero();
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.setIdentity() or *this = MatrixType::Identity(rows,cols), etc.
|
||||
* Set the matrix to identity.
|
||||
*/
|
||||
EIGEN_DEPRECATED void SetIdentity()
|
||||
{
|
||||
setIdentity();
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.col(j) = expression
|
||||
* Set the values of <I>j</I>-th column to v[j]
|
||||
* \param j the column index
|
||||
* \param v ...
|
||||
*/
|
||||
EIGEN_DEPRECATED void SetColumn(const unsigned int j, Scalar* v)
|
||||
{
|
||||
col(j) = Map<Matrix<Scalar,RowsAtCompileTime,1> >(v,cols(),1);
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.row(i) = expression
|
||||
* Set the elements of the <I>i</I>-th row to v[j]
|
||||
* \param i the row index
|
||||
* \param v ...
|
||||
*/
|
||||
EIGEN_DEPRECATED void SetRow(const unsigned int i, Scalar* v)
|
||||
{
|
||||
row(i) = Map<Matrix<Scalar,1,ColsAtCompileTime> >(v,1,rows());
|
||||
};
|
||||
|
||||
/*!
|
||||
* \deprecated use *this.diagonal() = expression
|
||||
* Set the diagonal elements <I>v<SUB>i,i</SUB></I> to v[i]
|
||||
* \param v
|
||||
*/
|
||||
EIGEN_DEPRECATED void SetDiagonal(Scalar *v)
|
||||
{
|
||||
assert(rows() == cols());
|
||||
diagonal() = Map<Matrix<Scalar,RowsAtCompileTime,1> >(v,cols(),1);
|
||||
}
|
||||
|
||||
|
||||
/*!
|
||||
* \deprecated use *this = *this.transpose()
|
||||
*/
|
||||
// Transpose already exist
|
||||
// EIGEN_DEPRECATED void Transpose()
|
||||
// {
|
||||
// assert(0 && "dangerous use of deprecated Transpose function, please use: m = m.transpose();");
|
||||
// };
|
||||
|
||||
|
||||
/*!
|
||||
* \deprecated use ostream << *this or ostream << *this.withFormat(...)
|
||||
* Print all matrix elements
|
||||
*/
|
||||
EIGEN_DEPRECATED void Dump()
|
||||
{
|
||||
unsigned int i, j;
|
||||
for (i=0; i<rows(); ++i)
|
||||
{
|
||||
printf("[\t");
|
||||
for (j=0; j<cols(); j++)
|
||||
printf("%f\t", coeff(i,j));
|
||||
printf("]\n");
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -8,7 +8,7 @@
|
|||
* \ *
|
||||
* All rights reserved. *
|
||||
* *
|
||||
* This program is free software; you can redistribute it and/or modify *
|
||||
* 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. *
|
||||
|
|
@ -20,759 +20,180 @@
|
|||
* 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
|
||||
#ifndef VCG_USE_EIGEN
|
||||
#include "deprecated_matrix.h"
|
||||
#endif
|
||||
|
||||
Revision 1.7 2006/04/29 10:26:04 fiorin
|
||||
Added some utility methods (swapping of columns and rows, matrix-vector multiplication)
|
||||
|
||||
Revision 1.6 2006/04/11 08:09:35 zifnab1974
|
||||
changes necessary for gcc 3.4.5 on linux 64bit. Please take note of case-sensitivity of filenames
|
||||
|
||||
Revision 1.5 2005/12/12 11:25:00 ganovelli
|
||||
added diagonal matrix, outer produce and namespace
|
||||
|
||||
***************************************************************************/
|
||||
|
||||
#ifndef MATRIX_VCGLIB
|
||||
#define MATRIX_VCGLIB
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <memory.h>
|
||||
#include <assert.h>
|
||||
#include <algorithm>
|
||||
#include "eigen.h"
|
||||
#include <vcg/space/point.h>
|
||||
#include <vcg/math/lin_algebra.h>
|
||||
|
||||
namespace vcg{
|
||||
namespace ndim{
|
||||
namespace ndim{
|
||||
template<class Scalar> class Matrix;
|
||||
}
|
||||
}
|
||||
|
||||
/** \addtogroup math */
|
||||
/* @{ */
|
||||
namespace Eigen{
|
||||
template<typename Scalar>
|
||||
struct ei_traits<vcg::ndim::Matrix<Scalar> > : ei_traits<Eigen::Matrix<Scalar,Dynamic,Dynamic> > {};
|
||||
}
|
||||
|
||||
/*!
|
||||
* This class represent a diagonal <I>m</I>×<I>m</I> matrix.
|
||||
*/
|
||||
|
||||
class MatrixDiagBase{public:
|
||||
virtual const int & Dimension()const =0;
|
||||
virtual const 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){}
|
||||
};
|
||||
namespace vcg{
|
||||
namespace ndim{
|
||||
|
||||
/** \addtogroup math */
|
||||
/* @{ */
|
||||
|
||||
/*!
|
||||
* This class represent a generic <I>m</I>×<I>n</I> matrix. The class is templated over the scalar type field.
|
||||
* @param TYPE (Templete Parameter) Specifies the ScalarType field.
|
||||
* This class represent a diagonal <I>m</I><EFBFBD><I>m</I> matrix.
|
||||
*/
|
||||
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(-1, 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>=0 && 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>=0 && 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;
|
||||
};
|
||||
|
||||
|
||||
/*!
|
||||
* 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;
|
||||
}
|
||||
|
||||
class MatrixDiagBase{public:
|
||||
virtual const int & Dimension()const =0;
|
||||
virtual const 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 Scalar (Templete Parameter) Specifies the ScalarType field.
|
||||
*/
|
||||
template<class _Scalar>
|
||||
class Matrix : public Eigen::Matrix<_Scalar,Eigen::Dynamic,Eigen::Dynamic> // FIXME col or row major ?
|
||||
{
|
||||
|
||||
typedef Eigen::Matrix<_Scalar,Eigen::Dynamic,Eigen::Dynamic> _Base;
|
||||
|
||||
public:
|
||||
|
||||
_EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix,_Base);
|
||||
typedef _Scalar ScalarType;
|
||||
|
||||
VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Matrix)
|
||||
|
||||
/*!
|
||||
* Default constructor
|
||||
* All the elements are initialized to zero.
|
||||
* \param m the number of matrix rows
|
||||
* \param n the number of matrix columns
|
||||
*/
|
||||
Matrix(unsigned int m, unsigned int n)
|
||||
: Base(m,n)
|
||||
{
|
||||
memset(Base::data(), 0, m*n*sizeof(Scalar));
|
||||
}
|
||||
}; // end of namespace
|
||||
|
||||
/*!
|
||||
* Constructor
|
||||
* The matrix elements are initialized with the values of the elements in \i values.
|
||||
* \param m the number of matrix rows
|
||||
* \param n the number of matrix columns
|
||||
* \param values the values of the matrix elements
|
||||
*/
|
||||
Matrix(unsigned int m, unsigned int n, Scalar *values)
|
||||
: Base(m.n)
|
||||
{
|
||||
*this = Eigen::Map<Eigen::Matrix<Scalar,Dynamic,Dynamic,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
|
||||
*/
|
||||
/*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()
|
||||
* 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));
|
||||
};
|
||||
|
||||
// EIGEN_DEPRECATED void Transpose()
|
||||
// {
|
||||
// assert(0 && "dangerous use of deprecated Transpose function, please use: m = m.transpose();");
|
||||
// }
|
||||
};
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -19,622 +19,212 @@
|
|||
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
||||
* for more details. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
History
|
||||
|
||||
$Log: not supported by cvs2svn $
|
||||
Revision 1.18 2007/04/19 14:30:26 pietroni
|
||||
added RotationMatrix method to calculate rotation matrix along an axis
|
||||
|
||||
Revision 1.17 2007/04/07 23:06:47 pietroni
|
||||
Added function RotationMatrix
|
||||
|
||||
Revision 1.16 2007/01/29 00:20:25 pietroni
|
||||
-Used scalar type passed as template argument istead of double to prevent warnings.. in Rotate function
|
||||
|
||||
Revision 1.15 2006/09/25 23:05:29 ganovelli
|
||||
added constructor from matrix44 excluding a row and colum
|
||||
|
||||
Revision 1.14 2006/06/22 08:00:05 ganovelli
|
||||
bug in operator + with MatrixxDig
|
||||
|
||||
Revision 1.13 2006/01/20 16:41:44 pietroni
|
||||
added operators:
|
||||
operator -= ( const Matrix33Diag<S> &p )
|
||||
Matrix33 operator - ( const Matrix33Diag<S> &p )
|
||||
Matrix33 operator + ( const Matrix33 &m )
|
||||
Matrix33 operator + ( const Matrix33Diag<S> &p )
|
||||
|
||||
Revision 1.12 2005/11/14 10:28:25 cignoni
|
||||
Changed Invert -> FastInvert for the function based on the maple expansion
|
||||
|
||||
Revision 1.11 2005/10/13 15:45:23 ponchio
|
||||
Changed a Zero in SetZero in WeightedCrossCovariance() (again)
|
||||
|
||||
Revision 1.10 2005/10/05 17:06:12 pietroni
|
||||
corrected sintax error on singular value decomposition
|
||||
|
||||
Revision 1.9 2005/09/29 09:53:58 ganovelli
|
||||
added inverse by SVD
|
||||
|
||||
Revision 1.8 2005/06/10 14:51:54 cignoni
|
||||
Changed a Zero in SetZero in WeightedCrossCovariance()
|
||||
|
||||
Revision 1.7 2005/06/10 11:46:49 pietroni
|
||||
Added Norm Function
|
||||
|
||||
Revision 1.6 2005/06/07 14:29:56 ganovelli
|
||||
changed from Matrix33Ide to MatrixeeDiag
|
||||
|
||||
Revision 1.5 2005/05/23 15:05:26 ganovelli
|
||||
Matrix33Diag Added: it implements diagonal matrix. Added only operator += in Matrix33
|
||||
|
||||
Revision 1.4 2005/04/11 14:11:22 pietroni
|
||||
changed swap to math::Swap in Traspose Function
|
||||
|
||||
Revision 1.3 2004/10/18 15:03:02 fiorin
|
||||
Updated interface: all Matrix classes have now the same interface
|
||||
|
||||
Revision 1.2 2004/07/13 06:48:26 cignoni
|
||||
removed uppercase references in include
|
||||
|
||||
Revision 1.1 2004/05/28 13:09:05 ganovelli
|
||||
created
|
||||
|
||||
Revision 1.1 2004/05/28 13:00:39 ganovelli
|
||||
created
|
||||
|
||||
|
||||
****************************************************************************/
|
||||
|
||||
// #ifndef VCG_USE_EIGEN
|
||||
#include "deprecated_matrix33.h"
|
||||
// #endif
|
||||
|
||||
#ifndef __VCGLIB_MATRIX33_H
|
||||
#define __VCGLIB_MATRIX33_H
|
||||
|
||||
#include <stdio.h>
|
||||
#include <vcg/math/lin_algebra.h>
|
||||
#include <vcg/math/matrix44.h>
|
||||
#include <vcg/space/point3.h>
|
||||
#include <vector>
|
||||
#include "eigen.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> > {};
|
||||
}
|
||||
|
||||
namespace vcg {
|
||||
|
||||
template <class S>
|
||||
class Matrix33Diag:public Point3<S>{
|
||||
public:
|
||||
|
||||
/** @name Matrix33
|
||||
Class Matrix33Diag.
|
||||
This is the class for definition of a diagonal matrix 3x3.
|
||||
@param S (Templete Parameter) Specifies the ScalarType field.
|
||||
*/
|
||||
Matrix33Diag(const S & p0,const S & p1,const S & p2):Point3<S>(p0,p1,p2){};
|
||||
Matrix33Diag(const Point3<S>&p ):Point3<S>(p){};
|
||||
};
|
||||
|
||||
template<class S>
|
||||
/** @name Matrix33
|
||||
Class Matrix33.
|
||||
This is the class for definition of a matrix 3x3.
|
||||
@param S (Templete Parameter) Specifies the ScalarType field.
|
||||
*/
|
||||
class Matrix33
|
||||
template<class _Scalar>
|
||||
class Matrix33 : public Eigen::Matrix<_Scalar,3,3,RowMajor> // FIXME col or row major ?
|
||||
{
|
||||
|
||||
typedef Eigen::Matrix<_Scalar,Eigen::Dynamic,Eigen::Dynamic> _Base;
|
||||
|
||||
public:
|
||||
typedef S ScalarType;
|
||||
|
||||
_EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix,_Base);
|
||||
typedef _Scalar ScalarType;
|
||||
|
||||
VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Matrix)
|
||||
|
||||
/// Default constructor
|
||||
inline Matrix33() {}
|
||||
inline Matrix33() : Base() {}
|
||||
|
||||
/// Copy constructor
|
||||
Matrix33( const Matrix33 & m )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] = m.a[i];
|
||||
}
|
||||
Matrix33(const Matrix33& m ) : Base(m) {}
|
||||
|
||||
/// create from array
|
||||
Matrix33( const S * v )
|
||||
{
|
||||
for(int i=0;i<9;++i) a[i] = v[i];
|
||||
}
|
||||
/// create from a \b row-major array
|
||||
Matrix33(const Scalar * v ) : Base(Map<Matrix<Scalar,3,3,RowMajor>(v)) {}
|
||||
|
||||
/// create from Matrix44 excluding row and column k
|
||||
Matrix33 (const Matrix44<S> & m, const int & k)
|
||||
{
|
||||
int i,j, r=0, c=0;
|
||||
for(i = 0; i< 4;++i)if(i!=k){c=0;
|
||||
for(j=0; j < 4;++j)if(j!=k)
|
||||
{ (*this)[r][c] = m[i][j]; ++c;}
|
||||
++r;
|
||||
}
|
||||
}
|
||||
Matrix33(const Matrix44<S> & m, const int & k) : Base(m.minor(k,k)) {}
|
||||
|
||||
/// Number of columns
|
||||
inline unsigned int ColumnsNumber() const
|
||||
{
|
||||
return 3;
|
||||
};
|
||||
|
||||
/// Number of rows
|
||||
inline unsigned int RowsNumber() const
|
||||
{
|
||||
return 3;
|
||||
};
|
||||
|
||||
/// Assignment operator
|
||||
Matrix33 & operator = ( const Matrix33 & m )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] = m.a[i];
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// Operatore di indicizzazione
|
||||
inline S * operator [] ( const int i )
|
||||
{
|
||||
return a+i*3;
|
||||
}
|
||||
/// Operatore const di indicizzazione
|
||||
inline const S * operator [] ( const int i ) const
|
||||
{
|
||||
return a+i*3;
|
||||
}
|
||||
|
||||
|
||||
/// Modificatore somma per matrici 3x3
|
||||
Matrix33 & operator += ( const Matrix33 &m )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] += m.a[i];
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Modificatore somma per matrici 3x3
|
||||
Matrix33 & operator += ( const Matrix33Diag<S> &p )
|
||||
{
|
||||
a[0] += p[0];
|
||||
a[4] += p[1];
|
||||
a[8] += p[2];
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Modificatore sottrazione per matrici 3x3
|
||||
Matrix33 & operator -= ( const Matrix33 &m )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] -= m.a[i];
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Modificatore somma per matrici 3x3
|
||||
Matrix33 & operator -= ( const Matrix33Diag<S> &p )
|
||||
{
|
||||
a[0] -= p[0];
|
||||
a[4] -= p[1];
|
||||
a[8] -= p[2];
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Modificatore divisione per scalare
|
||||
Matrix33 & operator /= ( const S &s )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] /= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/// Modificatore prodotto per matrice
|
||||
Matrix33 operator * ( const Matrix33< S> & t ) const
|
||||
{
|
||||
Matrix33<S> r;
|
||||
|
||||
int i,j;
|
||||
for(i=0;i<3;++i)
|
||||
for(j=0;j<3;++j)
|
||||
r[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j];
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Modificatore prodotto per matrice
|
||||
void operator *=( const Matrix33< S> & t )
|
||||
{
|
||||
int i,j;
|
||||
for(i=0;i<3;++i)
|
||||
for(j=0;j<3;++j)
|
||||
(*this)[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j];
|
||||
|
||||
}
|
||||
|
||||
/// Dot product with a diagonal matrix
|
||||
Matrix33 operator * ( const Matrix33Diag< S> & t ) const
|
||||
{
|
||||
Matrix33<S> r;
|
||||
|
||||
int i,j;
|
||||
for(i=0;i<3;++i)
|
||||
for(j=0;j<3;++j)
|
||||
r[i][j] = (*this)[i][j]*t[j];
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Dot product modifier with a diagonal matrix
|
||||
void operator *=( const Matrix33Diag< S> & t )
|
||||
{
|
||||
int i,j;
|
||||
for(i=0;i<3;++i)
|
||||
for(j=0;j<3;++j)
|
||||
(*this)[i][j] = (*this)[i][j]*t[j];
|
||||
}
|
||||
|
||||
/// Modificatore prodotto per costante
|
||||
Matrix33 & operator *= ( const S t )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
a[i] *= t;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Operatore prodotto per costante
|
||||
Matrix33 operator * ( const S t ) const
|
||||
{
|
||||
Matrix33<S> r;
|
||||
for(int i=0;i<9;++i)
|
||||
r.a[i] = a[i]* t;
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Operatore sottrazione per matrici 3x3
|
||||
Matrix33 operator - ( const Matrix33 &m ) const
|
||||
{
|
||||
Matrix33<S> r;
|
||||
for(int i=0;i<9;++i)
|
||||
r.a[i] = a[i] - m.a[i];
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Operatore sottrazione di matrici 3x3 con matrici diagonali
|
||||
Matrix33 operator - ( const Matrix33Diag<S> &p ) const
|
||||
{
|
||||
Matrix33<S> r=a;
|
||||
r[0][0] -= p[0];
|
||||
r[1][1] -= p[1];
|
||||
r[2][2] -= p[2];
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Operatore sottrazione per matrici 3x3
|
||||
Matrix33 operator + ( const Matrix33 &m ) const
|
||||
{
|
||||
Matrix33<S> r;
|
||||
for(int i=0;i<9;++i)
|
||||
r.a[i] = a[i] + m.a[i];
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Operatore addizione di matrici 3x3 con matrici diagonali
|
||||
Matrix33 operator + ( const Matrix33Diag<S> &p ) const
|
||||
{
|
||||
Matrix33<S> r=(*this);
|
||||
r[0][0] += p[0];
|
||||
r[1][1] += p[1];
|
||||
r[2][2] += p[2];
|
||||
return r;
|
||||
}
|
||||
|
||||
/** Operatore per il prodotto matrice-vettore.
|
||||
@param v A point in $R^{3}$
|
||||
@return Il vettore risultante in $R^{3}$
|
||||
/*!
|
||||
* \deprecated use *this.row(i)
|
||||
*/
|
||||
Point3<S> operator * ( const Point3<S> & v ) const
|
||||
{
|
||||
Point3<S> t;
|
||||
inline typename Base::RowXpr operator[](const unsigned int i)
|
||||
{ return Base::row(i); }
|
||||
|
||||
t[0] = a[0]*v[0] + a[1]*v[1] + a[2]*v[2];
|
||||
t[1] = a[3]*v[0] + a[4]*v[1] + a[5]*v[2];
|
||||
t[2] = a[6]*v[0] + a[7]*v[1] + a[8]*v[2];
|
||||
return t;
|
||||
}
|
||||
/*!
|
||||
* \deprecated use *this.row(i)
|
||||
*/
|
||||
inline const typename Base::RowXpr operator[](const unsigned int i) const
|
||||
{ return Base::row(i); }
|
||||
|
||||
void OuterProduct(Point3<S> const &p0, Point3<S> const &p1) {
|
||||
Point3<S> row;
|
||||
row = p1*p0[0];
|
||||
a[0] = row[0];a[1] = row[1];a[2] = row[2];
|
||||
row = p1*p0[1];
|
||||
a[3] = row[0]; a[4] = row[1]; a[5] = row[2];
|
||||
row = p1*p0[2];
|
||||
a[6] = row[0];a[7] = row[1];a[8] = row[2];
|
||||
}
|
||||
|
||||
Matrix33 & SetZero() {
|
||||
for(int i=0;i<9;++i) a[i] =0;
|
||||
return (*this);
|
||||
}
|
||||
Matrix33 & SetIdentity() {
|
||||
for(int i=0;i<9;++i) a[i] =0;
|
||||
a[0]=a[4]=a[8]=1.0;
|
||||
return (*this);
|
||||
}
|
||||
|
||||
/** \deprecated */
|
||||
Matrix33 & SetRotateRad(S angle, const Point3<S> & axis )
|
||||
{
|
||||
S c = cos(angle);
|
||||
S s = sin(angle);
|
||||
S q = 1-c;
|
||||
Point3<S> t = axis;
|
||||
t.Normalize();
|
||||
a[0] = t[0]*t[0]*q + c;
|
||||
a[1] = t[0]*t[1]*q - t[2]*s;
|
||||
a[2] = t[0]*t[2]*q + t[1]*s;
|
||||
a[3] = t[1]*t[0]*q + t[2]*s;
|
||||
a[4] = t[1]*t[1]*q + c;
|
||||
a[5] = t[1]*t[2]*q - t[0]*s;
|
||||
a[6] = t[2]*t[0]*q -t[1]*s;
|
||||
a[7] = t[2]*t[1]*q +t[0]*s;
|
||||
a[8] = t[2]*t[2]*q +c;
|
||||
*this = Eigen::AngleAxis<Scalar>(angle,axis).toRotationMatrix();
|
||||
return (*this);
|
||||
}
|
||||
/** \deprecated */
|
||||
Matrix33 & SetRotateDeg(S angle, const Point3<S> & axis ){
|
||||
return SetRotateRad(math::ToRad(angle),axis);
|
||||
}
|
||||
|
||||
/// Funzione per eseguire la trasposta della matrice
|
||||
Matrix33 & Transpose()
|
||||
{
|
||||
math::Swap(a[1],a[3]);
|
||||
math::Swap(a[2],a[6]);
|
||||
math::Swap(a[5],a[7]);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/// Funzione per costruire una matrice diagonale dati i tre elem.
|
||||
Matrix33 & SetDiagonal(S *v)
|
||||
{int i,j;
|
||||
for(i=0;i<3;i++)
|
||||
for(j=0;j<3;j++)
|
||||
if(i==j) (*this)[i][j] = v[i];
|
||||
else (*this)[i][j] = 0;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
/// Assegna l'n-simo vettore colonna
|
||||
void SetColumn(const int n, S* v){
|
||||
assert( (n>=0) && (n<3) );
|
||||
a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2];
|
||||
};
|
||||
|
||||
/// Assegna l'n-simo vettore riga
|
||||
void SetRow(const int n, S* v){
|
||||
assert( (n>=0) && (n<3) );
|
||||
int m=n*3;
|
||||
a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2];
|
||||
};
|
||||
|
||||
/// Assegna l'n-simo vettore colonna
|
||||
void SetColumn(const int n, const Point3<S> v){
|
||||
assert( (n>=0) && (n<3) );
|
||||
a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2];
|
||||
};
|
||||
|
||||
/// Assegna l'n-simo vettore riga
|
||||
void SetRow(const int n, const Point3<S> v){
|
||||
assert( (n>=0) && (n<3) );
|
||||
int m=n*3;
|
||||
a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2];
|
||||
};
|
||||
|
||||
/// Restituisce l'n-simo vettore colonna
|
||||
Point3<S> GetColumn(const int n) const {
|
||||
assert( (n>=0) && (n<3) );
|
||||
Point3<S> t;
|
||||
t[0]=a[n]; t[1]=a[n+3]; t[2]=a[n+6];
|
||||
return t;
|
||||
};
|
||||
|
||||
/// Restituisce l'n-simo vettore riga
|
||||
Point3<S> GetRow(const int n) const {
|
||||
assert( (n>=0) && (n<3) );
|
||||
Point3<S> t;
|
||||
int m=n*3;
|
||||
t[0]=a[m]; t[1]=a[m+1]; t[2]=a[m+2];
|
||||
return t;
|
||||
};
|
||||
|
||||
|
||||
|
||||
/// Funzione per il calcolo del determinante
|
||||
S Determinant() const
|
||||
{
|
||||
return a[0]*(a[4]*a[8]-a[5]*a[7]) -
|
||||
a[1]*(a[3]*a[8]-a[5]*a[6]) +
|
||||
a[2]*(a[3]*a[7]-a[4]*a[6]) ;
|
||||
}
|
||||
|
||||
// Warning, this Inversion code can be HIGHLY NUMERICALLY UNSTABLE!
|
||||
// In most case you are advised to use the Invert() method based on SVD decomposition.
|
||||
|
||||
Matrix33 & FastInvert()
|
||||
{
|
||||
// Maple produsse:
|
||||
S t4 = a[0]*a[4];
|
||||
S t6 = a[0]*a[5];
|
||||
S t8 = a[1]*a[3];
|
||||
S t10 = a[2]*a[3];
|
||||
S t12 = a[1]*a[6];
|
||||
S t14 = a[2]*a[6];
|
||||
S t17 = 1/(t4*a[8]-t6*a[7]-t8*a[8]+t10*a[7]+t12*a[5]-t14*a[4]);
|
||||
S a0 = a[0];
|
||||
S a1 = a[1];
|
||||
S a3 = a[3];
|
||||
S a4 = a[4];
|
||||
a[0] = (a[4]*a[8]-a[5]*a[7])*t17;
|
||||
a[1] = -(a[1]*a[8]-a[2]*a[7])*t17;
|
||||
a[2] = (a1 *a[5]-a[2]*a[4])*t17;
|
||||
a[3] = -(a[3]*a[8]-a[5]*a[6])*t17;
|
||||
a[4] = (a0 *a[8]-t14 )*t17;
|
||||
a[5] = -(t6 - t10)*t17;
|
||||
a[6] = (a3 *a[7]-a[4]*a[6])*t17;
|
||||
a[7] = -(a[0]*a[7]-t12)*t17;
|
||||
a[8] = (t4-t8)*t17;
|
||||
|
||||
return *this;
|
||||
}
|
||||
/** \deprecated */
|
||||
Matrix33 & FastInvert() { return *this = inverse(); }
|
||||
|
||||
void show(FILE * fp)
|
||||
{
|
||||
for(int i=0;i<3;++i)
|
||||
printf("| %g \t%g \t%g |\n",a[3*i+0],a[3*i+1],a[3*i+2]);
|
||||
printf("| %g \t%g \t%g |\n",coeff(i,0),coeff(i,1),coeff(i,2));
|
||||
}
|
||||
|
||||
// return the Trace of the matrix i.e. the sum of the diagonal elements
|
||||
S Trace() const
|
||||
{
|
||||
return a[0]+a[4]+a[8];
|
||||
}
|
||||
|
||||
/*
|
||||
compute the matrix generated by the product of a * b^T
|
||||
*/
|
||||
void ExternalProduct(const Point3<S> &a, const Point3<S> &b)
|
||||
{
|
||||
for(int i=0;i<3;++i)
|
||||
for(int j=0;j<3;++j)
|
||||
(*this)[i][j] = a[i]*b[j];
|
||||
}
|
||||
|
||||
/* Compute the Frobenius Norm of the Matrix
|
||||
*/
|
||||
ScalarType Norm()
|
||||
{
|
||||
ScalarType SQsum=0;
|
||||
for(int i=0;i<3;++i)
|
||||
for(int j=0;j<3;++j)
|
||||
SQsum += a[i]*a[i];
|
||||
return (math::Sqrt(SQsum));
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
It compute the covariance matrix of a set of 3d points. Returns the barycenter
|
||||
*/
|
||||
template <class STLPOINTCONTAINER >
|
||||
void Covariance(const STLPOINTCONTAINER &points, Point3<S> &bp) {
|
||||
assert(!points.empty());
|
||||
typedef typename STLPOINTCONTAINER::const_iterator PointIte;
|
||||
// first cycle: compute the barycenter
|
||||
bp.Zero();
|
||||
for( PointIte pi = points.begin(); pi != points.end(); ++pi) bp+= (*pi);
|
||||
bp/=points.size();
|
||||
// second cycle: compute the covariance matrix
|
||||
this->SetZero();
|
||||
vcg::Matrix33<ScalarType> A;
|
||||
for( PointIte pi = points.begin(); pi != points.end(); ++pi) {
|
||||
Point3<S> p = (*pi)-bp;
|
||||
A.OuterProduct(p,p);
|
||||
(*this)+= A;
|
||||
/*
|
||||
compute the matrix generated by the product of a * b^T
|
||||
*/
|
||||
void ExternalProduct(const Point3<S> &a, const Point3<S> &b)
|
||||
{
|
||||
for(int i=0;i<3;++i)
|
||||
for(int j=0;j<3;++j)
|
||||
(*this)[i][j] = a[i]*b[j];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
It compute the cross covariance matrix of two set of 3d points P and X;
|
||||
it returns also the barycenters of P and X.
|
||||
fonte:
|
||||
|
||||
Besl, McKay
|
||||
A method for registration o f 3d Shapes
|
||||
IEEE TPAMI Vol 14, No 2 1992
|
||||
|
||||
*/
|
||||
template <class STLPOINTCONTAINER >
|
||||
void CrossCovariance(const STLPOINTCONTAINER &P, const STLPOINTCONTAINER &X,
|
||||
Point3<S> &bp, Point3<S> &bx)
|
||||
{
|
||||
SetZero();
|
||||
assert(P.size()==X.size());
|
||||
bx.Zero();
|
||||
bp.Zero();
|
||||
Matrix33<S> tmp;
|
||||
typename std::vector <Point3<S> >::const_iterator pi,xi;
|
||||
for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
|
||||
bp+=*pi;
|
||||
bx+=*xi;
|
||||
tmp.ExternalProduct(*pi,*xi);
|
||||
(*this)+=tmp;
|
||||
/* Compute the Frobenius Norm of the Matrix
|
||||
*/
|
||||
ScalarType Norm()
|
||||
{
|
||||
// FIXME looks like there is a bug: j is not used !!!
|
||||
ScalarType SQsum=0;
|
||||
for(int i=0;i<3;++i)
|
||||
for(int j=0;j<3;++j)
|
||||
SQsum += a[i]*a[i];
|
||||
return (math::Sqrt(SQsum));
|
||||
}
|
||||
bp/=P.size();
|
||||
bx/=X.size();
|
||||
(*this)/=P.size();
|
||||
tmp.ExternalProduct(bp,bx);
|
||||
(*this)-=tmp;
|
||||
}
|
||||
|
||||
template <class STLPOINTCONTAINER, class STLREALCONTAINER>
|
||||
void WeightedCrossCovariance(const STLREALCONTAINER & weights,
|
||||
const STLPOINTCONTAINER &P,
|
||||
const STLPOINTCONTAINER &X,
|
||||
Point3<S> &bp,
|
||||
Point3<S> &bx)
|
||||
{
|
||||
SetZero();
|
||||
assert(P.size()==X.size());
|
||||
bx.Zero();
|
||||
bp.Zero();
|
||||
Matrix33<S> tmp;
|
||||
typename std::vector <Point3<S> >::const_iterator pi,xi;
|
||||
typename STLREALCONTAINER::const_iterator pw;
|
||||
|
||||
for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
|
||||
bp+=(*pi);
|
||||
bx+=(*xi);
|
||||
/**
|
||||
It computes the covariance matrix of a set of 3d points. Returns the barycenter
|
||||
*/
|
||||
// FIXME should be outside Matrix
|
||||
template <class STLPOINTCONTAINER >
|
||||
void Covariance(const STLPOINTCONTAINER &points, Point3<S> &bp) {
|
||||
assert(!points.empty());
|
||||
typedef typename STLPOINTCONTAINER::const_iterator PointIte;
|
||||
// first cycle: compute the barycenter
|
||||
bp.setZero();
|
||||
for( PointIte pi = points.begin(); pi != points.end(); ++pi) bp+= (*pi);
|
||||
bp/=points.size();
|
||||
// second cycle: compute the covariance matrix
|
||||
this->setZero();
|
||||
vcg::Matrix33<ScalarType> A;
|
||||
for( PointIte pi = points.begin(); pi != points.end(); ++pi) {
|
||||
Point3<S> p = (*pi)-bp;
|
||||
A.OuterProduct(p,p);
|
||||
(*this)+= A;
|
||||
}
|
||||
}
|
||||
bp/=P.size();
|
||||
bx/=X.size();
|
||||
|
||||
for(pi=P.begin(),xi=X.begin(),pw = weights.begin();pi!=P.end();++pi,++xi,++pw){
|
||||
|
||||
tmp.ExternalProduct(((*pi)-(bp)),((*xi)-(bp)));
|
||||
|
||||
(*this)+=tmp*(*pw);
|
||||
/**
|
||||
It computes the cross covariance matrix of two set of 3d points P and X;
|
||||
it returns also the barycenters of P and X.
|
||||
fonte:
|
||||
|
||||
Besl, McKay
|
||||
A method for registration o f 3d Shapes
|
||||
IEEE TPAMI Vol 14, No 2 1992
|
||||
|
||||
*/
|
||||
// FIXME should be outside Matrix
|
||||
template <class STLPOINTCONTAINER >
|
||||
void CrossCovariance(const STLPOINTCONTAINER &P, const STLPOINTCONTAINER &X,
|
||||
Point3<S> &bp, Point3<S> &bx)
|
||||
{
|
||||
setZero();
|
||||
assert(P.size()==X.size());
|
||||
bx.setZero();
|
||||
bp.setZero();
|
||||
Matrix33<S> tmp;
|
||||
typename std::vector <Point3<S> >::const_iterator pi,xi;
|
||||
for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
|
||||
bp+=*pi;
|
||||
bx+=*xi;
|
||||
tmp.ExternalProduct(*pi,*xi);
|
||||
(*this)+=tmp;
|
||||
}
|
||||
bp/=P.size();
|
||||
bx/=X.size();
|
||||
(*this)/=P.size();
|
||||
tmp.ExternalProduct(bp,bx);
|
||||
(*this)-=tmp;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
S a[9];
|
||||
template <class STLPOINTCONTAINER, class STLREALCONTAINER>
|
||||
void WeightedCrossCovariance(const STLREALCONTAINER & weights,
|
||||
const STLPOINTCONTAINER &P,
|
||||
const STLPOINTCONTAINER &X,
|
||||
Point3<S> &bp,
|
||||
Point3<S> &bx)
|
||||
{
|
||||
SetZero();
|
||||
assert(P.size()==X.size());
|
||||
bx.SetZero();
|
||||
bp.SetZero();
|
||||
Matrix33<S> tmp;
|
||||
typename std::vector <Point3<S> >::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)
|
||||
{
|
||||
Matrix33<S> v;
|
||||
Point3<typename Matrix33<S>::ScalarType> e;
|
||||
SingularValueDecomposition(m,&e[0],v);
|
||||
e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2];
|
||||
m.Transpose();
|
||||
m = v * Matrix33Diag<S>(e) * m;
|
||||
}
|
||||
void Invert(Matrix33<S> &m) { m = m.lu().inverse(); }
|
||||
|
||||
template <class S>
|
||||
Matrix33<S> Inverse(const Matrix33<S>&m)
|
||||
{
|
||||
Matrix33<S> v,m_copy=m;
|
||||
Point3<S> e;
|
||||
SingularValueDecomposition(m_copy,&e[0],v);
|
||||
m_copy.Transpose();
|
||||
e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2];
|
||||
return v * Matrix33Diag<S>(e) * m_copy;
|
||||
}
|
||||
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>
|
||||
|
|
|
|||
|
|
@ -20,94 +20,28 @@
|
|||
* for more details. *
|
||||
* *
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
History
|
||||
|
||||
$Log: not supported by cvs2svn $
|
||||
Revision 1.34 2007/07/12 06:42:01 cignoni
|
||||
added the missing static Construct() member
|
||||
|
||||
Revision 1.33 2007/07/03 16:06:48 corsini
|
||||
add DCM to Euler Angles conversion
|
||||
|
||||
Revision 1.32 2007/03/08 14:39:27 corsini
|
||||
final fix to euler angles transformation
|
||||
|
||||
Revision 1.31 2007/02/06 09:57:40 corsini
|
||||
fix euler angles computation
|
||||
|
||||
Revision 1.30 2007/02/05 14:16:33 corsini
|
||||
add from euler angles to rotation matrix conversion
|
||||
|
||||
Revision 1.29 2005/12/02 09:46:49 croccia
|
||||
Corrected bug in == and != Matrix44 operators
|
||||
|
||||
Revision 1.28 2005/06/28 17:42:47 ganovelli
|
||||
added Matrix44Diag
|
||||
|
||||
Revision 1.27 2005/06/17 05:28:47 cignoni
|
||||
Completed Shear Matrix code and comments,
|
||||
Added use of swap inside Transpose
|
||||
Added more complete comments on the usage of Decompose
|
||||
|
||||
Revision 1.26 2005/06/10 15:04:12 cignoni
|
||||
Added Various missing functions: SetShearXY, SetShearXZ, SetShearYZ, SetScale for point3 and Decompose
|
||||
Completed *=(scalar); made uniform GetRow and GetColumn
|
||||
|
||||
Revision 1.25 2005/04/14 11:35:09 ponchio
|
||||
*** empty log message ***
|
||||
|
||||
Revision 1.24 2005/03/18 00:14:39 cignoni
|
||||
removed small gcc compiling issues
|
||||
|
||||
Revision 1.23 2005/03/15 11:40:56 cignoni
|
||||
Added operator*=( std::vector<PointType> ...) to apply a matrix to a vector of vertexes (replacement of the old style mesh.Apply(tr).
|
||||
|
||||
Revision 1.22 2004/12/15 18:45:50 tommyfranken
|
||||
*** empty log message ***
|
||||
|
||||
Revision 1.21 2004/10/22 14:41:30 ponchio
|
||||
return in operator+ added.
|
||||
|
||||
Revision 1.20 2004/10/18 15:03:14 fiorin
|
||||
Updated interface: all Matrix classes have now the same interface
|
||||
|
||||
Revision 1.19 2004/10/07 14:23:57 ganovelli
|
||||
added function to take rows and comlumns. Added toMatrix and fromMatrix to comply
|
||||
RotationTYpe prototype in Similarity.h
|
||||
|
||||
Revision 1.18 2004/05/28 13:01:50 ganovelli
|
||||
changed scalar to ScalarType
|
||||
|
||||
Revision 1.17 2004/05/26 15:09:32 cignoni
|
||||
better comments in set rotate
|
||||
|
||||
Revision 1.16 2004/05/07 10:05:50 cignoni
|
||||
Corrected abuse of for index variable scope
|
||||
|
||||
Revision 1.15 2004/05/04 23:19:41 cignoni
|
||||
Clarified initial comment, removed vector*matrix operator (confusing!)
|
||||
Corrected translate and Rotate, removed gl stuff.
|
||||
|
||||
Revision 1.14 2004/05/04 02:34:03 ganovelli
|
||||
wrong use of operator [] corrected
|
||||
|
||||
Revision 1.13 2004/04/07 10:45:54 cignoni
|
||||
Added: [i][j] access, V() for the raw float values, constructor from T[16]
|
||||
|
||||
Revision 1.12 2004/03/25 14:57:49 ponchio
|
||||
|
||||
****************************************************************************/
|
||||
// #ifndef VCG_USE_EIGEN
|
||||
#include "deprecated_matrix44.h"
|
||||
// #endif
|
||||
|
||||
#ifndef __VCGLIB_MATRIX44
|
||||
#define __VCGLIB_MATRIX44
|
||||
|
||||
#include <memory.h>
|
||||
#include <vcg/math/base.h>
|
||||
#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> > {};
|
||||
}
|
||||
|
||||
namespace vcg {
|
||||
|
||||
|
|
@ -145,110 +79,49 @@ for 'column' vectors.
|
|||
|
||||
*/
|
||||
|
||||
template <class S>
|
||||
class Matrix44Diag:public Point4<S>{
|
||||
|
||||
/** 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,RowMajor> // FIXME col or row major !
|
||||
{
|
||||
|
||||
typedef Eigen::Matrix<_Scalar,Eigen::Dynamic,Eigen::Dynamic> _Base;
|
||||
|
||||
public:
|
||||
/** @name Matrix33
|
||||
Class Matrix33Diag.
|
||||
This is the class for definition of a diagonal matrix 4x4.
|
||||
@param S (Templete Parameter) Specifies the ScalarType field.
|
||||
*/
|
||||
Matrix44Diag(const S & p0,const S & p1,const S & p2,const S & p3):Point4<S>(p0,p1,p2,p3){};
|
||||
Matrix44Diag( const Point4<S> & p ):Point4<S>(p){};
|
||||
};
|
||||
|
||||
_EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix,_Base);
|
||||
typedef _Scalar ScalarType;
|
||||
|
||||
/** This class represent a 4x4 matrix. T is the kind of element in the matrix.
|
||||
*/
|
||||
template <class T> class Matrix44 {
|
||||
protected:
|
||||
T _a[16];
|
||||
VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Matrix)
|
||||
|
||||
public:
|
||||
typedef T ScalarType;
|
||||
|
||||
///@{
|
||||
|
||||
/** $name Constructors
|
||||
/** \name Constructors
|
||||
* No automatic casting and default constructor is empty
|
||||
*/
|
||||
Matrix44() {};
|
||||
~Matrix44() {};
|
||||
Matrix44(const Matrix44 &m);
|
||||
Matrix44(const T v[]);
|
||||
Matrix44() : Base() {}
|
||||
~Matrix44() {}
|
||||
Matrix44(const Matrix44 &m) : Base(m) {}
|
||||
Matrix33(const Scalar * v ) : Base(Map<Matrix<Scalar,4,4,RowMajor>(v)) {}
|
||||
|
||||
/// Number of columns
|
||||
inline unsigned int ColumnsNumber() const
|
||||
{
|
||||
return 4;
|
||||
};
|
||||
|
||||
/// Number of rows
|
||||
inline unsigned int RowsNumber() const
|
||||
{
|
||||
return 4;
|
||||
};
|
||||
|
||||
T &ElementAt(const int row, const int col);
|
||||
T ElementAt(const int row, const int col) const;
|
||||
//T &operator[](const int i);
|
||||
//const T &operator[](const int i) const;
|
||||
T *V();
|
||||
const T *V() const ;
|
||||
|
||||
T *operator[](const int i);
|
||||
const T *operator[](const int i) const;
|
||||
Scalar *V() { return Base::data(); }
|
||||
const Scalar *V() const { return Base::data(); }
|
||||
|
||||
// return a copy of the i-th column
|
||||
Point4<T> GetColumn4(const int& i)const{
|
||||
assert(i>=0 && i<4);
|
||||
return Point4<T>(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i),ElementAt(3,i));
|
||||
//return Point4<T>(_a[i],_a[i+4],_a[i+8],_a[i+12]);
|
||||
}
|
||||
typename Base::ColXpr GetColumn4(const int& i) const { return col(i); }
|
||||
Block<Matrix,3,1> GetColumn3(const int& i) const { return block<3,1>(0,i); }
|
||||
|
||||
Point3<T> GetColumn3(const int& i)const{
|
||||
assert(i>=0 && i<4);
|
||||
return Point3<T>(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i));
|
||||
}
|
||||
|
||||
Point4<T> GetRow4(const int& i)const{
|
||||
assert(i>=0 && i<4);
|
||||
return Point4<T>(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2),ElementAt(i,3));
|
||||
// return *((Point4<T>*)(&_a[i<<2])); alternativa forse + efficiente
|
||||
}
|
||||
|
||||
Point3<T> GetRow3(const int& i)const{
|
||||
assert(i>=0 && i<4);
|
||||
return Point3<T>(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2));
|
||||
// return *((Point4<T>*)(&_a[i<<2])); alternativa forse + efficiente
|
||||
}
|
||||
|
||||
Matrix44 operator+(const Matrix44 &m) const;
|
||||
Matrix44 operator-(const Matrix44 &m) const;
|
||||
Matrix44 operator*(const Matrix44 &m) const;
|
||||
Matrix44 operator*(const Matrix44Diag<T> &m) const;
|
||||
Point4<T> operator*(const Point4<T> &v) const;
|
||||
|
||||
bool operator==(const Matrix44 &m) const;
|
||||
bool operator!= (const Matrix44 &m) const;
|
||||
|
||||
Matrix44 operator-() const;
|
||||
Matrix44 operator*(const T k) const;
|
||||
void operator+=(const Matrix44 &m);
|
||||
void operator-=(const Matrix44 &m);
|
||||
void operator*=( const Matrix44 & m );
|
||||
void operator*=( const T k );
|
||||
typename Base::RowXpr GetRow4(const int& i) const { return col(i); }
|
||||
Block<Matrix,1,3> GetRow3(const int& i) const { return block<1,3>(i,0); }
|
||||
|
||||
template <class Matrix44Type>
|
||||
void ToMatrix(Matrix44Type & m) const {for(int i = 0; i < 16; i++) m.V()[i]=V()[i];}
|
||||
void ToMatrix(Matrix44Type & m) const { m = (*this).cast<typename Matrix44Type::Scalar>(); }
|
||||
|
||||
void ToEulerAngles(T &alpha, T &beta, T &gamma);
|
||||
void ToEulerAngles(Scalar &alpha, Scalar &beta, Scalar &gamma);
|
||||
|
||||
template <class Matrix44Type>
|
||||
void FromMatrix(const Matrix44Type & m){for(int i = 0; i < 16; i++) V()[i]=m.V()[i];}
|
||||
void FromMatrix(const Matrix44Type & m) { for(int i = 0; i < 16; i++) data()[i] = m.data()[i]; }
|
||||
|
||||
void FromEulerAngles(T alpha, T beta, T gamma);
|
||||
void SetZero();
|
||||
void SetIdentity();
|
||||
void SetDiagonal(const T k);
|
||||
Matrix44 &SetScale(const T sx, const T sy, const T sz);
|
||||
Matrix44 &SetScale(const Point3<T> &t);
|
||||
|
|
@ -274,12 +147,6 @@ public:
|
|||
Matrix44<T> tmp; tmp.FromMatrix(b);
|
||||
return tmp;
|
||||
}
|
||||
|
||||
static inline const Matrix44 &Identity( )
|
||||
{
|
||||
static Matrix44<T> tmp; tmp.SetIdentity();
|
||||
return tmp;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
|
@ -289,7 +156,7 @@ public:
|
|||
template <class T> class LinearSolve: public Matrix44<T> {
|
||||
public:
|
||||
LinearSolve(const Matrix44<T> &m);
|
||||
Point4<T> Solve(const Point4<T> &b); // solve A · x = b
|
||||
Point4<T> Solve(const Point4<T> &b); // solve A <EFBFBD> x = b
|
||||
///If you need to solve some equation you can use this function instead of Matrix44 one for speed.
|
||||
T Determinant() const;
|
||||
protected:
|
||||
|
|
|
|||
|
|
@ -62,7 +62,11 @@ void RotationalPartByPolarDecomposition( const vcg::Matrix33<S> & m, vcg::Matrix
|
|||
e[0]=math::Sqrt(e[0]);
|
||||
e[1]=math::Sqrt(e[1]);
|
||||
e[2]=math::Sqrt(e[2]);
|
||||
#ifdef VCG_USE_EIGEN
|
||||
tmp = tmp*e.asDiagonal()*res;
|
||||
#else
|
||||
tmp = tmp*Matrix33Diag<S>(e)*res;
|
||||
#endif
|
||||
|
||||
bool s1 = SingularValueDecomposition<vcg::Matrix33<S> >(tmp,&e[0],res);
|
||||
tmp.Transpose();
|
||||
|
|
@ -70,7 +74,11 @@ void RotationalPartByPolarDecomposition( const vcg::Matrix33<S> & m, vcg::Matrix
|
|||
e[1]=1/e[1];
|
||||
e[2]=1/e[2];
|
||||
|
||||
#ifdef VCG_USE_EIGEN
|
||||
tmp = res*e.asDiagonal()*tmp;
|
||||
#else
|
||||
tmp = res*Matrix33Diag<S>(e)*tmp;
|
||||
#endif
|
||||
|
||||
r = m*tmp;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -311,8 +311,8 @@ public:
|
|||
/// sets a point to Zero
|
||||
inline void Zero()
|
||||
{
|
||||
_min.Zero();
|
||||
_max.Zero();
|
||||
_min.SetZero();
|
||||
_max.SetZero();
|
||||
}
|
||||
inline Box operator + ( Box const & p) const
|
||||
{
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ Point3<S> PlaneFittingPoints( std::vector< Point3<S> > & samples,Plane3<S> &p){
|
|||
Matrix44<S> m;m.SetZero();
|
||||
typename std::vector< Point3<S> > ::iterator i;
|
||||
|
||||
Point3<S> c; c.Zero();
|
||||
Point3<S> c; c.SetZero();
|
||||
for(i = samples.begin(); i != samples.end(); ++i)
|
||||
c+=*i;
|
||||
c/=samples.size();
|
||||
|
|
@ -147,7 +147,7 @@ bool PlaneFittingPointsOld( std::vector< Point3<S> > & samples, Plane3<S> & p )
|
|||
m[i][j]=P[i][j];
|
||||
|
||||
|
||||
// Point4<S> s;s.Zero();
|
||||
// Point4<S> s;s.SetZero();
|
||||
//
|
||||
// s.Normalize();
|
||||
// printf("\n RES %f %f %f %f \n",s[0],s[1],s[2],s[3]);
|
||||
|
|
|
|||
|
|
@ -1038,7 +1038,7 @@ namespace vcg
|
|||
{
|
||||
object = NULL;
|
||||
distance = ScalarType(-1.0);
|
||||
nearest_point.Zero();
|
||||
nearest_point.SetZero();
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -82,7 +82,7 @@ namespace vcg
|
|||
// Plane structure: identify a plain as a <center, normal> pair
|
||||
struct Plane
|
||||
{
|
||||
Plane() { center.Zero(); normal.Zero();};
|
||||
Plane() { center.SetZero(); normal.SetZero();};
|
||||
|
||||
// Object functor: return the bounding-box enclosing a given plane
|
||||
inline void GetBBox(BoundingBoxType &bb) { bb.Set(center); };
|
||||
|
|
|
|||
|
|
@ -974,7 +974,7 @@ static void SetSubView(vcg::Camera<S> & camera,vcg::Point2<S> p0,S nearDist, S f
|
|||
//
|
||||
// // prendi la matrice di proiezione
|
||||
// Matrix44d P;
|
||||
// P.Zero();
|
||||
// P.SetZero();
|
||||
//
|
||||
// if(!IsOrtho())// prospettica
|
||||
// {
|
||||
|
|
@ -1047,7 +1047,7 @@ static void SetSubView(vcg::Camera<S> & camera,vcg::Point2<S> p0,S nearDist, S f
|
|||
//
|
||||
// // prendi la matrice di proiezione
|
||||
// Matrix44d P;
|
||||
// P.Zero();
|
||||
// P.SetZero();
|
||||
//
|
||||
// if(!IsOrtho())// prospettica
|
||||
// {
|
||||
|
|
|
|||
Loading…
Reference in New Issue