X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2FSGMatrix.hxx;h=a7517a698602b79348f124b51d1e5949c2a36cf5;hb=914d3e6a2b323cf9f186cbef2aef7865ea07b309;hp=3e5628c45e4313ca8812300a87f028e934e9756c;hpb=75f817b39c693b2679ebc4b95bf554e65307c067;p=simgear.git

diff --git a/simgear/math/SGMatrix.hxx b/simgear/math/SGMatrix.hxx
index 3e5628c4..a7517a69 100644
--- a/simgear/math/SGMatrix.hxx
+++ b/simgear/math/SGMatrix.hxx
@@ -1,3 +1,20 @@
+// Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
+//
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Library General Public
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+//
+
 #ifndef SGMatrix_H
 #define SGMatrix_H
 
@@ -5,9 +22,6 @@
 template<typename T>
 struct TransNegRef;
 
-template<typename T>
-class SGMatrix;
-
 /// 3D Matrix Class
 template<typename T>
 class SGMatrix {
@@ -49,14 +63,13 @@ public:
   }
 
   /// Constructor, build up a SGMatrix from a translation
-  SGMatrix(const SGVec3<T>& trans)
+  template<typename S>
+  SGMatrix(const SGVec3<S>& trans)
   { set(trans); }
 
   /// Constructor, build up a SGMatrix from a rotation and a translation
-  SGMatrix(const SGQuat<T>& quat, const SGVec3<T>& trans)
-  { set(quat, trans); }
-  /// Constructor, build up a SGMatrix from a rotation and a translation
-  SGMatrix(const SGQuat<T>& quat)
+  template<typename S>
+  SGMatrix(const SGQuat<S>& quat)
   { set(quat); }
 
   /// Copy constructor for a transposed negated matrix
@@ -64,39 +77,22 @@ public:
   { set(tm); }
 
   /// Set from a tranlation
-  void set(const SGVec3<T>& trans)
+  template<typename S>
+  void set(const SGVec3<S>& trans)
   {
     _data.flat[0] = 1; _data.flat[4] = 0;
-    _data.flat[8] = 0; _data.flat[12] = -trans(0);
+    _data.flat[8] = 0; _data.flat[12] = T(trans(0));
     _data.flat[1] = 0; _data.flat[5] = 1;
-    _data.flat[9] = 0; _data.flat[13] = -trans(1);
+    _data.flat[9] = 0; _data.flat[13] = T(trans(1));
     _data.flat[2] = 0; _data.flat[6] = 0;
-    _data.flat[10] = 1; _data.flat[14] = -trans(2);
+    _data.flat[10] = 1; _data.flat[14] = T(trans(2));
     _data.flat[3] = 0; _data.flat[7] = 0;
     _data.flat[11] = 0; _data.flat[15] = 1;
   }
 
   /// Set from a scale/rotation and tranlation
