Repository: ddemidov/amgcl (language: python)

Issue:
amgcl::math::inverse fails for a not that bad static_matrix
The following code fails:

```cpp
#include <amgcl/value_type/static_matrix.hpp>
#include <iostream>
int main() {
    amgcl::static_matrix<double, 4, 4> M {
    1, -0.1, -0.028644256, 0.25684664,
    1, -0.1, -0.025972342, 0.25663863,
    1, -0.095699158, -0.029327056, 0.25554974,
    1, -0.09543351, -0.026189496, 0.25796741,
    };

    std::cout << M << std::endl;

    auto I = amgcl::math::inverse(M);

    std::cout << I << std::endl;
}
```
with the message

```
 1 -0.1 -0.0286443 0.256847
 1 -0.1 -0.0259723 0.256639
 1 -0.0956992 -0.0293271 0.25555
 1 -0.0954335 -0.0261895 0.257967

a.out: ../external/amgcl/amgcl/detail/inverse.hpp:47: void amgcl::detail::inverse(int, value_type*, value_type*) [with value_type = double]: Assertion `!math::is_zero(d)' failed.
Aborted (core dumped)
```
However, the matrix itself is not particularly bad, and numpy inverts it sucessfully:

```python
import numpy as np

m = np.asarray([[1, -0.1, -0.028644256, 0.25684664],
                [1, -0.1, -0.025972342, 0.25663863],
                [1, -0.095699158, -0.029327056, 0.25554974],
                [1, -0.09543351, -0.026189496, 0.25796741],
                ])

print('m = \n', m)
print('norm2 =', np.linalg.norm(m, ord=2))
print('det   =', np.linalg.det(m))
i = np.linalg.inv(m);
print('inv =\n', i)
print('i*m =\n', np.dot(i, m))
```
output:
```
m = 
 [[ 1.         -0.1        -0.02864426  0.25684664]
 [ 1.         -0.1        -0.02597234  0.25663863]
 [ 1.         -0.09569916 -0.02932706  0.25554974]
 [ 1.         -0.09543351 -0.0261895   0.25796741]]
norm2 = 2.074840307233657
det   = -3.1547779068784255e-08
inv =
 [[-140.70139646  113.42780461  109.58514433  -81.31155249]
 [-148.79442631  -76.65568098  111.1081599   114.34194739]
 [-338.76483367  340.51638139  -30.1091111    28.35756338]
 [ 455.98528057 -433.48643498 -386.75523038  364.25638478]]
i*m =
 [[ 1.00000000e+00  9.65014612e-16 -3.61623848e-16  3.62053939e-15]
 [-8.52651283e-14  1.00000000e+00  1.56497697e-15 -1.78827904e-14]
 [ 3.90798505e-14 -6.99167378e-15  1.00000000e+00  1.74933439e-14]
 [ 0.00000000e+00  3.74157745e-15 -8.23967291e-17  1.00000000e+00]]
