diff --git a/vcg/math/matrix44.h b/vcg/math/matrix44.h index bf8cc87b..956f96f8 100644 --- a/vcg/math/matrix44.h +++ b/vcg/math/matrix44.h @@ -35,8 +35,8 @@ namespace vcg { - /* - Annotations: +/* + Annotations: Opengl stores matrix in column-major order. That is, the matrix is stored as: a0 a4 a8 a12 @@ -69,153 +69,153 @@ for 'column' vectors. */ - /** This class represent a 4x4 matrix. T is the kind of element in the matrix. - */ +/** This class represent a 4x4 matrix. T is the kind of element in the matrix. + */ template class Matrix44 { protected: - T _a[16]; + T _a[16]; public: - typedef T ScalarType; + typedef T ScalarType; -///@{ + ///@{ - /** $name Constructors - * No automatic casting and default constructor is empty - */ - Matrix44() {} - ~Matrix44() {} - Matrix44(const Matrix44 &m); - Matrix44(const T v[]); + /** $name Constructors + * No automatic casting and default constructor is empty + */ + Matrix44() {} + ~Matrix44() {} + Matrix44(const Matrix44 &m); + Matrix44(const T v[]); - 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 &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; + T *operator[](const int i); + const T *operator[](const int i) const; // return a copy of the i-th column Point4 GetColumn4(const int& i)const{ assert(i>=0 && i<4); return Point4(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i),ElementAt(3,i)); - //return Point4(_a[i],_a[i+4],_a[i+8],_a[i+12]); - } + //return Point4(_a[i],_a[i+4],_a[i+8],_a[i+12]); + } - Point3 GetColumn3(const int& i)const{ - assert(i>=0 && i<4); - return Point3(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i)); - } + Point3 GetColumn3(const int& i)const{ + assert(i>=0 && i<4); + return Point3(ElementAt(0,i),ElementAt(1,i),ElementAt(2,i)); + } - Point4 GetRow4(const int& i)const{ - assert(i>=0 && i<4); - return Point4(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2),ElementAt(i,3)); - // return *((Point4*)(&_a[i<<2])); alternativa forse + efficiente - } + Point4 GetRow4(const int& i)const{ + assert(i>=0 && i<4); + return Point4(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2),ElementAt(i,3)); + // return *((Point4*)(&_a[i<<2])); alternativa forse + efficiente + } - Point3 GetRow3(const int& i)const{ - assert(i>=0 && i<4); - return Point3(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2)); - // return *((Point4*)(&_a[i<<2])); alternativa forse + efficiente - } + Point3 GetRow3(const int& i)const{ + assert(i>=0 && i<4); + return Point3(ElementAt(i,0),ElementAt(i,1),ElementAt(i,2)); + // return *((Point4*)(&_a[i<<2])); alternativa forse + efficiente + } - Matrix44 operator+(const Matrix44 &m) const; - Matrix44 operator-(const Matrix44 &m) const; - Matrix44 operator*(const Matrix44 &m) const; - Point4 operator*(const Point4 &v) const; + Matrix44 operator+(const Matrix44 &m) const; + Matrix44 operator-(const Matrix44 &m) const; + Matrix44 operator*(const Matrix44 &m) const; + Point4 operator*(const Point4 &v) const; - bool operator==(const Matrix44 &m) const; - bool operator!= (const Matrix44 &m) 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 ); + Matrix44 operator-() const; + Matrix44 operator*(const T k) const; + void operator+=(const Matrix44 &m); + void operator-=(const Matrix44 &m); + void operator*=( const Matrix44 & m ); + void operator*=( const T k ); - template - void ToMatrix(Matrix44Type & m) const {for(int i = 0; i < 16; i++) m.V()[i]=V()[i];} + template + void ToMatrix(Matrix44Type & m) const {for(int i = 0; i < 16; i++) m.V()[i]=V()[i];} - void ToEulerAngles(T &alpha, T &beta, T &gamma); + void ToEulerAngles(T &alpha, T &beta, T &gamma); template void FromMatrix(const Matrix44Type & m){for(int i = 0; i < 16; i++) V()[i]=m.V()[i];} - template - 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 + 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 void FromEigenMatrix(const EigenMatrix44Type & m){ - for(int i = 0; i < 4; i++) - for(int j = 0; j < 4; j++) - ElementAt(i,j)=m(i,j); + for(int i = 0; i < 4; i++) + for(int j = 0; j < 4; j++) + ElementAt(i,j)=m(i,j); } void FromEulerAngles(T alpha, T beta, T gamma); void SetZero(); - void SetIdentity(); - void SetDiagonal(const T k); - Matrix44 &SetScale(const T sx, const T sy, const T sz); - Matrix44 &SetScale(const Point3 &t); - Matrix44& SetColumn(const unsigned int ii,const Point4 &t); - Matrix44& SetColumn(const unsigned int ii,const Point3 &t); - Matrix44 &SetTranslate(const Point3 &t); - Matrix44 &SetTranslate(const T sx, const T sy, const T sz); - Matrix44 &SetShearXY(const T sz); - Matrix44 &SetShearXZ(const T sy); - Matrix44 &SetShearYZ(const T sx); + void SetIdentity(); + void SetDiagonal(const T k); + Matrix44 &SetScale(const T sx, const T sy, const T sz); + Matrix44 &SetScale(const Point3 &t); + Matrix44& SetColumn(const unsigned int ii,const Point4 &t); + Matrix44& SetColumn(const unsigned int ii,const Point3 &t); + Matrix44 &SetTranslate(const Point3 &t); + Matrix44 &SetTranslate(const T sx, const T sy, const T sz); + Matrix44 &SetShearXY(const T sz); + Matrix44 &SetShearXZ(const T sy); + Matrix44 &SetShearYZ(const T sx); - ///use radiants for angle. - Matrix44 &SetRotateDeg(T AngleDeg, const Point3 & axis); - Matrix44 &SetRotateRad(T AngleRad, const Point3 & axis); + ///use radiants for angle. + Matrix44 &SetRotateDeg(T AngleDeg, const Point3 & axis); + Matrix44 &SetRotateRad(T AngleRad, const Point3 & axis); - T Determinant() const; + T Determinant() const; - template void Import(const Matrix44 &m) { - for(int i = 0; i < 16; i++) - _a[i] = (T)(m.V()[i]); - } - template - static inline Matrix44 Construct( const Matrix44 & b ) - { - Matrix44 tmp; tmp.FromMatrix(b); - return tmp; - } + template void Import(const Matrix44 &m) { + for(int i = 0; i < 16; i++) + _a[i] = (T)(m.V()[i]); + } + template + static inline Matrix44 Construct( const Matrix44 & b ) + { + Matrix44 tmp; tmp.FromMatrix(b); + return tmp; + } - static inline const Matrix44 &Identity( ) - { - static Matrix44 tmp; tmp.SetIdentity(); - return tmp; - } + static inline const Matrix44 &Identity( ) + { + static Matrix44 tmp; tmp.SetIdentity(); + return tmp; + } - // for the transistion to eigen - Matrix44 transpose() const - { - Matrix44 res = *this; - Transpose(res); - return res; - } - void transposeInPlace() { Transpose(*this); } + // for the transistion to eigen + Matrix44 transpose() const + { + Matrix44 res = *this; + Transpose(res); + return res; + } + void transposeInPlace() { Transpose(*this); } void print() { - unsigned int i, j, p; - for (i=0, p=0; i<4; i++, p+=4) - { - std::cout << "[\t"; - for (j=0; j<4; j++) - std::cout << _a[p+j] << "\t"; + unsigned int i, j, p; + for (i=0, p=0; i<4; i++, p+=4) + { + std::cout << "[\t"; + for (j=0; j<4; j++) + std::cout << _a[p+j] << "\t"; - std::cout << "]\n"; - } - std::cout << "\n"; + std::cout << "]\n"; + } + std::cout << "\n"; } }; @@ -224,16 +224,16 @@ public: /** Class for solving A * x = b. */ template class LinearSolve: public Matrix44 { public: - LinearSolve(const Matrix44 &m); - Point4 Solve(const Point4 &b); // solve A � x = b - ///If you need to solve some equation you can use this function instead of Matrix44 one for speed. - T Determinant() const; + LinearSolve(const Matrix44 &m); + Point4 Solve(const Point4 &b); // solve A � x = b + ///If you need to solve some equation you can use this function instead of Matrix44 one for speed. + T Determinant() const; protected: - ///Holds row permutation. - int index[4]; //hold permutation - ///Hold sign of row permutation (used for determinant sign) - T d; - bool Decompose(); + ///Holds row permutation. + int index[4]; //hold permutation + ///Hold sign of row permutation (used for determinant sign) + T d; + bool Decompose(); }; /*** Postmultiply */ @@ -254,135 +254,135 @@ typedef Matrix44 Matrix44d; template Matrix44::Matrix44(const Matrix44 &m) { - memcpy((T *)_a, (T *)m._a, 16 * sizeof(T)); + memcpy((T *)_a, (T *)m._a, 16 * sizeof(T)); } template Matrix44::Matrix44(const T v[]) { - memcpy((T *)_a, v, 16 * sizeof(T)); + memcpy((T *)_a, v, 16 * sizeof(T)); } template T &Matrix44::ElementAt(const int row, const int col) { - assert(row >= 0 && row < 4); - assert(col >= 0 && col < 4); - return _a[(row<<2) + col]; + assert(row >= 0 && row < 4); + assert(col >= 0 && col < 4); + return _a[(row<<2) + col]; } template T Matrix44::ElementAt(const int row, const int col) const { - assert(row >= 0 && row < 4); - assert(col >= 0 && col < 4); - return _a[(row<<2) + col]; + assert(row >= 0 && row < 4); + assert(col >= 0 && col < 4); + return _a[(row<<2) + col]; } //template T &Matrix44::operator[](const int i) { -// assert(i >= 0 && i < 16); -// return ((T *)_a)[i]; +// assert(i >= 0 && i < 16); +// return ((T *)_a)[i]; //} // //template const T &Matrix44::operator[](const int i) const { -// assert(i >= 0 && i < 16); -// return ((T *)_a)[i]; +// assert(i >= 0 && i < 16); +// return ((T *)_a)[i]; //} template T *Matrix44::operator[](const int i) { - assert(i >= 0 && i < 4); - return _a+i*4; + assert(i >= 0 && i < 4); + return _a+i*4; } template const T *Matrix44::operator[](const int i) const { - assert(i >= 0 && i < 4); - return _a+i*4; + assert(i >= 0 && i < 4); + return _a+i*4; } template T *Matrix44::V() { return _a;} template const T *Matrix44::V() const { return _a;} template Matrix44 Matrix44::operator+(const Matrix44 &m) const { - Matrix44 ret; - for(int i = 0; i < 16; i++) - ret.V()[i] = V()[i] + m.V()[i]; - return ret; + Matrix44 ret; + for(int i = 0; i < 16; i++) + ret.V()[i] = V()[i] + m.V()[i]; + return ret; } template Matrix44 Matrix44::operator-(const Matrix44 &m) const { - Matrix44 ret; - for(int i = 0; i < 16; i++) - ret.V()[i] = V()[i] - m.V()[i]; - return ret; + Matrix44 ret; + for(int i = 0; i < 16; i++) + ret.V()[i] = V()[i] - m.V()[i]; + return ret; } template Matrix44 Matrix44::operator*(const Matrix44 &m) const { - Matrix44 ret; - for(int i = 0; i < 4; i++) - for(int j = 0; j < 4; j++) { - T t = 0.0; - for(int k = 0; k < 4; k++) - t += ElementAt(i, k) * m.ElementAt(k, j); - ret.ElementAt(i, j) = t; - } - return ret; + 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 Point4 Matrix44::operator*(const Point4 &v) const { - Point4 ret; - for(int i = 0; i < 4; i++){ - T t = 0.0; - for(int k = 0; k < 4; k++) - t += ElementAt(i,k) * v[k]; - ret[i] = t; - } - return ret; + Point4 ret; + for(int i = 0; i < 4; i++){ + T t = 0.0; + for(int k = 0; k < 4; k++) + t += ElementAt(i,k) * v[k]; + ret[i] = t; + } + return ret; } template bool Matrix44::operator==(const Matrix44 &m) const { - for(int i = 0; i < 4; ++i) - for(int j = 0; j < 4; ++j) - if(ElementAt(i,j) != m.ElementAt(i,j)) - return false; - return true; + for(int i = 0; i < 4; ++i) + for(int j = 0; j < 4; ++j) + if(ElementAt(i,j) != m.ElementAt(i,j)) + return false; + return true; } template bool Matrix44::operator!=(const Matrix44 &m) const { - for(int i = 0; i < 4; ++i) - for(int j = 0; j < 4; ++j) - if(ElementAt(i,j) != m.ElementAt(i,j)) - return true; - return false; + for(int i = 0; i < 4; ++i) + for(int j = 0; j < 4; ++j) + if(ElementAt(i,j) != m.ElementAt(i,j)) + return true; + return false; } template Matrix44 Matrix44::operator-() const { - Matrix44 res; - for(int i = 0; i < 16; i++) - res.V()[i] = -V()[i]; - return res; + Matrix44 res; + for(int i = 0; i < 16; i++) + res.V()[i] = -V()[i]; + return res; } template Matrix44 Matrix44::operator*(const T k) const { - Matrix44 res; - for(int i = 0; i < 16; i++) - res.V()[i] =V()[i] * k; - return res; + Matrix44 res; + for(int i = 0; i < 16; i++) + res.V()[i] =V()[i] * k; + return res; } template void Matrix44::operator+=(const Matrix44 &m) { - for(int i = 0; i < 16; i++) - V()[i] += m.V()[i]; + for(int i = 0; i < 16; i++) + V()[i] += m.V()[i]; } template void Matrix44::operator-=(const Matrix44 &m) { - for(int i = 0; i < 16; i++) - V()[i] -= m.V()[i]; + for(int i = 0; i < 16; i++) + V()[i] -= m.V()[i]; } template void Matrix44::operator*=( const Matrix44 & m ) { - *this = *this *m; + *this = *this *m; } template < class PointType , class T > void operator*=( std::vector &vert, const Matrix44 & m ) { - typename std::vector::iterator ii; - for(ii=vert.begin();ii!=vert.end();++ii) - (*ii).P()=m * (*ii).P(); + typename std::vector::iterator ii; + for(ii=vert.begin();ii!=vert.end();++ii) + (*ii).P()=m * (*ii).P(); } template void Matrix44::operator*=( const T k ) { - for(int i = 0; i < 16; i++) - _a[i] *= k; + for(int i = 0; i < 16; i++) + _a[i] *= k; } template @@ -390,7 +390,7 @@ void Matrix44::ToEulerAngles(T &alpha, T &beta, T &gamma) { alpha = atan2(ElementAt(1,2), ElementAt(2,2)); beta = asin(-ElementAt(0,2)); - gamma = atan2(ElementAt(0,1), ElementAt(0,0)); + gamma = atan2(ElementAt(0,1), ElementAt(0,0)); } template @@ -421,48 +421,48 @@ void Matrix44::FromEulerAngles(T alpha, T beta, T gamma) } template void Matrix44::SetZero() { - memset((T *)_a, 0, 16 * sizeof(T)); + memset((T *)_a, 0, 16 * sizeof(T)); } template void Matrix44::SetIdentity() { - SetDiagonal(1); + SetDiagonal(1); } template void Matrix44::SetDiagonal(const T k) { - SetZero(); - ElementAt(0, 0) = k; - ElementAt(1, 1) = k; - ElementAt(2, 2) = k; - ElementAt(3, 3) = 1; + SetZero(); + ElementAt(0, 0) = k; + ElementAt(1, 1) = k; + ElementAt(2, 2) = k; + ElementAt(3, 3) = 1; } template Matrix44 &Matrix44::SetScale(const Point3 &t) { - SetScale(t[0], t[1], t[2]); - return *this; + SetScale(t[0], t[1], t[2]); + return *this; } template Matrix44 &Matrix44::SetScale(const T sx, const T sy, const T sz) { - SetZero(); - ElementAt(0, 0) = sx; - ElementAt(1, 1) = sy; - ElementAt(2, 2) = sz; - ElementAt(3, 3) = 1; - return *this; + SetZero(); + ElementAt(0, 0) = sx; + ElementAt(1, 1) = sy; + ElementAt(2, 2) = sz; + ElementAt(3, 3) = 1; + return *this; } template Matrix44 &Matrix44::SetTranslate(const Point3 &t) { - SetTranslate(t[0], t[1], t[2]); - return *this; + SetTranslate(t[0], t[1], t[2]); + return *this; } template Matrix44 &Matrix44::SetTranslate(const T tx, const T ty, const T tz) { - SetIdentity(); - ElementAt(0, 3) = tx; - ElementAt(1, 3) = ty; - ElementAt(2, 3) = tz; - return *this; + SetIdentity(); + ElementAt(0, 3) = tx; + ElementAt(1, 3) = ty; + ElementAt(2, 3) = tz; + return *this; } template Matrix44 &Matrix44::SetColumn(const unsigned int ii,const Point3 &t) { - assert((ii >= 0) && (ii < 4)); + assert( ii < 4 ); ElementAt(0, ii) = t.X(); ElementAt(1, ii) = t.Y(); ElementAt(2, ii) = t.Z(); @@ -470,53 +470,53 @@ template Matrix44 &Matrix44::SetColumn(const unsigned int ii,con } template Matrix44 &Matrix44::SetColumn(const unsigned int ii,const Point4 &t) { - assert((ii < 4)); - ElementAt(0, ii) = t[0]; - ElementAt(1, ii) = t[1]; - ElementAt(2, ii) = t[2]; - ElementAt(3, ii) = t[3]; - return *this; + assert( ii < 4 ); + ElementAt(0, ii) = t[0]; + ElementAt(1, ii) = t[1]; + ElementAt(2, ii) = t[2]; + ElementAt(3, ii) = t[3]; + return *this; } template Matrix44 &Matrix44::SetRotateDeg(T AngleDeg, const Point3 & axis) { - return SetRotateRad(math::ToRad(AngleDeg),axis); + return SetRotateRad(math::ToRad(AngleDeg),axis); } template Matrix44 &Matrix44::SetRotateRad(T AngleRad, const Point3 & axis) { - //angle = angle*(T)3.14159265358979323846/180; e' in radianti! - T c = math::Cos(AngleRad); - T s = math::Sin(AngleRad); - T q = 1-c; - Point3 t = axis; - t.Normalize(); - ElementAt(0,0) = t[0]*t[0]*q + c; - ElementAt(0,1) = t[0]*t[1]*q - t[2]*s; - ElementAt(0,2) = t[0]*t[2]*q + t[1]*s; - ElementAt(0,3) = 0; - ElementAt(1,0) = t[1]*t[0]*q + t[2]*s; - ElementAt(1,1) = t[1]*t[1]*q + c; - ElementAt(1,2) = t[1]*t[2]*q - t[0]*s; - ElementAt(1,3) = 0; - ElementAt(2,0) = t[2]*t[0]*q -t[1]*s; - ElementAt(2,1) = t[2]*t[1]*q +t[0]*s; - ElementAt(2,2) = t[2]*t[2]*q +c; - ElementAt(2,3) = 0; - ElementAt(3,0) = 0; - ElementAt(3,1) = 0; - ElementAt(3,2) = 0; - ElementAt(3,3) = 1; - return *this; + //angle = angle*(T)3.14159265358979323846/180; e' in radianti! + T c = math::Cos(AngleRad); + T s = math::Sin(AngleRad); + T q = 1-c; + Point3 t = axis; + t.Normalize(); + ElementAt(0,0) = t[0]*t[0]*q + c; + ElementAt(0,1) = t[0]*t[1]*q - t[2]*s; + ElementAt(0,2) = t[0]*t[2]*q + t[1]*s; + ElementAt(0,3) = 0; + ElementAt(1,0) = t[1]*t[0]*q + t[2]*s; + ElementAt(1,1) = t[1]*t[1]*q + c; + ElementAt(1,2) = t[1]*t[2]*q - t[0]*s; + ElementAt(1,3) = 0; + ElementAt(2,0) = t[2]*t[0]*q -t[1]*s; + ElementAt(2,1) = t[2]*t[1]*q +t[0]*s; + ElementAt(2,2) = t[2]*t[2]*q +c; + ElementAt(2,3) = 0; + ElementAt(3,0) = 0; + ElementAt(3,1) = 0; + ElementAt(3,2) = 0; + ElementAt(3,3) = 1; + return *this; } /* 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 +- Scale,Shear,Rotation e Translation - ScaleV and Tranv are obiviously scaling and translation. -- ShearV contains three scalars with, respectively - ShearXY, ShearXZ e ShearYZ +- ShearV contains three scalars with, respectively, + ShearXY, ShearXZ and 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. @@ -535,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); @@ -556,8 +556,9 @@ 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 bool Decompose(Matrix44 &M, Point3 &ScaleV, Point3 &ShearV, Point3 &RotV,Point3 &TranV) { @@ -565,9 +566,8 @@ bool Decompose(Matrix44 &M, Point3 &ScaleV, Point3 &ShearV, Point3 & 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); - + // First Step recover the traslation + TranV=M.GetColumn3(3); // Second Step Recover Scale and Shearing interleaved ScaleV[0]=Norm(M.GetColumn3(0)); @@ -577,10 +577,10 @@ bool Decompose(Matrix44 &M, Point3 &ScaleV, Point3 &ShearV, Point3 & 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]; + 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]; @@ -598,81 +598,81 @@ bool Decompose(Matrix44 &M, Point3 &ScaleV, Point3 &ShearV, Point3 & 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]; + 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 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; - } + 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); + RotV[0]=math::ToDeg(alpha); + RotV[1]=math::ToDeg(beta); + RotV[2]=math::ToDeg(gamma); - return true; + return true; } template T Matrix44::Determinant() const { - Eigen::Matrix4d mm; - this->ToEigenMatrix(mm); - return mm.determinant(); + Eigen::Matrix4d mm; + this->ToEigenMatrix(mm); + return mm.determinant(); } template Point3 operator*(const Matrix44 &m, const Point3 &p) { - T w; - Point3 s; - s[0] = m.ElementAt(0, 0)*p[0] + m.ElementAt(0, 1)*p[1] + m.ElementAt(0, 2)*p[2] + m.ElementAt(0, 3); - s[1] = m.ElementAt(1, 0)*p[0] + m.ElementAt(1, 1)*p[1] + m.ElementAt(1, 2)*p[2] + m.ElementAt(1, 3); - s[2] = m.ElementAt(2, 0)*p[0] + m.ElementAt(2, 1)*p[1] + m.ElementAt(2, 2)*p[2] + m.ElementAt(2, 3); - w = m.ElementAt(3, 0)*p[0] + m.ElementAt(3, 1)*p[1] + m.ElementAt(3, 2)*p[2] + m.ElementAt(3, 3); - if(w!= 0) s /= w; - return s; + T w; + Point3 s; + s[0] = m.ElementAt(0, 0)*p[0] + m.ElementAt(0, 1)*p[1] + m.ElementAt(0, 2)*p[2] + m.ElementAt(0, 3); + s[1] = m.ElementAt(1, 0)*p[0] + m.ElementAt(1, 1)*p[1] + m.ElementAt(1, 2)*p[2] + m.ElementAt(1, 3); + s[2] = m.ElementAt(2, 0)*p[0] + m.ElementAt(2, 1)*p[1] + m.ElementAt(2, 2)*p[2] + m.ElementAt(2, 3); + w = m.ElementAt(3, 0)*p[0] + m.ElementAt(3, 1)*p[1] + m.ElementAt(3, 2)*p[2] + m.ElementAt(3, 3); + if(w!= 0) s /= w; + return s; } template Matrix44 &Transpose(Matrix44 &m) { - for(int i = 1; i < 4; i++) - for(int j = 0; j < i; j++) { - math::Swap(m.ElementAt(i, j), m.ElementAt(j, i)); - } - return m; + 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; } template Matrix44 Inverse(const Matrix44 &m) { - Eigen::Matrix4d mm,mmi; - m.ToEigenMatrix(mm); - mmi=mm.inverse(); - Matrix44 res; - res.FromEigenMatrix(mmi); - return res; + Eigen::Matrix4d mm,mmi; + m.ToEigenMatrix(mm); + mmi=mm.inverse(); + Matrix44 res; + res.FromEigenMatrix(mmi); + return res; } } //namespace