-  void set(const SGQuat<T>& quat, const SGVec3<T>& trans)
-  {
-    T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
-    T xx = x*x; T yy = y*y; T zz = z*z;
-    T wx = w*x; T wy = w*y; T wz = w*z;
-    T xy = x*y; T xz = x*z; T yz = y*z;
-    _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz);
-    _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0;
-    _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz);
-    _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0;
-    _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx);
-    _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0;
-    // Well, this one is ugly here, as that xform method on the current
-    // object needs the above data to be already set ...
-    SGVec3<T> t = xformVec(trans);
-    _data.flat[12] = -t(0); _data.flat[13] = -t(1);
-    _data.flat[14] = -t(2); _data.flat[15] = 1;
-  }
-  /// Set from a scale/rotation and tranlation
-  void set(const SGQuat<T>& quat)
+  template<typename S>
+  void set(const SGQuat<S>& quat)
   {
     T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z();
     T xx = x*x; T yy = y*y; T zz = z*z;
@@ -185,6 +181,52 @@ public:
   /// Inplace matrix multiplication, post multiply
   SGMatrix& operator*=(const SGMatrix<T>& m2);
 
+  template<typename S>
+  SGMatrix& preMultTranslate(const SGVec3<S>& t)
+  {
+    for (unsigned i = 0; i < 3; ++i) {
+      T tmp = T(t(i));
+      if (tmp == 0)
+        continue;
+      (*this)(i,0) += tmp*(*this)(3,0);
+      (*this)(i,1) += tmp*(*this)(3,1);
+      (*this)(i,2) += tmp*(*this)(3,2);
+      (*this)(i,3) += tmp*(*this)(3,3);
+    }
+    return *this;
+  }
+  template<typename S>
+  SGMatrix& postMultTranslate(const SGVec3<S>& t)
+  {
+    SGVec4<T> col3((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3));
+    for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i) {
+      SGVec4<T> tmp((*this)(0,i), (*this)(1,i), (*this)(2,i), (*this)(3,i));
+      col3 += T(t(i))*tmp;
+    }
+    (*this)(0,3) = col3(0); (*this)(1,3) = col3(1);
+    (*this)(2,3) = col3(2); (*this)(3,3) = col3(3);
+    return *this;
+  }
+
+  SGMatrix& preMultRotate(const SGQuat<T>& r)
+  {
+    for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
+      SGVec3<T> col((*this)(0,i), (*this)(1,i), (*this)(2,i));
+      col = r.transform(col);
+      (*this)(0,i) = col(0); (*this)(1,i) = col(1); (*this)(2,i) = col(2);
+    }
+    return *this;
+  }
+  SGMatrix& postMultRotate(const SGQuat<T>& r)
+  {
+    for (unsigned i = 0; i < SGMatrix<T>::nCols; ++i) {
+      SGVec3<T> col((*this)(i,0), (*this)(i,1), (*this)(i,2));
+      col = r.backTransform(col);
+      (*this)(i,0) = col(0); (*this)(i,1) = col(1); (*this)(i,2) = col(2);
+    }
+    return *this;
+  }
+
   SGVec3<T> xformPt(const SGVec3<T>& pt) const
   {
     SGVec3<T> tpt;
@@ -554,18 +596,14 @@ operator<<(std::basic_ostream<char_type, traits_type>& s, const SGMatrix<T>& m)
   return s;
 }
 
-/// Two classes doing actually the same on different types
-typedef SGMatrix<float> SGMatrixf;
-typedef SGMatrix<double> SGMatrixd;
-
 inline
 SGMatrixf
 toMatrixf(const SGMatrixd& m)
 {
-  return SGMatrixf(m(0,0), m(0,1), m(0,2), m(0,3),
-                   m(1,0), m(1,1), m(1,2), m(1,3),
-                   m(3,0), m(2,1), m(2,2), m(2,3),
-                   m(4,0), m(4,1), m(4,2), m(4,3));
+  return SGMatrixf((float)m(0,0), (float)m(0,1), (float)m(0,2), (float)m(0,3),
+                   (float)m(1,0), (float)m(1,1), (float)m(1,2), (float)m(1,3),
+                   (float)m(2,0), (float)m(2,1), (float)m(2,2), (float)m(2,3),
+                   (float)m(3,0), (float)m(3,1), (float)m(3,2), (float)m(3,3));
 }
 
 inline
@@ -574,8 +612,8 @@ toMatrixd(const SGMatrixf& m)
 {
   return SGMatrixd(m(0,0), m(0,1), m(0,2), m(0,3),
                    m(1,0), m(1,1), m(1,2), m(1,3),
-                   m(3,0), m(2,1), m(2,2), m(2,3),
-                   m(4,0), m(4,1), m(4,2), m(4,3));
+                   m(2,0), m(2,1), m(2,2), m(2,3),
+                   m(3,0), m(3,1), m(3,2), m(3,3));
 }
 
 #endif