Merge branch 'tat/framework'

[simgear.git] / simgear / math / SGMatrix.hxx

diff --git a/simgear/math/SGMatrix.hxx b/simgear/math/SGMatrix.hxx
index e91140d28d2c4f503da1a140cb34cb54dfd18f84..a7517a698602b79348f124b51d1e5949c2a36cf5 100644 (file)
--- a/simgear/math/SGMatrix.hxx
+++ b/simgear/math/SGMatrix.hxx
@@ -184,8 +184,15 @@ public:
   template<typename S>
   SGMatrix& preMultTranslate(const SGVec3<S>& t)
   {
-    for (unsigned i = 0; i < SGMatrix<T>::nCols-1; ++i)
-      (*this)(i,3) += T(t(i));
+    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>
@@ -193,7 +200,7 @@ public:
   {
     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,3), (*this)(1,3), (*this)(2,3), (*this)(3,3));
+      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);