HEAVY CHANGE. Further cleaning of the matrix classes of VCG.
Get rid of all the unused stuff. internally use Eigen for computing Inverse. Removed the stupid incomprehensible method Invert() that changed the matrix itself. Nobody was using it in a reasonable way.
This commit is contained in:
parent
f156a5a82c
commit
2e65cae10e
|
|
@ -20,75 +20,6 @@
|
|||
* 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
|
||||
|
||||
|
|
@ -99,24 +30,11 @@ created
|
|||
|
||||
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.
|
||||
@param S (Template Parameter) Specifies the ScalarType field.
|
||||
*/
|
||||
class Matrix33
|
||||
{
|
||||
|
|
@ -206,14 +124,6 @@ public:
|
|||
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 )
|
||||
|
|
@ -223,16 +133,7 @@ public:
|
|||
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
|
||||
/// Modificatore divisione per scalare
|
||||
Matrix33 & operator /= ( const S &s )
|
||||
{
|
||||
for(int i=0;i<9;++i)
|
||||
|
|
@ -265,30 +166,6 @@ public:
|
|||
for(i=0;i<9;++i) this->a[i] = r.a[i];
|
||||
}
|
||||
|
||||
/// 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 )
|
||||
{
|
||||
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];
|
||||
for(i=0;i<9;++i) this->a[i] = r.a[i];
|
||||
}
|
||||
|
||||
/// Modificatore prodotto per costante
|
||||
Matrix33 & operator *= ( const S t )
|
||||
{
|
||||
|
|
@ -316,17 +193,6 @@ public:
|
|||
|
||||
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
|
||||
{
|
||||
|
|
@ -336,17 +202,6 @@ public:
|
|||
|
||||
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}$
|
||||
|
|
@ -486,42 +341,6 @@ public:
|
|||
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
|
||||
{
|
||||
|
|
@ -663,27 +482,14 @@ vcg::Matrix33<S> TransformationMatrix(const vcg::Point3<S> dirX,
|
|||
/////then find the inverse
|
||||
return (Trans);
|
||||
}
|
||||
|
||||
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;
|
||||
Eigen::Matrix3d mm,mmi;
|
||||
m.ToEigenMatrix(mm);
|
||||
mmi=mm.inverse();
|
||||
Matrix33<S> res;
|
||||
res.FromEigenMatrix(mmi);
|
||||
}
|
||||
|
||||
///given 2 vector centered into origin calculate the rotation matrix from first to the second
|
||||
|
|
|
|||
|
|
@ -19,84 +19,6 @@
|
|||
* 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
|
||||
|
|
@ -108,12 +30,13 @@ Revision 1.12 2004/03/25 14:57:49 ponchio
|
|||
#include <vcg/space/point4.h>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
|
||||
#include <eigenlib/Eigen/Core>
|
||||
#include <eigenlib/Eigen/LU>
|
||||
|
||||
namespace vcg {
|
||||
|
||||
/*
|
||||
Annotations:
|
||||
Annotations:
|
||||
Opengl stores matrix in column-major order. That is, the matrix is stored as:
|
||||
|
||||
a0 a4 a8 a12
|
||||
|
|
@ -146,19 +69,6 @@ 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 {
|
||||
|
|
@ -173,23 +83,11 @@ public:
|
|||
/** $name Constructors
|
||||
* No automatic casting and default constructor is empty
|
||||
*/
|
||||
Matrix44() {};
|
||||
~Matrix44() {};
|
||||
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);
|
||||
|
|
@ -205,29 +103,28 @@ public:
|
|||
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));
|
||||
}
|
||||
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));
|
||||
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));
|
||||
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;
|
||||
|
|
@ -241,13 +138,20 @@ public:
|
|||
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 ToMatrix(Matrix44Type & m) const {for(int i = 0; i < 16; i++) m.V()[i]=V()[i];}
|
||||
|
||||
void ToEulerAngles(T &alpha, T &beta, T &gamma);
|
||||
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];}
|
||||
|
||||
template <class EigenMatrix44Type>
|
||||
void ToEigenMatrix(EigenMatrix44Type & m) const {
|
||||
for(int i = 0; i < 4; i++)
|
||||
for(int j = 0; j < 4; j++)
|
||||
m(i,j)=(*this)[i][j];
|
||||
}
|
||||
|
||||
template <class EigenMatrix44Type>
|
||||
void FromEigenMatrix(const EigenMatrix44Type & m){
|
||||
for(int i = 0; i < 4; i++)
|
||||
|
|
@ -259,12 +163,12 @@ public:
|
|||
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<T>& SetColumn(const unsigned int ii,const Point4<T> &t);
|
||||
Matrix44<T>& SetColumn(const unsigned int ii,const Point3<T> &t);
|
||||
Matrix44 &SetScale(const T sx, const T sy, const T sz);
|
||||
Matrix44 &SetScale(const Point3<T> &t);
|
||||
Matrix44<T>& SetColumn(const unsigned int ii,const Point4<T> &t);
|
||||
Matrix44<T>& SetColumn(const unsigned int ii,const Point3<T> &t);
|
||||
Matrix44 &SetTranslate(const Point3<T> &t);
|
||||
Matrix44 &SetTranslate(const T sx, const T sy, const T sz);
|
||||
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);
|
||||
|
|
@ -279,27 +183,27 @@ public:
|
|||
for(int i = 0; i < 16; i++)
|
||||
_a[i] = (T)(m.V()[i]);
|
||||
}
|
||||
template <class Q>
|
||||
template <class Q>
|
||||
static inline Matrix44 Construct( const Matrix44<Q> & b )
|
||||
{
|
||||
Matrix44<T> tmp; tmp.FromMatrix(b);
|
||||
Matrix44<T> tmp; tmp.FromMatrix(b);
|
||||
return tmp;
|
||||
}
|
||||
|
||||
static inline const Matrix44 &Identity( )
|
||||
{
|
||||
static Matrix44<T> tmp; tmp.SetIdentity();
|
||||
static Matrix44<T> tmp; tmp.SetIdentity();
|
||||
return tmp;
|
||||
}
|
||||
|
||||
// for the transistion to eigen
|
||||
// for the transistion to eigen
|
||||
Matrix44 transpose() const
|
||||
{
|
||||
Matrix44 res = *this;
|
||||
Transpose(res);
|
||||
return res;
|
||||
}
|
||||
void transposeInPlace() { Transpose(*this); }
|
||||
{
|
||||
Matrix44 res = *this;
|
||||
Transpose(res);
|
||||
return res;
|
||||
}
|
||||
void transposeInPlace() { Transpose(*this); }
|
||||
|
||||
void print() {
|
||||
unsigned int i, j, p;
|
||||
|
|
@ -340,7 +244,6 @@ 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;
|
||||
|
|
@ -418,14 +321,6 @@ template <class T> Matrix44<T> Matrix44<T>::operator*(const Matrix44 &m) const {
|
|||
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++){
|
||||
|
|
@ -439,15 +334,15 @@ template <class T> Point4<T> Matrix44<T>::operator*(const Point4<T> &v) const {
|
|||
|
||||
|
||||
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)
|
||||
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)
|
||||
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;
|
||||
|
|
@ -477,17 +372,6 @@ template <class T> void Matrix44<T>::operator-=(const Matrix44 &m) {
|
|||
}
|
||||
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 ) {
|
||||
|
|
@ -571,7 +455,7 @@ template <class T> Matrix44<T> &Matrix44<T>::SetTranslate(const Point3<T> &t) {
|
|||
}
|
||||
template <class T> Matrix44<T> &Matrix44<T>::SetTranslate(const T tx, const T ty, const T tz) {
|
||||
SetIdentity();
|
||||
ElementAt(0, 3) = tx;
|
||||
ElementAt(0, 3) = tx;
|
||||
ElementAt(1, 3) = ty;
|
||||
ElementAt(2, 3) = tz;
|
||||
return *this;
|
||||
|
|
@ -591,78 +475,40 @@ template <class T> Matrix44<T> &Matrix44<T>::SetColumn(const unsigned int ii,con
|
|||
ElementAt(1, ii) = t[1];
|
||||
ElementAt(2, ii) = t[2];
|
||||
ElementAt(3, ii) = t[3];
|
||||
return *this;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
template <class T> Matrix44<T> &Matrix44<T>::SetRotateDeg(T AngleDeg, const Point3<T> & axis) {
|
||||
return SetRotateRad(math::ToRad(AngleDeg),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;
|
||||
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
|
||||
|
|
@ -689,14 +535,14 @@ double srv() { return (double(rand()%40)-20)/2.0; } // small random value
|
|||
|
||||
Matrix44d Scl; Scl.SetScale(ScV);
|
||||
Matrix44d Sxy; Sxy.SetShearXY(ShV[0]);
|
||||
Matrix44d Sxz; Sxz.SetShearXZ(ShV[1]);
|
||||
Matrix44d Syz; Syz.SetShearYZ(ShV[2]);
|
||||
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 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 StartM = Trn * Rtx*Rty*Rtz * Syz*Sxz*Sxy *Scl;
|
||||
Matrix44d ResultM=StartM;
|
||||
Decompose(ResultM,ScVOut,ShVOut,RtVOut,TrVOut);
|
||||
|
||||
|
|
@ -710,7 +556,7 @@ double srv() { return (double(rand()%40)-20)/2.0; } // small random value
|
|||
Trn.SetTranslate(TrVOut);
|
||||
|
||||
// Now Rebuild is equal to StartM
|
||||
Matrix44d RebuildM = Trn * Rtx*Rty*Rtz * Syz*Sxz*Sxy * Scl ;
|
||||
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)
|
||||
|
|
@ -720,7 +566,7 @@ bool Decompose(Matrix44<T> &M, Point3<T> &ScaleV, Point3<T> &ShearV, Point3<T> &
|
|||
if(math::Abs(M.Determinant())<1e-10) return false; // matrix should be at least invertible...
|
||||
|
||||
// First Step recover the traslation
|
||||
TranV=M.GetColumn3(3);
|
||||
TranV=M.GetColumn3(3);
|
||||
|
||||
|
||||
// Second Step Recover Scale and Shearing interleaved
|
||||
|
|
@ -732,9 +578,9 @@ bool Decompose(Matrix44<T> &M, Point3<T> &ScaleV, Point3<T> &ShearV, Point3<T> &
|
|||
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];
|
||||
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];
|
||||
|
|
@ -753,50 +599,51 @@ bool Decompose(Matrix44<T> &M, Point3<T> &ScaleV, Point3<T> &ShearV, Point3<T> &
|
|||
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];
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
{
|
||||
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;
|
||||
}
|
||||
{
|
||||
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);
|
||||
RotV[1]=math::ToDeg(beta);
|
||||
RotV[2]=math::ToDeg(gamma);
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
template <class T> T Matrix44<T>::Determinant() const {
|
||||
LinearSolve<T> solve(*this);
|
||||
return solve.Determinant();
|
||||
Eigen::Matrix4d mm;
|
||||
this->ToEigenMatrix(mm);
|
||||
return mm.determinant();
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -807,225 +654,24 @@ template <class T> Point3<T> operator*(const Matrix44<T> &m, const Point3<T> &p)
|
|||
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;
|
||||
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));
|
||||
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);
|
||||
Eigen::Matrix4d mm,mmi;
|
||||
m.ToEigenMatrix(mm);
|
||||
mmi=mm.inverse();
|
||||
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;
|
||||
res.FromEigenMatrix(mmi);
|
||||
}
|
||||
|
||||
} //namespace
|
||||
|
|
|
|||
|
|
@ -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. *
|
||||
|
|
@ -98,7 +98,7 @@ y is upward!
|
|||
|
||||
namespace vcg {
|
||||
/**
|
||||
This class represent the viewing parameters under opengl.
|
||||
This class represent the viewing parameters under opengl.
|
||||
Mainly it stores the projection and modelview matrix and the viewport
|
||||
and it is used to simply project back and forth points, computing line of sight, planes etc.
|
||||
|
||||
|
|
@ -119,21 +119,21 @@ public:
|
|||
|
||||
/// Return the line of view passing through point P.
|
||||
Line3<T> ViewLineFromModel(const Point3<T> &p);
|
||||
|
||||
|
||||
/// Return the line passing through the point p and the observer.
|
||||
Line3<T> ViewLineFromWindow(const Point3<T> &p);
|
||||
|
||||
|
||||
/// Convert coordinates from the range -1..1 of Normalized Device Coords to range 0 viewport[2]
|
||||
Point3<T> NormDevCoordToWindowCoord(const Point3<T> &p) const;
|
||||
|
||||
|
||||
/// Convert coordinates from 0--viewport[2] to the range -1..1 of Normalized Device Coords to
|
||||
Point3<T> WindowCoordToNormDevCoord(const Point3<T> &p) const;
|
||||
|
||||
Matrix44<T> proj;
|
||||
Matrix44<T> model;
|
||||
Matrix44<T> matrix;
|
||||
Matrix44<T> inverse;
|
||||
int viewport[4];
|
||||
Matrix44<T> inverse;
|
||||
int viewport[4];
|
||||
};
|
||||
|
||||
template <class T> void View<T>::GetView() {
|
||||
|
|
@ -142,8 +142,7 @@ template <class T> void View<T>::GetView() {
|
|||
glGetIntegerv(GL_VIEWPORT, (GLint*)viewport);
|
||||
|
||||
matrix = proj*model;
|
||||
inverse = matrix;
|
||||
Invert(inverse);
|
||||
inverse = vcg::Inverse(matrix);
|
||||
}
|
||||
|
||||
template <class T> void View<T>::SetView(const float *_proj,
|
||||
|
|
@ -162,16 +161,13 @@ template <class T> void View<T>::SetView(const float *_proj,
|
|||
}
|
||||
|
||||
template <class T> Point3<T> View<T>::ViewPoint() const {
|
||||
Matrix44<T> mi=model;
|
||||
Invert(mi);
|
||||
return mi* Point3<T>(0, 0, 0);
|
||||
return vcg::Inverse(model)* Point3<T>(0, 0, 0);
|
||||
}
|
||||
// Note that p it is assumed to be in model coordinate.
|
||||
template <class T> Plane3<T> View<T>::ViewPlaneFromModel(const Point3<T> &p)
|
||||
{
|
||||
//compute normal, pointing away from view.
|
||||
Matrix44<T> imodel = model;
|
||||
Invert(imodel);
|
||||
Matrix44<T> imodel = vcg::Inverse(model);
|
||||
Point3<T> vp=ViewPoint();
|
||||
vcg::Point3f n = imodel * vcg::Point3<T>(0.0f, 0, -1.0f) - vp;
|
||||
|
||||
|
|
@ -214,14 +210,14 @@ template <class T> Point3<T> View<T>::UnProject(const Point3<T> &p) const {
|
|||
}
|
||||
|
||||
// Come spiegato nelle glspec
|
||||
// dopo la perspective division le coordinate sono dette normalized device coords ( NDC ).
|
||||
// dopo la perspective division le coordinate sono dette normalized device coords ( NDC ).
|
||||
// Per passare alle window coords si deve fare la viewport transformation.
|
||||
// Le coordinate di viewport stanno tra -1 e 1
|
||||
|
||||
template <class T> Point3<T> View<T>::NormDevCoordToWindowCoord(const Point3<T> &p) const {
|
||||
Point3<T> a;
|
||||
a[0] = (p[0]+1)*(viewport[2]/(T)2.0)+viewport[0];
|
||||
a[1] = (p[1]+1)*(viewport[3]/(T)2.0)+viewport[1];
|
||||
a[1] = (p[1]+1)*(viewport[3]/(T)2.0)+viewport[1];
|
||||
//a[1] = viewport[3] - a[1];
|
||||
a[2] = (p[2]+1)/2;
|
||||
return a;
|
||||
|
|
@ -231,9 +227,9 @@ template <class T> Point3<T> View<T>::NormDevCoordToWindowCoord(const Point3<T>
|
|||
template <class T> Point3<T> View<T>::WindowCoordToNormDevCoord(const Point3<T> &p) const {
|
||||
Point3<T> a;
|
||||
a[0] = (p[0]- viewport[0])/ (viewport[2]/(T)2.0) - 1;
|
||||
a[1] = (p[1]- viewport[1])/ (viewport[3]/(T)2.0) - 1;
|
||||
a[1] = (p[1]- viewport[1])/ (viewport[3]/(T)2.0) - 1;
|
||||
//a[1] = -a[1];
|
||||
a[2] = 2*p[2] - 1;
|
||||
a[2] = 2*p[2] - 1;
|
||||
return a;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue