Always pivot rows when inverting for better numerical stability

diff --git a/source/linal/squarematrix.h b/source/linal/squarematrix.h
index 1cdc56726455f9ab2b97589b1b93bacbad702eac..5117bb3fa189ee4339d0ce52cdbb4d2ff141b83b 100644 (file)
--- a/source/linal/squarematrix.h
+++ b/source/linal/squarematrix.h
@@ -58,16 +58,16 @@ SquareMatrix<T, S> &SquareMatrix<T, S>::invert()
        SquareMatrix<T, S> r = identity();
        for(unsigned i=0; i<S; ++i)
        {
-               if(this->element(i, i)==T(0))
-               {
-                       unsigned pivot = i;
-                       for(unsigned j=i+1; j<S; ++j)
-                               if(abs(this->element(j, i))>abs(this->element(pivot, i)))
-                                       pivot = j;
+               unsigned pivot = i;
+               for(unsigned j=i+1; j<S; ++j)
+                       if(abs(this->element(j, i))>abs(this->element(pivot, i)))
+                               pivot = j;
 
-                       if(pivot==i)
-                               throw not_invertible();
+               if(this->element(pivot, i)==T(0))
+                       throw not_invertible();
 
+               if(pivot!=i)
+               {
                        this->exchange_rows(i, pivot);
                        r.exchange_rows(i, pivot);
                }