1 /**************************************************************************** 2 * VCGLib o o * 3 * Visual and Computer Graphics Library o o * 4 * _ O _ * 5 * Copyright(C) 2004-2016 \/)\/ * 6 * Visual Computing Lab /\/| * 7 * ISTI - Italian National Research Council | * 8 * \ * 9 * All rights reserved. * 10 * * 11 * This program is free software; you can redistribute it and/or modify * 12 * it under the terms of the GNU General Public License as published by * 13 * the Free Software Foundation; either version 2 of the License, or * 14 * (at your option) any later version. * 15 * * 16 * This program is distributed in the hope that it will be useful, * 17 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 19 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * 20 * for more details. * 21 * * 22 ****************************************************************************/ 23 24 #ifndef VCG_USE_EIGEN 25 #include "deprecated_matrix44.h" 26 #else 27 28 #ifndef __VCGLIB_MATRIX44 29 #define __VCGLIB_MATRIX44 30 31 #include "eigen.h" 32 #include <vcg/space/point3.h> 33 #include <vcg/space/point4.h> 34 #include <memory.h> 35 #include <vector> 36 37 namespace vcg{ 38 template<class Scalar> class Matrix44; 39 } 40 41 namespace Eigen{ 42 template<typename Scalar> 43 struct ei_traits<vcg::Matrix44<Scalar> > : ei_traits<Eigen::Matrix<Scalar,4,4,RowMajor> > {}; 44 template<typename XprType> struct ei_to_vcgtype<XprType,4,4,RowMajor,4,4> 45 { typedef vcg::Matrix44<typename XprType::Scalar> type; }; 46 } 47 48 namespace vcg { 49 50 /* 51 Annotations: 52 Opengl stores matrix in column-major order. That is, the matrix is stored as: 53 54 a0 a4 a8 a12 55 a1 a5 a9 a13 56 a2 a6 a10 a14 57 a3 a7 a11 a15 58 59 Usually in opengl (see opengl specs) vectors are 'column' vectors 60 so usually matrix are PRE-multiplied for a vector. 61 So the command glTranslate generate a matrix that 62 is ready to be premultipled for a vector: 63 64 1 0 0 tx 65 0 1 0 ty 66 0 0 1 tz 67 0 0 0 1 68 69 Matrix44 stores matrix in row-major order i.e. 70 71 a0 a1 a2 a3 72 a4 a5 a6 a7 73 a8 a9 a10 a11 74 a12 a13 a14 a15 75 76 So for the use of that matrix in opengl with their supposed meaning you have to transpose them before feeding to glMultMatrix. 77 This mechanism is hidden by the templated function defined in wrap/gl/math.h; 78 If your machine has the ARB_transpose_matrix extension it will use the appropriate; 79 The various gl-like command SetRotate, SetTranslate assume that you are making matrix 80 for 'column' vectors. 81 82 */ 83 84 // Note that we have to pass Dim and HDim because it is not allowed to use a template 85 // parameter to define a template specialization. To be more precise, in the following 86 // specializations, it is not allowed to use Dim+1 instead of HDim. 87 template< typename Other, 88 int OtherRows=Eigen::ei_traits<Other>::RowsAtCompileTime, 89 int OtherCols=Eigen::ei_traits<Other>::ColsAtCompileTime> 90 struct ei_matrix44_product_impl; 91 92 /** \deprecated use Eigen::Matrix<Scalar,4,4> (or the typedef) you want a real 4x4 matrix, or use Eigen::Transform<Scalar,3> if you want a transformation matrix for a 3D space (a Eigen::Transform<Scalar,3> is internally a 4x4 col-major matrix) 93 * 94 * This class represents a 4x4 matrix. T is the kind of element in the matrix. 95 */ 96 template<typename _Scalar> 97 class Matrix44 : public Eigen::Matrix<_Scalar,4,4,Eigen::RowMajor> // FIXME col or row major ! 98 { 99 100 typedef Eigen::Matrix<_Scalar,4,4,Eigen::RowMajor> _Base; 101 public: 102 103 using _Base::coeff; 104 using _Base::coeffRef; 105 using _Base::ElementAt; 106 using _Base::setZero; 107 108 _EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix44,_Base); 109 typedef _Scalar ScalarType; 110 VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Matrix44) 111 112 Matrix44() : Base() {} 113 ~Matrix44() {} 114 Matrix44(const Matrix44 &m) : Base(m) {} 115 Matrix44(const Scalar * v ) : Base(Eigen::Map<Eigen::Matrix<Scalar,4,4,Eigen::RowMajor> >(v)) {} 116 template<typename OtherDerived> 117 Matrix44(const Eigen::MatrixBase<OtherDerived>& other) : Base(other) {} 118 119 const typename Base::RowXpr operator[](int i) const { return Base::row(i); } 120 typename Base::RowXpr operator[](int i) { return Base::row(i); } 121 122 typename Base::ColXpr GetColumn4(const int& i) const { return Base::col(i); } 123 const Eigen::Block<Base,3,1> GetColumn3(const int& i) const { return this->template block<3,1>(0,i); } 124 125 typename Base::RowXpr GetRow4(const int& i) const { return Base::row(i); } 126 Eigen::Block<Base,1,3> GetRow3(const int& i) const { return this->template block<1,3>(i,0); } 127 128 template <class Matrix44Type> 129 void ToMatrix(Matrix44Type & m) const { m = (*this).template cast<typename Matrix44Type::Scalar>(); } 130 131 void ToEulerAngles(Scalar &alpha, Scalar &beta, Scalar &gamma); 132 133 template <class Matrix44Type> 134 void FromMatrix(const Matrix44Type & m) { for(int i = 0; i < 16; i++) Base::data()[i] = m.data()[i]; } 135 136 void FromEulerAngles(Scalar alpha, Scalar beta, Scalar gamma); 137 void SetDiagonal(const Scalar k); 138 Matrix44 &SetScale(const Scalar sx, const Scalar sy, const Scalar sz); 139 Matrix44 &SetScale(const Point3<Scalar> &t); 140 Matrix44 &SetTranslate(const Point3<Scalar> &t); 141 Matrix44 &SetTranslate(const Scalar sx, const Scalar sy, const Scalar sz); 142 Matrix44 &SetShearXY(const Scalar sz); 143 Matrix44 &SetShearXZ(const Scalar sy); 144 Matrix44 &SetShearYZ(const Scalar sx); 145 146 ///use radiants for angle. 147 Matrix44 &SetRotateDeg(Scalar AngleDeg, const Point3<Scalar> & axis); 148 Matrix44 &SetRotateRad(Scalar AngleRad, const Point3<Scalar> & axis); 149 150 /** taken from Eigen::Transform 151 * \returns the product between the transform \c *this and a matrix expression \a other 152 * 153 * The right hand side \a other might be either: 154 * \li a matrix expression with 4 rows 155 * \li a 3D vector/point 156 */ 157 template<typename OtherDerived> 158 inline const typename ei_matrix44_product_impl<OtherDerived>::ResultType 159 operator * (const Eigen::MatrixBase<OtherDerived> &other) const 160 { return ei_matrix44_product_impl<OtherDerived>::run(*this,other.derived()); } 161 162 void print() {std::cout << *this << "\n\n";} 163 164 }; 165 166 //return NULL matrix if not invertible 167 template <class T> Matrix44<T> &Invert(Matrix44<T> &m); 168 template <class T> Matrix44<T> Inverse(const Matrix44<T> &m); 169 170 typedef Matrix44<short> Matrix44s; 171 typedef Matrix44<int> Matrix44i; 172 typedef Matrix44<float> Matrix44f; 173 typedef Matrix44<double> Matrix44d; 174 175 template < class PointType , class T > void operator*=( std::vector<PointType> &vert, const Matrix44<T> & m ) { 176 typename std::vector<PointType>::iterator ii; 177 for(ii=vert.begin();ii!=vert.end();++ii) 178 (*ii).P()=m * (*ii).P(); 179 } 180 181 template <class T> 182 void Matrix44<T>::ToEulerAngles(Scalar &alpha, Scalar &beta, Scalar &gamma) 183 { 184 alpha = atan2(coeff(1,2), coeff(2,2)); 185 beta = asin(-coeff(0,2)); 186 gamma = atan2(coeff(0,1), coeff(1,1)); 187 } 188 189 template <class T> 190 void Matrix44<T>::FromEulerAngles(Scalar alpha, Scalar beta, Scalar gamma) 191 { 192 this->SetZero(); 193 194 T cosalpha = cos(alpha); 195 T cosbeta = cos(beta); 196 T cosgamma = cos(gamma); 197 T sinalpha = sin(alpha); 198 T sinbeta = sin(beta); 199 T singamma = sin(gamma); 200 201 ElementAt(0,0) = cosbeta * cosgamma; 202 ElementAt(1,0) = -cosalpha * singamma + sinalpha * sinbeta * cosgamma; 203 ElementAt(2,0) = sinalpha * singamma + cosalpha * sinbeta * cosgamma; 204 205 ElementAt(0,1) = cosbeta * singamma; 206 ElementAt(1,1) = cosalpha * cosgamma + sinalpha * sinbeta * singamma; 207 ElementAt(2,1) = -sinalpha * cosgamma + cosalpha * sinbeta * singamma; 208 209 ElementAt(0,2) = -sinbeta; 210 ElementAt(1,2) = sinalpha * cosbeta; 211 ElementAt(2,2) = cosalpha * cosbeta; 212 213 ElementAt(3,3) = 1; 214 } 215 216 template <class T> void Matrix44<T>::SetDiagonal(const Scalar k) { 217 setZero(); 218 ElementAt(0, 0) = k; 219 ElementAt(1, 1) = k; 220 ElementAt(2, 2) = k; 221 ElementAt(3, 3) = 1; 222 } 223 224 template <class T> Matrix44<T> &Matrix44<T>::SetScale(const Point3<Scalar> &t) { 225 SetScale(t[0], t[1], t[2]); 226 return *this; 227 } 228 template <class T> Matrix44<T> &Matrix44<T>::SetScale(const Scalar sx, const Scalar sy, const Scalar sz) { 229 setZero(); 230 ElementAt(0, 0) = sx; 231 ElementAt(1, 1) = sy; 232 ElementAt(2, 2) = sz; 233 ElementAt(3, 3) = 1; 234 return *this; 235 } 236 237 template <class T> Matrix44<T> &Matrix44<T>::SetTranslate(const Point3<Scalar> &t) { 238 SetTranslate(t[0], t[1], t[2]); 239 return *this; 240 } 241 template <class T> Matrix44<T> &Matrix44<T>::SetTranslate(const Scalar tx, const Scalar ty, const Scalar tz) { 242 Base::setIdentity(); 243 ElementAt(0, 3) = tx; 244 ElementAt(1, 3) = ty; 245 ElementAt(2, 3) = tz; 246 return *this; 247 } 248 249 template <class T> Matrix44<T> &Matrix44<T>::SetRotateDeg(Scalar AngleDeg, const Point3<Scalar> & axis) { 250 return SetRotateRad(math::ToRad(AngleDeg),axis); 251 } 252 253 template <class T> Matrix44<T> &Matrix44<T>::SetRotateRad(Scalar AngleRad, const Point3<Scalar> & axis) { 254 //angle = angle*(T)3.14159265358979323846/180; e' in radianti! 255 T c = math::Cos(AngleRad); 256 T s = math::Sin(AngleRad); 257 T q = 1-c; 258 Point3<T> t = axis; 259 t.Normalize(); 260 ElementAt(0,0) = t[0]*t[0]*q + c; 261 ElementAt(0,1) = t[0]*t[1]*q - t[2]*s; 262 ElementAt(0,2) = t[0]*t[2]*q + t[1]*s; 263 ElementAt(0,3) = 0; 264 ElementAt(1,0) = t[1]*t[0]*q + t[2]*s; 265 ElementAt(1,1) = t[1]*t[1]*q + c; 266 ElementAt(1,2) = t[1]*t[2]*q - t[0]*s; 267 ElementAt(1,3) = 0; 268 ElementAt(2,0) = t[2]*t[0]*q -t[1]*s; 269 ElementAt(2,1) = t[2]*t[1]*q +t[0]*s; 270 ElementAt(2,2) = t[2]*t[2]*q +c; 271 ElementAt(2,3) = 0; 272 ElementAt(3,0) = 0; 273 ElementAt(3,1) = 0; 274 ElementAt(3,2) = 0; 275 ElementAt(3,3) = 1; 276 return *this; 277 } 278 279 /* Shear Matrixes 280 XY 281 1 k 0 0 x x+ky 282 0 1 0 0 y y 283 0 0 1 0 z z 284 0 0 0 1 1 1 285 286 1 0 k 0 x x+kz 287 0 1 0 0 y y 288 0 0 1 0 z z 289 0 0 0 1 1 1 290 291 1 1 0 0 x x 292 0 1 k 0 y y+kz 293 0 0 1 0 z z 294 0 0 0 1 1 1 295 296 */ 297 298 template <class T> Matrix44<T> & Matrix44<T>::SetShearXY( const Scalar sh) {// shear the X coordinate as the Y coordinate change 299 Base::setIdentity(); 300 ElementAt(0,1) = sh; 301 return *this; 302 } 303 304 template <class T> Matrix44<T> & Matrix44<T>::SetShearXZ( const Scalar sh) {// shear the X coordinate as the Z coordinate change 305 Base::setIdentity(); 306 ElementAt(0,2) = sh; 307 return *this; 308 } 309 310 template <class T> Matrix44<T> &Matrix44<T>::SetShearYZ( const Scalar sh) {// shear the Y coordinate as the Z coordinate change 311 Base::setIdentity(); 312 ElementAt(1,2) = sh; 313 return *this; 314 } 315 316 317 /* 318 Given a non singular, non projective matrix (e.g. with the last row equal to [0,0,0,1] ) 319 This procedure decompose it in a sequence of 320 Scale,Shear,Rotation e Translation 321 322 - ScaleV and Tranv are obiviously scaling and translation. 323 - ShearV contains three scalars with, respectively 324 ShearXY, ShearXZ e ShearYZ 325 - RotateV contains the rotations (in degree!) around the x,y,z axis 326 The input matrix is modified leaving inside it a simple roto translation. 327 328 To obtain the original matrix the above transformation have to be applied in the strict following way: 329 330 OriginalMatrix = Trn * Rtx*Rty*Rtz * ShearYZ*ShearXZ*ShearXY * Scl 331 332 Example Code: 333 double srv() { return (double(rand()%40)-20)/2.0; } // small random value 334 335 srand(time(0)); 336 Point3d ScV(10+srv(),10+srv(),10+srv()),ScVOut(-1,-1,-1); 337 Point3d ShV(srv(),srv(),srv()),ShVOut(-1,-1,-1); 338 Point3d RtV(10+srv(),srv(),srv()),RtVOut(-1,-1,-1); 339 Point3d TrV(srv(),srv(),srv()),TrVOut(-1,-1,-1); 340 341 Matrix44d Scl; Scl.SetScale(ScV); 342 Matrix44d Sxy; Sxy.SetShearXY(ShV[0]); 343 Matrix44d Sxz; Sxz.SetShearXZ(ShV[1]); 344 Matrix44d Syz; Syz.SetShearYZ(ShV[2]); 345 Matrix44d Rtx; Rtx.SetRotate(math::ToRad(RtV[0]),Point3d(1,0,0)); 346 Matrix44d Rty; Rty.SetRotate(math::ToRad(RtV[1]),Point3d(0,1,0)); 347 Matrix44d Rtz; Rtz.SetRotate(math::ToRad(RtV[2]),Point3d(0,0,1)); 348 Matrix44d Trn; Trn.SetTranslate(TrV); 349 350 Matrix44d StartM = Trn * Rtx*Rty*Rtz * Syz*Sxz*Sxy *Scl; 351 Matrix44d ResultM=StartM; 352 Decompose(ResultM,ScVOut,ShVOut,RtVOut,TrVOut); 353 354 Scl.SetScale(ScVOut); 355 Sxy.SetShearXY(ShVOut[0]); 356 Sxz.SetShearXZ(ShVOut[1]); 357 Syz.SetShearYZ(ShVOut[2]); 358 Rtx.SetRotate(math::ToRad(RtVOut[0]),Point3d(1,0,0)); 359 Rty.SetRotate(math::ToRad(RtVOut[1]),Point3d(0,1,0)); 360 Rtz.SetRotate(math::ToRad(RtVOut[2]),Point3d(0,0,1)); 361 Trn.SetTranslate(TrVOut); 362 363 // Now Rebuild is equal to StartM 364 Matrix44d RebuildM = Trn * Rtx*Rty*Rtz * Syz*Sxz*Sxy * Scl ; 365 */ 366 template <class T> 367 bool Decompose(Matrix44<T> &M, Point3<T> &ScaleV, Point3<T> &ShearV, Point3<T> &RotV,Point3<T> &TranV) 368 { 369 if(!(M(3,0)==0 && M(3,1)==0 && M(3,2)==0 && M(3,3)==1) ) // the matrix is projective 370 return false; 371 if(math::Abs(M.Determinant())<1e-10) return false; // matrix should be at least invertible... 372 373 // First Step recover the traslation 374 TranV=M.GetColumn3(3); 375 376 // Second Step Recover Scale and Shearing interleaved 377 ScaleV[0]=Norm(M.GetColumn3(0)); 378 Point3<T> R[3]; 379 R[0]=M.GetColumn3(0); 380 R[0].Normalize(); 381 382 ShearV[0]=R[0].dot(M.GetColumn3(1)); // xy shearing 383 R[1]= M.GetColumn3(1)-R[0]*ShearV[0]; 384 assert(math::Abs(R[1].dot(R[0]))<1e-10); 385 ScaleV[1]=Norm(R[1]); // y scaling 386 R[1]=R[1]/ScaleV[1]; 387 ShearV[0]=ShearV[0]/ScaleV[1]; 388 389 ShearV[1]=R[0].dot(M.GetColumn3(2)); // xz shearing 390 R[2]= M.GetColumn3(2)-R[0]*ShearV[1]; 391 assert(math::Abs(R[2].dot(R[0]))<1e-10); 392 393 R[2] = R[2]-R[1]*(R[2].dot(R[1])); 394 assert(math::Abs(R[2].dot(R[1]))<1e-10); 395 assert(math::Abs(R[2].dot(R[0]))<1e-10); 396 397 ScaleV[2]=Norm(R[2]); 398 ShearV[1]=ShearV[1]/ScaleV[2]; 399 R[2]=R[2]/ScaleV[2]; 400 assert(math::Abs(R[2].dot(R[1]))<1e-10); 401 assert(math::Abs(R[2].dot(R[0]))<1e-10); 402 403 ShearV[2]=R[1].dot(M.GetColumn3(2)); // yz shearing 404 ShearV[2]=ShearV[2]/ScaleV[2]; 405 int i,j; 406 for(i=0;i<3;++i) 407 for(j=0;j<3;++j) 408 M(i,j)=R[j][i]; 409 410 // Third and last step: Recover the rotation 411 //now the matrix should be a pure rotation matrix so its determinant is +-1 412 double det=M.Determinant(); 413 if(math::Abs(det)<1e-10) return false; // matrix should be at least invertible... 414 assert(math::Abs(math::Abs(det)-1.0)<1e-10); // it should be +-1... 415 if(det<0) { 416 ScaleV *= -1; 417 M *= -1; 418 } 419 420 double alpha,beta,gamma; // rotations around the x,y and z axis 421 beta=asin( M(0,2)); 422 double cosbeta=cos(beta); 423 if(math::Abs(cosbeta) > 1e-5) 424 { 425 alpha=asin(-M(1,2)/cosbeta); 426 if((M(2,2)/cosbeta) < 0 ) alpha=M_PI-alpha; 427 gamma=asin(-M(0,1)/cosbeta); 428 if((M(0,0)/cosbeta)<0) gamma = M_PI-gamma; 429 } 430 else 431 { 432 alpha=asin(-M(1,0)); 433 if(M(1,1)<0) alpha=M_PI-alpha; 434 gamma=0; 435 } 436 437 RotV[0]=math::ToDeg(alpha); 438 RotV[1]=math::ToDeg(beta); 439 RotV[2]=math::ToDeg(gamma); 440 441 return true; 442 } 443 444 /* 445 To invert a matrix you can 446 either invert the matrix inplace calling 447 448 vcg::Invert(yourMatrix); 449 450 or get the inverse matrix of a given matrix without touching it: 451 452 invertedMatrix = vcg::Inverse(untouchedMatrix); 453 454 */ 455 template <class T> Matrix44<T> & Invert(Matrix44<T> &m) { 456 return m = m.lu().inverse(); 457 } 458 459 template <class T> Matrix44<T> Inverse(const Matrix44<T> &m) { 460 return m.lu().inverse(); 461 } 462 463 template<typename Other,int OtherCols> 464 struct ei_matrix44_product_impl<Other, 4,OtherCols> 465 { 466 typedef typename Other::Scalar Scalar; 467 typedef typename Eigen::ProductReturnType<typename Matrix44<Scalar>::Base,Other>::Type ResultType; 468 static ResultType run(const Matrix44<Scalar>& tr, const Other& other) 469 { return (static_cast<const typename Matrix44<Scalar>::Base&>(tr)) * other; } 470 }; 471 472 template<typename Other> 473 struct ei_matrix44_product_impl<Other, 3,1> 474 { 475 typedef typename Other::Scalar Scalar; 476 typedef Eigen::Matrix<Scalar,3,1> ResultType; 477 static ResultType run(const Matrix44<Scalar>& tr, const Other& p) 478 { 479 Scalar w; 480 Eigen::Matrix<Scalar,3,1> s; 481 s[0] = tr.ElementAt(0, 0)*p[0] + tr.ElementAt(0, 1)*p[1] + tr.ElementAt(0, 2)*p[2] + tr.ElementAt(0, 3); 482 s[1] = tr.ElementAt(1, 0)*p[0] + tr.ElementAt(1, 1)*p[1] + tr.ElementAt(1, 2)*p[2] + tr.ElementAt(1, 3); 483 s[2] = tr.ElementAt(2, 0)*p[0] + tr.ElementAt(2, 1)*p[1] + tr.ElementAt(2, 2)*p[2] + tr.ElementAt(2, 3); 484 w = tr.ElementAt(3, 0)*p[0] + tr.ElementAt(3, 1)*p[1] + tr.ElementAt(3, 2)*p[2] + tr.ElementAt(3, 3); 485 if(w!= 0) s /= w; 486 return s; 487 } 488 }; 489 490 } //namespace 491 #endif 492 493 #endif 494