simgear/math/SGMatrix.hxx

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