X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2FSGMatrix.hxx;h=a7517a698602b79348f124b51d1e5949c2a36cf5;hb=6a7c2000027cd22eea603e936ddbad1a5bfc8b04;hp=e91140d28d2c4f503da1a140cb34cb54dfd18f84;hpb=5127e2f89c915f3441c4ba1a777d94c344f33e7e;p=simgear.git

diff --git a/simgear/math/SGMatrix.hxx b/simgear/math/SGMatrix.hxx
index e91140d2..a7517a69 100644
--- 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);