simgear/math/SGMatrix.hxx

   1 // Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Library General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Library General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU General Public License
  14 // along with this program; if not, write to the Free Software
  15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  16 //
  17
  18 #ifndef SGMatrix_H
  19 #define SGMatrix_H
  20
  21 /// Expression templates for poor programmers ... :)
  22 template<typename T>
  23 struct TransNegRef;
  24
  25 /// 3D Matrix Class
  26 template<typename T>
  27 class SGMatrix {
  28 public:
  29   enum { nCols = 4, nRows = 4, nEnts = 16 };
  30   typedef T value_type;
  31
  32   /// Default constructor. Does not initialize at all.
  33   /// If you need them zero initialized, use SGMatrix::zeros()
  34   SGMatrix(void)
  35   {
  36     /// Initialize with nans in the debug build, that will guarantee to have
  37     /// a fast uninitialized default constructor in the release but shows up
  38     /// uninitialized values in the debug build very fast ...
  39 #ifndef NDEBUG
  40     for (unsigned i = 0; i < nEnts; ++i)
  41       _data.flat[i] = SGLimits<T>::quiet_NaN();
  42 #endif
  43   }
  44   /// Constructor. Initialize by the content of a plain array,
  45   /// make sure it has at least 16 elements
  46   explicit SGMatrix(const T* data)
  47   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] = data[i]; }
  48
  49   /// Constructor, build up a SGMatrix from given elements
  50   SGMatrix(T m00, T m01, T m02, T m03,
  51            T m10, T m11, T m12, T m13,
  52            T m20, T m21, T m22, T m23,
  53            T m30, T m31, T m32, T m33)
  54   {
  55     _data.flat[0] = m00; _data.flat[1] = m10;
  56     _data.flat[2] = m20; _data.flat[3] = m30;
  57     _data.flat[4] = m01; _data.flat[5] = m11;
  58     _data.flat[6] = m21; _data.flat[7] = m31;
  59     _data.flat[8] = m02; _data.flat[9] = m12;
  60     _data.flat[10] = m22; _data.flat[11] = m32;
  61     _data.flat[12] = m03; _data.flat[13] = m13;
  62     _data.flat[14] = m23; _data.flat[15] = m33;
  63   }
  64
  65   /// Constructor, build up a SGMatrix from a translation
  66   SGMatrix(const SGVec3<T>& trans)
  67   { set(trans); }
  68
  69   /// Constructor, build up a SGMatrix from a rotation and a translation
  70   SGMatrix(const SGQuat<T>& quat, const SGVec3<T>& trans)
  71   { set(quat, trans); }
  72   /// Constructor, build up a SGMatrix from a rotation and a translation
  73   SGMatrix(const SGQuat<T>& quat)
  74   { set(quat); }
  75
  76   /// Copy constructor for a transposed negated matrix
  77   SGMatrix(const TransNegRef<T>& tm)
  78   { set(tm); }
  79
  80   /// Set from a tranlation
  81   void set(const SGVec3<T>& trans)
  82   {
  83     _data.flat[0] = 1; _data.flat[4] = 0;
  84     _data.flat[8] = 0; _data.flat[12] = -trans(0);
  85     _data.flat[1] = 0; _data.flat[5] = 1;
  86     _data.flat[9] = 0; _data.flat[13] = -trans(1);
  87     _data.flat[2] = 0; _data.flat[6] = 0;
  88     _data.flat[10] = 1; _data.flat[14] = -trans(2);
  89     _data.flat[3] = 0; _data.flat[7] = 0;
  90     _data.flat[11] = 0; _data.flat[15] = 1;
  91   }
  92
  93   /// Set from a scale/rotation and tranlation
  94   void set(const SGQuat<T>& quat, const SGVec3<T>& trans)
  95   {
  96     T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
  97     T xx = x*x; T yy = y*y; T zz = z*z;
  98     T wx = w*x; T wy = w*y; T wz = w*z;
  99     T xy = x*y; T xz = x*z; T yz = y*z;
 100     _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz);
 101     _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0;
 102     _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz);
 103     _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0;
 104     _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx);
 105     _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0;
 106     // Well, this one is ugly here, as that xform method on the current
 107     // object needs the above data to be already set ...
 108     SGVec3<T> t = xformVec(trans);
 109     _data.flat[12] = -t(0); _data.flat[13] = -t(1);
 110     _data.flat[14] = -t(2); _data.flat[15] = 1;
 111   }
 112   /// Set from a scale/rotation and tranlation
 113   void set(const SGQuat<T>& quat)
 114   {
 115     T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
 116     T xx = x*x; T yy = y*y; T zz = z*z;
 117     T wx = w*x; T wy = w*y; T wz = w*z;
 118     T xy = x*y; T xz = x*z; T yz = y*z;
 119     _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz);
 120     _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0;
 121     _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz);
 122     _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0;
 123     _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx);
 124     _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0;
 125     _data.flat[12] = 0; _data.flat[13] = 0;
 126     _data.flat[14] = 0; _data.flat[15] = 1;
 127   }
 128
 129   /// set from a transposed negated matrix
 130   void set(const TransNegRef<T>& tm)
 131   {
 132     const SGMatrix& m = tm.m;
 133     _data.flat[0] = m(0,0);
 134     _data.flat[1] = m(0,1);
 135     _data.flat[2] = m(0,2);
 136     _data.flat[3] = m(3,0);
 137
 138     _data.flat[4] = m(1,0);
 139     _data.flat[5] = m(1,1);
 140     _data.flat[6] = m(1,2);
 141     _data.flat[7] = m(3,1);
 142
 143     _data.flat[8] = m(2,0);
 144     _data.flat[9] = m(2,1);
 145     _data.flat[10] = m(2,2);
 146     _data.flat[11] = m(3,2);
 147
 148     // Well, this one is ugly here, as that xform method on the current
 149     // object needs the above data to be already set ...
 150     SGVec3<T> t = xformVec(SGVec3<T>(m(0,3), m(1,3), m(2,3)));
 151     _data.flat[12] = -t(0);
 152     _data.flat[13] = -t(1);
 153     _data.flat[14] = -t(2);
 154     _data.flat[15] = m(3,3);
 155   }
 156
 157   /// Access by index, the index is unchecked
 158   const T& operator()(unsigned i, unsigned j) const
 159   { return _data.flat[i + 4*j]; }
 160   /// Access by index, the index is unchecked
 161   T& operator()(unsigned i, unsigned j)
 162   { return _data.flat[i + 4*j]; }
 163
 164   /// Access raw data by index, the index is unchecked
 165   const T& operator[](unsigned i) const
 166   { return _data.flat[i]; }
 167   /// Access by index, the index is unchecked
 168   T& operator[](unsigned i)
 169   { return _data.flat[i]; }
 170
 171   /// Get the data pointer
 172   const T* data(void) const
 173   { return _data.flat; }
 174   /// Get the data pointer
 175   T* data(void)
 176   { return _data.flat; }
 177
 178   /// Readonly interface function to ssg's sgMat4/sgdMat4
 179   const T (&sg(void) const)[4][4]
 180   { return _data.carray; }
 181   /// Interface function to ssg's sgMat4/sgdMat4
 182   T (&sg(void))[4][4]
 183   { return _data.carray; }
 184
 185   /// Inplace addition
 186   SGMatrix& operator+=(const SGMatrix& m)
 187   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] += m._data.flat[i]; return *this; }
 188   /// Inplace subtraction
 189   SGMatrix& operator-=(const SGMatrix& m)
 190   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] -= m._data.flat[i]; return *this; }
 191   /// Inplace scalar multiplication
 192   template<typename S>
 193   SGMatrix& operator*=(S s)
 194   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] *= s; return *this; }
 195   /// Inplace scalar multiplication by 1/s
 196   template<typename S>
 197   SGMatrix& operator/=(S s)
 198   { return operator*=(1/T(s)); }
 199   /// Inplace matrix multiplication, post multiply
 200   SGMatrix& operator*=(const SGMatrix<T>& m2);
 201
 202   SGVec3<T> xformPt(const SGVec3<T>& pt) const
 203   {
 204     SGVec3<T> tpt;
 205     tpt(0) = (*this)(0,3);
 206     tpt(1) = (*this)(1,3);
 207     tpt(2) = (*this)(2,3);
 208     for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
 209       T tmp = pt(i);
 210       tpt(0) += tmp*(*this)(0,i);
 211       tpt(1) += tmp*(*this)(1,i);
 212       tpt(2) += tmp*(*this)(2,i);
 213     }
 214     return tpt;
 215   }
 216   SGVec3<T> xformVec(const SGVec3<T>& v) const
 217   {
 218     SGVec3<T> tv;
 219     T tmp = v(0);
 220     tv(0) = tmp*(*this)(0,0);
 221     tv(1) = tmp*(*this)(1,0);
 222     tv(2) = tmp*(*this)(2,0);
 223     for (unsigned i = 1; i < SGMatrix<T>::nCols-1; ++i) {
 224       T tmp = v(i);
 225       tv(0) += tmp*(*this)(0,i);
 226       tv(1) += tmp*(*this)(1,i);
 227       tv(2) += tmp*(*this)(2,i);
 228     }
 229     return tv;
 230   }
 231
 232   /// Return an all zero matrix
 233   static SGMatrix zeros(void)
 234   { return SGMatrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); }
 235
 236   /// Return a unit matrix
 237   static SGMatrix unit(void)
 238   { return SGMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); }
 239
 240 private:
 241   /// Required to make that alias safe.
 242   union Data {
 243     T flat[16];
 244     T carray[4][4];
 245   };
 246
 247   /// The actual data, the matrix is stored in column major order,
 248   /// that matches the storage format of OpenGL
 249   Data _data;
 250 };
 251
 252 /// Class to distinguish between a matrix and the matrix with a transposed
 253 /// rotational part and a negated translational part
 254 template<typename T>
 255 struct TransNegRef {
 256   TransNegRef(const SGMatrix<T>& _m) : m(_m) {}
 257   const SGMatrix<T>& m;
 258 };
 259
 260 /// Unary +, do nothing ...
 261 template<typename T>
 262 inline
 263 const SGMatrix<T>&
 264 operator+(const SGMatrix<T>& m)
 265 { return m; }
 266
 267 /// Unary -, do nearly nothing
 268 template<typename T>
 269 inline
 270 SGMatrix<T>
 271 operator-(const SGMatrix<T>& m)
 272 {
 273   SGMatrix<T> ret;
 274   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 275     ret[i] = -m[i];
 276   return ret;
 277 }
 278
 279 /// Binary +
 280 template<typename T>
 281 inline
 282 SGMatrix<T>
 283 operator+(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 284 {
 285   SGMatrix<T> ret;
 286   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 287     ret[i] = m1[i] + m2[i];
 288   return ret;
 289 }
 290
 291 /// Binary -
 292 template<typename T>
 293 inline
 294 SGMatrix<T>
 295 operator-(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 296 {
 297   SGMatrix<T> ret;
 298   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 299     ret[i] = m1[i] - m2[i];
 300   return ret;
 301 }
 302
 303 /// Scalar multiplication
 304 template<typename S, typename T>
 305 inline
 306 SGMatrix<T>
 307 operator*(S s, const SGMatrix<T>& m)
 308 {
 309   SGMatrix<T> ret;
 310   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 311     ret[i] = s*m[i];
 312   return ret;
 313 }
 314
 315 /// Scalar multiplication
 316 template<typename S, typename T>
 317 inline
 318 SGMatrix<T>
 319 operator*(const SGMatrix<T>& m, S s)
 320 {
 321   SGMatrix<T> ret;
 322   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 323     ret[i] = s*m[i];
 324   return ret;
 325 }
 326
 327 /// Vector multiplication
 328 template<typename T>
 329 inline
 330 SGVec4<T>
 331 operator*(const SGMatrix<T>& m, const SGVec4<T>& v)
 332 {
 333   SGVec4<T> mv;
 334   T tmp = v(0);
 335   mv(0) = tmp*m(0,0);
 336   mv(1) = tmp*m(1,0);
 337   mv(2) = tmp*m(2,0);
 338   mv(3) = tmp*m(3,0);
 339   for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
 340     T tmp = v(i);
 341     mv(0) += tmp*m(0,i);
 342     mv(1) += tmp*m(1,i);
 343     mv(2) += tmp*m(2,i);
 344     mv(3) += tmp*m(3,i);
 345   }
 346   return mv;
 347 }
 348
 349 /// Vector multiplication
 350 template<typename T>
 351 inline
 352 SGVec4<T>
 353 operator*(const TransNegRef<T>& tm, const SGVec4<T>& v)
 354 {
 355   const SGMatrix<T>& m = tm.m;
 356   SGVec4<T> mv;
 357   SGVec3<T> v2;
 358   T tmp = v(3);
 359   mv(0) = v2(0) = -tmp*m(0,3);
 360   mv(1) = v2(1) = -tmp*m(1,3);
 361   mv(2) = v2(2) = -tmp*m(2,3);
 362   mv(3) = tmp*m(3,3);
 363   for (unsigned i = 0; i < SGMatrix<T>::nCols - 1; ++i) {
 364     T tmp = v(i) + v2(i);
 365     mv(0) += tmp*m(i,0);
 366     mv(1) += tmp*m(i,1);
 367     mv(2) += tmp*m(i,2);
 368     mv(3) += tmp*m(3,i);
 369   }
 370   return mv;
 371 }
 372
 373 /// Matrix multiplication
 374 template<typename T>
 375 inline
 376 SGMatrix<T>
 377 operator*(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 378 {
 379   SGMatrix<T> m;
 380   for (unsigned j = 0; j < SGMatrix<T>::nCols; ++j) {
 381     T tmp = m2(0,j);
 382     m(0,j) = tmp*m1(0,0);
 383     m(1,j) = tmp*m1(1,0);
 384     m(2,j) = tmp*m1(2,0);
 385     m(3,j) = tmp*m1(3,0);
 386     for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
 387       T tmp = m2(i,j);
 388       m(0,j) += tmp*m1(0,i);
 389       m(1,j) += tmp*m1(1,i);
 390       m(2,j) += tmp*m1(2,i);
 391       m(3,j) += tmp*m1(3,i);
 392     }
 393   }
 394   return m;
 395 }
 396
 397 /// Inplace matrix multiplication, post multiply
 398 template<typename T>
 399 inline
 400 SGMatrix<T>&
 401 SGMatrix<T>::operator*=(const SGMatrix<T>& m2)
 402 { (*this) = operator*(*this, m2); return *this; }
 403
 404 /// Return a reference to the matrix typed to make sure we use the transposed
 405 /// negated matrix
 406 template<typename T>
 407 inline
 408 TransNegRef<T>
 409 transNeg(const SGMatrix<T>& m)
 410 { return TransNegRef<T>(m); }
 411
 412 /// Compute the inverse if the matrix src. Store the result in dst.
 413 /// Return if the matrix is nonsingular. If it is singular dst contains
 414 /// undefined values
 415 template<typename T>
 416 inline
 417 bool
 418 invert(SGMatrix<T>& dst, const SGMatrix<T>& src)
 419 {
 420   // Do a LU decomposition with row pivoting and solve into dst
 421   SGMatrix<T> tmp = src;
 422   dst = SGMatrix<T>::unit();
 423
 424   for (unsigned i = 0; i < 4; ++i) {
 425     T val = tmp(i,i);
 426     unsigned ind = i;
 427
 428     // Find the row with the maximum value in the i-th colum
 429     for (unsigned j = i + 1; j < 4; ++j) {
 430       if (fabs(tmp(j, i)) > fabs(val)) {
 431         ind = j;
 432         val = tmp(j, i);
 433       }
 434     }
 435
 436     // Do row pivoting
 437     if (ind != i) {
 438       for (unsigned j = 0; j < 4; ++j) {
 439         T t;
 440         t = dst(i,j); dst(i,j) = dst(ind,j); dst(ind,j) = t;
 441         t = tmp(i,j); tmp(i,j) = tmp(ind,j); tmp(ind,j) = t;
 442       }
 443     }
 444
 445     // Check for singularity
 446     if (fabs(val) <= SGLimits<T>::min())
 447       return false;
 448
 449     T ival = 1/val;
 450     for (unsigned j = 0; j < 4; ++j) {
 451       tmp(i,j) *= ival;
 452       dst(i,j) *= ival;
 453     }
 454
 455     for (unsigned j = 0; j < 4; ++j) {
 456       if (j == i)
 457         continue;
 458
 459       val = tmp(j,i);
 460       for (unsigned k = 0; k < 4; ++k) {
 461         tmp(j,k) -= tmp(i,k) * val;
 462         dst(j,k) -= dst(i,k) * val;
 463       }
 464     }
 465   }
 466   return true;
 467 }
 468
 469 /// The 1-norm of the matrix, this is the largest column sum of
 470 /// the absolute values of A.
 471 template<typename T>
 472 inline
 473 T
 474 norm1(const SGMatrix<T>& m)
 475 {
 476   T nrm = 0;
 477   for (unsigned i = 0; i < SGMatrix<T>::nRows; ++i) {
 478     T sum = fabs(m(i, 0)) + fabs(m(i, 1)) + fabs(m(i, 2)) + fabs(m(i, 3));
 479     if (nrm < sum)
 480       nrm = sum;
 481   }
 482   return nrm;
 483 }
 484
 485 /// The inf-norm of the matrix, this is the largest row sum of
 486 /// the absolute values of A.
 487 template<typename T>
 488 inline
 489 T
 490 normInf(const SGMatrix<T>& m)
 491 {
 492   T nrm = 0;
 493   for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
 494     T sum = fabs(m(0, i)) + fabs(m(1, i)) + fabs(m(2, i)) + fabs(m(3, i));
 495     if (nrm < sum)
 496       nrm = sum;
 497   }
 498   return nrm;
 499 }
 500
 501 /// Return true if exactly the same
 502 template<typename T>
 503 inline
 504 bool
 505 operator==(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 506 {
 507   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
 508     if (m1[i] != m2[i])
 509       return false;
 510   return true;
 511 }
 512
 513 /// Return true if not exactly the same
 514 template<typename T>
 515 inline
 516 bool
 517 operator!=(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 518 { return ! (m1 == m2); }
 519
 520 /// Return true if equal to the relative tolerance tol
 521 template<typename T>
 522 inline
 523 bool
 524 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol, T atol)
 525 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)) + atol; }
 526
 527 /// Return true if equal to the relative tolerance tol
 528 template<typename T>
 529 inline
 530 bool
 531 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol)
 532 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)); }
 533
 534 /// Return true if about equal to roundoff of the underlying type
 535 template<typename T>
 536 inline
 537 bool
 538 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
 539 {
 540   T tol = 100*SGLimits<T>::epsilon();
 541   return equivalent(m1, m2, tol, tol);
 542 }
 543
 544 #ifndef NDEBUG
 545 template<typename T>
 546 inline
 547 bool
 548 isNaN(const SGMatrix<T>& m)
 549 {
 550   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i) {
 551     if (SGMisc<T>::isNaN(m[i]))
 552       return true;
 553   }
 554   return false;
 555 }
 556 #endif
 557
 558 /// Output to an ostream
 559 template<typename char_type, typename traits_type, typename T>
 560 inline
 561 std::basic_ostream<char_type, traits_type>&
 562 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGMatrix<T>& m)
 563 {
 564   s << "[ " << m(0,0) << ", " << m(0,1) << ", " << m(0,2) << ", " << m(0,3) << "\n";
 565   s << "  " << m(1,0) << ", " << m(1,1) << ", " << m(1,2) << ", " << m(1,3) << "\n";
 566   s << "  " << m(2,0) << ", " << m(2,1) << ", " << m(2,2) << ", " << m(2,3) << "\n";
 567   s << "  " << m(3,0) << ", " << m(3,1) << ", " << m(3,2) << ", " << m(3,3) << " ]";
 568   return s;
 569 }
 570
 571 inline
 572 SGMatrixf
 573 toMatrixf(const SGMatrixd& m)
 574 {
 575   return SGMatrixf((float)m(0,0), (float)m(0,1), (float)m(0,2), (float)m(0,3),
 576                    (float)m(1,0), (float)m(1,1), (float)m(1,2), (float)m(1,3),
 577                    (float)m(3,0), (float)m(2,1), (float)m(2,2), (float)m(2,3),
 578                    (float)m(4,0), (float)m(4,1), (float)m(4,2), (float)m(4,3));
 579 }
 580
 581 inline
 582 SGMatrixd
 583 toMatrixd(const SGMatrixf& m)
 584 {
 585   return SGMatrixd(m(0,0), m(0,1), m(0,2), m(0,3),
 586                    m(1,0), m(1,1), m(1,2), m(1,3),
 587                    m(3,0), m(2,1), m(2,2), m(2,3),
 588                    m(4,0), m(4,1), m(4,2), m(4,3));
 589 }
 590
 591 #endif