```


Discussion / hints:
The inverse implementation in amgcl is a simple LU decomposition without pivoting:
https://github.com/ddemidov/amgcl/blob/master/amgcl/detail/inverse.hpp#L43.
And it looks like this particular matrix requires pivoting.

Fix patch:
diff --git a/amgcl/deflated_solver.hpp b/amgcl/deflated_solver.hpp
index 98c2e815..b97d417b 100644
--- a/amgcl/deflated_solver.hpp
+++ b/amgcl/deflated_solver.hpp
@@ -159,7 +159,8 @@ class deflated_solver : public amgcl::detail::non_copyable {
             }
 
             std::vector<scalar_type> t(E.size());
-            detail::inverse(prm.nvec, E.data(), t.data());
+            std::vector<int> p(prm.nvec);
+            detail::inverse(prm.nvec, E.data(), t.data(), p.data());
         }
 
         /** Computes the solution for the given system matrix \p A and the
diff --git a/amgcl/detail/inverse.hpp b/amgcl/detail/inverse.hpp
index 9f7adf59..12a2b14d 100644
--- a/amgcl/detail/inverse.hpp
+++ b/amgcl/detail/inverse.hpp
@@ -31,9 +31,10 @@ THE SOFTWARE.
  * \brief  Inverse of a dense matrix.
  */
 
-#include <vector>
 #include <algorithm>
 #include <cassert>
+#include <numeric>
+#include <utility>
 #include <amgcl/util.hpp>
 #include <amgcl/value_type/interface.hpp>
 
@@ -41,34 +42,54 @@ namespace amgcl {
 namespace detail {
 
     template <typename value_type>
-    static void inverse(int n, value_type *A, value_type *t) {
+    static void inverse(int n, value_type *A, value_type *t, int *p) {
+        std::iota(p, p + n, 0);
+
         // Perform LU-factorization of A in-place
-        for(int k = 0; k < n; ++k) {
-            value_type d = math::inverse(A[k*n+k]);
+        for(int col = 0; col < n; ++col) {
+            // Find pivot element
+            int pivot_i = col;
+            using mag_type = typename math::scalar_of<value_type>::type;
+            mag_type pivot_mag = math::zero<mag_type>();
+            for (int i = col; i < n; ++i) {
+                int row = p[i];
+                mag_type mag = math::norm(A[row*n+col]);
+                if (mag > pivot_mag) {
+                    pivot_mag = mag;
+                    pivot_i = i;
+                }
+            }
+            std::swap(p[col], p[pivot_i]);
+            int pivot_row = p[col];
+            // We have found pivot element, perform Gauss elimination
+            value_type d = math::inverse(A[pivot_row*n+col]);
             assert(!math::is_zero(d));
-            for(int i = k+1; i < n; ++i) {
-                A[i*n+k] *= d;
-                for(int j = k+1; j < n; ++j)
-                    A[i*n+j] -= A[i*n+k] * A[k*n+j];
+            for (int i = col+1; i < n; ++i) {
+                int row = p[i];
+                A[row*n+col] *= d;
+                for(int j = col+1; j < n; ++j)
+                    A[row*n+j] -= A[row*n+col] * A[pivot_row*n+j];
             }
-            A[k*n+k] = d;
+            A[pivot_row*n+col] = d;
         }
 
         // Invert identity matrix in-place to get the solution.
         for(int k = 0; k < n; ++k) {
             // Lower triangular solve:
             for(int i = 0; i < n; ++i) {
-                value_type b = (i == k) ? math::identity<value_type>() : math::zero<value_type>();
+                int row = p[i];
+                value_type b = (row == k) ? math::identity<value_type>() : math::zero<value_type>();
                 for(int j = 0; j < i; ++j)
-                    b -= A[i*n+j] * t[j*n+k];
+                    b -= A[row*n+j] * t[j*n+k];
                 t[i*n+k] = b;
             }
 
             // Upper triangular solve:
             for(int i = n; i --> 0; ) {
+                int row = p[i];
                 for(int j = i+1; j < n; ++j)
-                    t[i*n+k] -= A[i*n+j] * t[j*n+k];
-                t[i*n+k] *= A[i*n+i];
+                    t[i*n+k] -= A[row*n+j] * t[j*n+k];
+                t[i*n+k] *= A[row*n+i];
             }
         }
 
diff --git a/amgcl/value_type/static_matrix.hpp b/amgcl/value_type/static_matrix.hpp
index 7a59d64d..ad0f65a0 100644
--- a/amgcl/value_type/static_matrix.hpp
+++ b/amgcl/value_type/static_matrix.hpp
@@ -323,7 +323,8 @@ struct inverse_impl< static_matrix<T, N, N> >
 {
     static static_matrix<T, N, N> get(static_matrix<T, N, N> A) {
         std::array<T, N * N> buf;
-        detail::inverse(N, A.data(), buf.data());
+        std::array<int, N> p;
+        detail::inverse(N, A.data(), buf.data(), p.data());
         return A;
     }
 };


Test patch:
diff --git a/tests/test_static_matrix.cpp b/tests/test_static_matrix.cpp
index 3b3ba5c7..08081b74 100644
--- a/tests/test_static_matrix.cpp
+++ b/tests/test_static_matrix.cpp
@@ -59,4 +59,34 @@ BOOST_AUTO_TEST_CASE( inverse ) {
             BOOST_CHECK_SMALL(c(i,j) - (i == j), 1e-8);
 }
 
+BOOST_AUTO_TEST_CASE( inverse_pivoting ) {
+    amgcl::static_matrix<double, 4, 4> a {{
+    1, -0.1, -0.028644256, 0.25684664,
+    1, -0.1, -0.025972342, 0.25663863,
+    1, -0.095699158, -0.029327056, 0.25554974,
+    1, -0.09543351, -0.026189496, 0.25796741,
+    }};
+    amgcl::static_matrix<double, 4, 4> b = amgcl::math::inverse(a);
+    amgcl::static_matrix<double, 4, 4> c = b * a;
+
+    for(int i = 0; i < 4; ++i)
+        for(int j = 0; j < 4; ++j)
+            BOOST_CHECK_SMALL(c(i,j) - (i == j), 1e-8);
+}
+
+BOOST_AUTO_TEST_CASE( inverse_pivoting_2 ) {
+    amgcl::static_matrix<double, 4, 4> a {{
+    0, 1, 0, 0,
+    0, 0, 1, 0,
+    1, 0, 0, 0,
+    0, 0, 0, 1,
+    }};
+    amgcl::static_matrix<double, 4, 4> b = amgcl::math::inverse(a);
+    amgcl::static_matrix<double, 4, 4> c = b * a;
+
+    for(int i = 0; i < 4; ++i)
+        for(int j = 0; j < 4; ++j)
+            BOOST_CHECK_SMALL(c(i,j) - (i == j), 1e-8);
+}
+
 BOOST_AUTO_TEST_SUITE_END()