SimGear: SGMatrix.hxx Source File

00001 // Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
00002 //
00003 // This library is free software; you can redistribute it and/or
00004 // modify it under the terms of the GNU Library General Public
00005 // License as published by the Free Software Foundation; either
00006 // version 2 of the License, or (at your option) any later version.
00007 //
00008 // This library is distributed in the hope that it will be useful,
00009 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00010 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00011 // Library General Public License for more details.
00012 //
00013 // You should have received a copy of the GNU General Public License
00014 // along with this program; if not, write to the Free Software
00015 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00016 //
00017 
00018 #ifndef SGMatrix_H
00019 #define SGMatrix_H
00020 
00022 template<typename T>
00023 struct TransNegRef;
00024 
00026 template<typename T>
00027 class SGMatrix {
00028 public:
00029   enum { nCols = 4, nRows = 4, nEnts = 16 };
00030   typedef T value_type;
00031 
00034   SGMatrix(void)
00035   {
00039 #ifndef NDEBUG
00040     for (unsigned i = 0; i < nEnts; ++i)
00041       _data.flat[i] = SGLimits<T>::quiet_NaN();
00042 #endif
00043   }
00046   explicit SGMatrix(const T* data)
00047   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] = data[i]; }
00048 
00050   SGMatrix(T m00, T m01, T m02, T m03,
00051            T m10, T m11, T m12, T m13,
00052            T m20, T m21, T m22, T m23,
00053            T m30, T m31, T m32, T m33)
00054   {
00055     _data.flat[0] = m00; _data.flat[1] = m10;
00056     _data.flat[2] = m20; _data.flat[3] = m30;
00057     _data.flat[4] = m01; _data.flat[5] = m11;
00058     _data.flat[6] = m21; _data.flat[7] = m31;
00059     _data.flat[8] = m02; _data.flat[9] = m12;
00060     _data.flat[10] = m22; _data.flat[11] = m32;
00061     _data.flat[12] = m03; _data.flat[13] = m13;
00062     _data.flat[14] = m23; _data.flat[15] = m33;
00063   }
00064 
00066   template<typename S>
00067   SGMatrix(const SGVec3<S>& trans)
00068   { set(trans); }
00069 
00071   template<typename S>
00072   SGMatrix(const SGQuat<S>& quat)
00073   { set(quat); }
00074 
00076   SGMatrix(const TransNegRef<T>& tm)
00077   { set(tm); }
00078 
00080   template<typename S>
00081   void set(const SGVec3<S>& trans)
00082   {
00083     _data.flat[0] = 1; _data.flat[4] = 0;
00084     _data.flat[8] = 0; _data.flat[12] = T(trans(0));
00085     _data.flat[1] = 0; _data.flat[5] = 1;
00086     _data.flat[9] = 0; _data.flat[13] = T(trans(1));
00087     _data.flat[2] = 0; _data.flat[6] = 0;
00088     _data.flat[10] = 1; _data.flat[14] = T(trans(2));
00089     _data.flat[3] = 0; _data.flat[7] = 0;
00090     _data.flat[11] = 0; _data.flat[15] = 1;
00091   }
00092 
00094   template<typename S>
00095   void set(const SGQuat<S>& quat)
00096   {
00097     T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
00098     T xx = x*x; T yy = y*y; T zz = z*z;
00099     T wx = w*x; T wy = w*y; T wz = w*z;
00100     T xy = x*y; T xz = x*z; T yz = y*z;
00101     _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz);
00102     _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0;
00103     _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz);
00104     _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0;
00105     _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx);
00106     _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0;
00107     _data.flat[12] = 0; _data.flat[13] = 0;
00108     _data.flat[14] = 0; _data.flat[15] = 1;
00109   }
00110 
00112   void set(const TransNegRef<T>& tm)
00113   {
00114     const SGMatrix& m = tm.m;
00115     _data.flat[0] = m(0,0);
00116     _data.flat[1] = m(0,1);
00117     _data.flat[2] = m(0,2);
00118     _data.flat[3] = m(3,0);
00119 
00120     _data.flat[4] = m(1,0);
00121     _data.flat[5] = m(1,1);
00122     _data.flat[6] = m(1,2);
00123     _data.flat[7] = m(3,1);
00124 
00125     _data.flat[8] = m(2,0);
00126     _data.flat[9] = m(2,1);
00127     _data.flat[10] = m(2,2);
00128     _data.flat[11] = m(3,2);
00129 
00130     // Well, this one is ugly here, as that xform method on the current
00131     // object needs the above data to be already set ...
00132     SGVec3<T> t = xformVec(SGVec3<T>(m(0,3), m(1,3), m(2,3)));
00133     _data.flat[12] = -t(0);
00134     _data.flat[13] = -t(1);
00135     _data.flat[14] = -t(2);
00136     _data.flat[15] = m(3,3);
00137   }
00138 
00140   const T& operator()(unsigned i, unsigned j) const
00141   { return _data.flat[i + 4*j]; }
00143   T& operator()(unsigned i, unsigned j)
00144   { return _data.flat[i + 4*j]; }
00145 
00147   const T& operator[](unsigned i) const
00148   { return _data.flat[i]; }
00150   T& operator[](unsigned i)
00151   { return _data.flat[i]; }
00152 
00154   const T* data(void) const
00155   { return _data.flat; }
00157   T* data(void)
00158   { return _data.flat; }
00159 
00161   const T (&sg(void) const)[4][4]
00162   { return _data.carray; }
00164   T (&sg(void))[4][4]
00165   { return _data.carray; }
00166 
00168   SGMatrix& operator+=(const SGMatrix& m)
00169   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] += m._data.flat[i]; return *this; }
00171   SGMatrix& operator-=(const SGMatrix& m)
00172   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] -= m._data.flat[i]; return *this; }
00174   template<typename S>
00175   SGMatrix& operator*=(S s)
00176   { for (unsigned i = 0; i < nEnts; ++i) _data.flat[i] *= s; return *this; }
00178   template<typename S>
00179   SGMatrix& operator/=(S s)
00180   { return operator*=(1/T(s)); }
00182   SGMatrix& operator*=(const SGMatrix<T>& m2);
00183 
00184   template<typename S>
00185   SGMatrix& preMultTranslate(const SGVec3<S>& t)
00186   {
00187     for (unsigned i = 0; i < 3; ++i) {
00188       T tmp = T(t(i));
00189       if (tmp == 0)
00190         continue;
00191       (*this)(i,0) += tmp*(*this)(3,0);
00192       (*this)(i,1) += tmp*(*this)(3,1);
00193       (*this)(i,2) += tmp*(*this)(3,2);
00194       (*this)(i,3) += tmp*(*this)(3,3);
00195     }
00196     return *this;
00197   }
00198   template<typename S>
00199   SGMatrix& postMultTranslate(const SGVec3<S>& t)
00200   {
00201     SGVec4<T> col3((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3));
00202     for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
00203       SGVec4<T> tmp((*this)(0,i), (*this)(1,i), (*this)(2,i), (*this)(3,i));
00204       col3 += T(t(i))*tmp;
00205     }
00206     (*this)(0,3) = col3(0); (*this)(1,3) = col3(1);
00207     (*this)(2,3) = col3(2); (*this)(3,3) = col3(3);
00208     return *this;
00209   }
00210 
00211   SGMatrix& preMultRotate(const SGQuat<T>& r)
00212   {
00213     for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
00214       SGVec3<T> col((*this)(0,i), (*this)(1,i), (*this)(2,i));
00215       col = r.transform(col);
00216       (*this)(0,i) = col(0); (*this)(1,i) = col(1); (*this)(2,i) = col(2);
00217     }
00218     return *this;
00219   }
00220   SGMatrix& postMultRotate(const SGQuat<T>& r)
00221   {
00222     for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
00223       SGVec3<T> col((*this)(i,0), (*this)(i,1), (*this)(i,2));
00224       col = r.backTransform(col);
00225       (*this)(i,0) = col(0); (*this)(i,1) = col(1); (*this)(i,2) = col(2);
00226     }
00227     return *this;
00228   }
00229 
00230   SGVec3<T> xformPt(const SGVec3<T>& pt) const
00231   {
00232     SGVec3<T> tpt;
00233     tpt(0) = (*this)(0,3);
00234     tpt(1) = (*this)(1,3);
00235     tpt(2) = (*this)(2,3);
00236     for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
00237       T tmp = pt(i);
00238       tpt(0) += tmp*(*this)(0,i);
00239       tpt(1) += tmp*(*this)(1,i);
00240       tpt(2) += tmp*(*this)(2,i);
00241     }
00242     return tpt;
00243   }
00244   SGVec3<T> xformVec(const SGVec3<T>& v) const
00245   {
00246     SGVec3<T> tv;
00247     T tmp = v(0);
00248     tv(0) = tmp*(*this)(0,0);
00249     tv(1) = tmp*(*this)(1,0);
00250     tv(2) = tmp*(*this)(2,0);
00251     for (unsigned i = 1; i < SGMatrix<T>::nCols-1; ++i) {
00252       T tmp = v(i);
00253       tv(0) += tmp*(*this)(0,i);
00254       tv(1) += tmp*(*this)(1,i);
00255       tv(2) += tmp*(*this)(2,i);
00256     }
00257     return tv;
00258   }
00259 
00261   static SGMatrix zeros(void)
00262   { return SGMatrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); }
00263 
00265   static SGMatrix unit(void)
00266   { return SGMatrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); }
00267 
00268 private:
00270   union Data {
00271     T flat[16];
00272     T carray[4][4];
00273   };
00274 
00277   Data _data;
00278 };
00279 
00282 template<typename T>
00283 struct TransNegRef {
00284   TransNegRef(const SGMatrix<T>& _m) : m(_m) {}
00285   const SGMatrix<T>& m;
00286 };
00287 
00289 template<typename T>
00290 inline
00291 const SGMatrix<T>&
00292 operator+(const SGMatrix<T>& m)
00293 { return m; }
00294 
00296 template<typename T>
00297 inline
00298 SGMatrix<T>
00299 operator-(const SGMatrix<T>& m)
00300 {
00301   SGMatrix<T> ret;
00302   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
00303     ret[i] = -m[i];
00304   return ret;
00305 }
00306 
00308 template<typename T>
00309 inline
00310 SGMatrix<T>
00311 operator+(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
00312 {
00313   SGMatrix<T> ret;
00314   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
00315     ret[i] = m1[i] + m2[i];
00316   return ret;
00317 }
00318 
00320 template<typename T>
00321 inline
00322 SGMatrix<T>
00323 operator-(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
00324 {
00325   SGMatrix<T> ret;
00326   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
00327     ret[i] = m1[i] - m2[i];
00328   return ret;
00329 }
00330 
00332 template<typename S, typename T>
00333 inline
00334 SGMatrix<T>
00335 operator*(S s, const SGMatrix<T>& m)
00336 {
00337   SGMatrix<T> ret;
00338   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
00339     ret[i] = s*m[i];
00340   return ret;
00341 }
00342 
00344 template<typename S, typename T>
00345 inline
00346 SGMatrix<T>
00347 operator*(const SGMatrix<T>& m, S s)
00348 {
00349   SGMatrix<T> ret;
00350   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
00351     ret[i] = s*m[i];
00352   return ret;
00353 }
00354 
00356 template<typename T>
00357 inline
00358 SGVec4<T>
00359 operator*(const SGMatrix<T>& m, const SGVec4<T>& v)
00360 {
00361   SGVec4<T> mv;
00362   T tmp = v(0);
00363   mv(0) = tmp*m(0,0);
00364   mv(1) = tmp*m(1,0);
00365   mv(2) = tmp*m(2,0);
00366   mv(3) = tmp*m(3,0);
00367   for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
00368     T tmp = v(i);
00369     mv(0) += tmp*m(0,i);
00370     mv(1) += tmp*m(1,i);
00371     mv(2) += tmp*m(2,i);
00372     mv(3) += tmp*m(3,i);
00373   }
00374   return mv;
00375 }
00376 
00378 template<typename T>
00379 inline
00380 SGVec4<T>
00381 operator*(const TransNegRef<T>& tm, const SGVec4<T>& v)
00382 {
00383   const SGMatrix<T>& m = tm.m;
00384   SGVec4<T> mv;
00385   SGVec3<T> v2;
00386   T tmp = v(3);
00387   mv(0) = v2(0) = -tmp*m(0,3);
00388   mv(1) = v2(1) = -tmp*m(1,3);
00389   mv(2) = v2(2) = -tmp*m(2,3);
00390   mv(3) = tmp*m(3,3);
00391   for (unsigned i = 0; i < SGMatrix<T>::nCols - 1; ++i) {
00392     T tmp = v(i) + v2(i);
00393     mv(0) += tmp*m(i,0);
00394     mv(1) += tmp*m(i,1);
00395     mv(2) += tmp*m(i,2);
00396     mv(3) += tmp*m(3,i);
00397   }
00398   return mv;
00399 }
00400 
00402 template<typename T>
00403 inline
00404 SGMatrix<T>
00405 operator*(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
00406 {
00407   SGMatrix<T> m;
00408   for (unsigned j = 0; j < SGMatrix<T>::nCols; ++j) {
00409     T tmp = m2(0,j);
00410     m(0,j) = tmp*m1(0,0);
00411     m(1,j) = tmp*m1(1,0);
00412     m(2,j) = tmp*m1(2,0);
00413     m(3,j) = tmp*m1(3,0);
00414     for (unsigned i = 1; i < SGMatrix<T>::nCols; ++i) {
00415       T tmp = m2(i,j);
00416       m(0,j) += tmp*m1(0,i);
00417       m(1,j) += tmp*m1(1,i);
00418       m(2,j) += tmp*m1(2,i);
00419       m(3,j) += tmp*m1(3,i);
00420     }
00421   }
00422   return m;
00423 }
00424 
00426 template<typename T>
00427 inline
00428 SGMatrix<T>&
00429 SGMatrix<T>::operator*=(const SGMatrix<T>& m2)
00430 { (*this) = operator*(*this, m2); return *this; }
00431 
00434 template<typename T>
00435 inline
00436 TransNegRef<T>
00437 transNeg(const SGMatrix<T>& m)
00438 { return TransNegRef<T>(m); }
00439 
00443 template<typename T>
00444 inline
00445 bool
00446 invert(SGMatrix<T>& dst, const SGMatrix<T>& src)
00447 {
00448   // Do a LU decomposition with row pivoting and solve into dst
00449   SGMatrix<T> tmp = src;
00450   dst = SGMatrix<T>::unit();
00451 
00452   for (unsigned i = 0; i < 4; ++i) {
00453     T val = tmp(i,i);
00454     unsigned ind = i;
00455 
00456     // Find the row with the maximum value in the i-th colum
00457     for (unsigned j = i + 1; j < 4; ++j) {
00458       if (fabs(tmp(j, i)) > fabs(val)) {
00459         ind = j;
00460         val = tmp(j, i);
00461       }
00462     }
00463 
00464     // Do row pivoting
00465     if (ind != i) {
00466       for (unsigned j = 0; j < 4; ++j) {
00467         T t;
00468         t = dst(i,j); dst(i,j) = dst(ind,j); dst(ind,j) = t;
00469         t = tmp(i,j); tmp(i,j) = tmp(ind,j); tmp(ind,j) = t;
00470       }
00471     }
00472 
00473     // Check for singularity
00474     if (fabs(val) <= SGLimits<T>::min())
00475       return false;
00476 
00477     T ival = 1/val;
00478     for (unsigned j = 0; j < 4; ++j) {
00479       tmp(i,j) *= ival;
00480       dst(i,j) *= ival;
00481     }
00482 
00483     for (unsigned j = 0; j < 4; ++j) {
00484       if (j == i)
00485         continue;
00486 
00487       val = tmp(j,i);
00488       for (unsigned k = 0; k < 4; ++k) {
00489         tmp(j,k) -= tmp(i,k) * val;
00490         dst(j,k) -= dst(i,k) * val;
00491       }
00492     }
00493   }
00494   return true;
00495 }
00496 
00499 template<typename T>
00500 inline
00501 T
00502 norm1(const SGMatrix<T>& m)
00503 {
00504   T nrm = 0;
00505   for (unsigned i = 0; i < SGMatrix<T>::nRows; ++i) {
00506     T sum = fabs(m(i, 0)) + fabs(m(i, 1)) + fabs(m(i, 2)) + fabs(m(i, 3));
00507     if (nrm < sum)
00508       nrm = sum;
00509   }
00510   return nrm;
00511 }
00512 
00515 template<typename T>
00516 inline
00517 T
00518 normInf(const SGMatrix<T>& m)
00519 {
00520   T nrm = 0;
00521   for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
00522     T sum = fabs(m(0, i)) + fabs(m(1, i)) + fabs(m(2, i)) + fabs(m(3, i));
00523     if (nrm < sum)
00524       nrm = sum;
00525   }
00526   return nrm;
00527 }
00528 
00530 template<typename T>
00531 inline
00532 bool
00533 operator==(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
00534 {
00535   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i)
00536     if (m1[i] != m2[i])
00537       return false;
00538   return true;
00539 }
00540 
00542 template<typename T>
00543 inline
00544 bool
00545 operator!=(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
00546 { return ! (m1 == m2); }
00547 
00549 template<typename T>
00550 inline
00551 bool
00552 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol, T atol)
00553 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)) + atol; }
00554 
00556 template<typename T>
00557 inline
00558 bool
00559 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2, T rtol)
00560 { return norm1(m1 - m2) < rtol*(norm1(m1) + norm1(m2)); }
00561 
00563 template<typename T>
00564 inline
00565 bool
00566 equivalent(const SGMatrix<T>& m1, const SGMatrix<T>& m2)
00567 {
00568   T tol = 100*SGLimits<T>::epsilon();
00569   return equivalent(m1, m2, tol, tol);
00570 }
00571 
00572 #ifndef NDEBUG
00573 template<typename T>
00574 inline
00575 bool
00576 isNaN(const SGMatrix<T>& m)
00577 {
00578   for (unsigned i = 0; i < SGMatrix<T>::nEnts; ++i) {
00579     if (SGMisc<T>::isNaN(m[i]))
00580       return true;
00581   }
00582   return false;
00583 }
00584 #endif
00585 
00587 template<typename char_type, typename traits_type, typename T> 
00588 inline
00589 std::basic_ostream<char_type, traits_type>&
00590 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGMatrix<T>& m)
00591 {
00592   s << "[ " << m(0,0) << ", " << m(0,1) << ", " << m(0,2) << ", " << m(0,3) << "\n";
00593   s << "  " << m(1,0) << ", " << m(1,1) << ", " << m(1,2) << ", " << m(1,3) << "\n";
00594   s << "  " << m(2,0) << ", " << m(2,1) << ", " << m(2,2) << ", " << m(2,3) << "\n";
00595   s << "  " << m(3,0) << ", " << m(3,1) << ", " << m(3,2) << ", " << m(3,3) << " ]";
00596   return s;
00597 }
00598 
00599 inline
00600 SGMatrixf
00601 toMatrixf(const SGMatrixd& m)
00602 {
00603   return SGMatrixf((float)m(0,0), (float)m(0,1), (float)m(0,2), (float)m(0,3),
00604                    (float)m(1,0), (float)m(1,1), (float)m(1,2), (float)m(1,3),
00605                    (float)m(2,0), (float)m(2,1), (float)m(2,2), (float)m(2,3),
00606                    (float)m(3,0), (float)m(3,1), (float)m(3,2), (float)m(3,3));
00607 }
00608 
00609 inline
00610 SGMatrixd
00611 toMatrixd(const SGMatrixf& m)
00612 {
00613   return SGMatrixd(m(0,0), m(0,1), m(0,2), m(0,3),
00614                    m(1,0), m(1,1), m(1,2), m(1,3),
00615                    m(2,0), m(2,1), m(2,2), m(2,3),
00616                    m(3,0), m(3,1), m(3,2), m(3,3));
00617 }
00618 
00619 #endif