Add dense inverse and its tests

power_grid_model_c/power_grid_model/include/power_grid_model/math_solver/sparse_lu_solver.hpp

@@ -71,7 +71,7 @@ template <rk2_tensor Matrix> class DenseLUFactor {
     // put permutation in block_perm in place
     // put pivot perturbation in has_pivot_perturbation in place
     template <class Derived>
-    static void factorize_block_in_place(Eigen::MatrixBase<Derived>&& matrix, BlockPerm& block_perm,
+    static void factorize_block_in_place(Eigen::MatrixBase<Derived>& matrix, BlockPerm& block_perm,
                                          double perturb_threshold, bool use_pivot_perturbation,
                                          bool& has_pivot_perturbation)
         requires(std::same_as<typename Derived::Scalar, Scalar> && rk2_tensor<Derived> &&
@@ -150,7 +150,66 @@ template <rk2_tensor Matrix> class DenseLUFactor {
                 throw SparseMatrixError{}; // can not specify error code
             }
         }
-        capturing::into_the_void(std::move(matrix));
+    }
+
+    // Forward substitution with the L matrix. The diagonal entries of L are implicit 1.0
+    // The rhs may be a vector or a matrix; matrix rhs columns are solved simultaneously.
+    template <class LUDerived, class RHSDerived>
+    static void forward_substitute_inplace(Eigen::MatrixBase<LUDerived> const& lu_matrix, RHSDerived& rhs)
+        requires(std::same_as<typename LUDerived::Scalar, Scalar> &&
+                 std::same_as<typename RHSDerived::Scalar, Scalar> && rk2_tensor<LUDerived> &&
+                 (LUDerived::RowsAtCompileTime == size) && (LUDerived::ColsAtCompileTime == size) &&
+                 (RHSDerived::RowsAtCompileTime == size))
+    {
+        for (int8_t row = 0; row < size; ++row) {
+            for (int8_t col = 0; col < row; ++col) {
+                rhs.row(row) -= lu_matrix(row, col) * rhs.row(col);
+            }
+        }
+    }
+
+    // Backward substitution with the U matrix stored in lu_matrix.
+    // The rhs may be a vector or a matrix; matrix rhs columns are solved simultaneously.
+    template <class LUDerived, class RHSDerived>
+    static void backward_substitute_inplace(Eigen::MatrixBase<LUDerived> const& lu_matrix, RHSDerived& rhs)
+        requires(std::same_as<typename LUDerived::Scalar, Scalar> &&
+                 std::same_as<typename RHSDerived::Scalar, Scalar> && rk2_tensor<LUDerived> &&
+                 (LUDerived::RowsAtCompileTime == size) && (LUDerived::ColsAtCompileTime == size) &&
+                 (RHSDerived::RowsAtCompileTime == size))
+    {
+        for (int8_t row = size - 1; row > -1; --row) {
+            for (int8_t col = size - 1; col > row; --col) {
+                rhs.row(row) -= lu_matrix(row, col) * rhs.row(col);
+            }
+            rhs.row(row) /= lu_matrix(row, row);
+        }
+    }
+
+    // given the factorized block satisfies L * U = P * A * Q
+    // compute the inverse of the factorized L * U * X = I
+    // returns X = (L * U)^-1 = (P * A * Q)^-1
+    template <class Derived>
+    static Matrix inverse_factorized_block(Eigen::MatrixBase<Derived> const& lu_matrix)
+        requires(std::same_as<typename Derived::Scalar, Scalar> && rk2_tensor<Derived> &&
+                 (Derived::RowsAtCompileTime == size) && (Derived::ColsAtCompileTime == size))
+    {
+        Matrix inverse = Matrix::Identity();
+        forward_substitute_inplace(lu_matrix, inverse);
+        backward_substitute_inplace(lu_matrix, inverse);
+        return inverse;
+    }
+
+    template <class Derived>
+    static Matrix dense_inverse(Eigen::MatrixBase<Derived> const& lu_matrix, BlockPerm const& block_perm)
+        requires(std::same_as<typename Derived::Scalar, Scalar> && rk2_tensor<Derived> &&
+                 (Derived::RowsAtCompileTime == size) && (Derived::ColsAtCompileTime == size))
+    {
+        // given the factorized block satisfies L * U = P * A * Q
+        // return A^-1 = Q * (L * U)^-1 * P
+        // lu_matrix is read-only: the packed L/U factor is preserved.
+        // inverse_factorized_block() performs in-place substitutions only on
+        // its local Identity() RHS, not on lu_matrix.
+        return block_perm.q * inverse_factorized_block(lu_matrix) * block_perm.p;
     }
 };
 
@@ -291,9 +350,9 @@ template <class Tensor, class RHSVector, class XVector> class SparseLUSolver {
                 if constexpr (is_block) {
                     // use machine precision by default
                     // record block permutation
-                    LUFactor::factorize_block_in_place(lu_matrix[pivot_idx].matrix(), block_perm_array[pivot_row_col],
-                                                       perturb_threshold, use_pivot_perturbation,
-                                                       has_pivot_perturbation_);
+                    auto pivot_matrix = lu_matrix[pivot_idx].matrix();
+                    LUFactor::factorize_block_in_place(pivot_matrix, block_perm_array[pivot_row_col], perturb_threshold,
+                                                       use_pivot_perturbation, has_pivot_perturbation_);
                     return block_perm_array[pivot_row_col];
                 } else {
                     if (use_pivot_perturbation) {
@@ -612,13 +671,8 @@ template <class Tensor, class RHSVector, class XVector> class SparseLUSolver {
             }
             // forward substitution inside block, for block matrix
             if constexpr (is_block) {
-                XVector& xb = x[row];
                 Tensor const& pivot = lu_matrix[diag_lu[row]];
-                for (Idx br = 0; br < block_size; ++br) {
-                    for (Idx bc = 0; bc < br; ++bc) {
-                        xb(br) -= pivot(br, bc) * xb(bc);
-                    }
-                }
+                LUFactor::forward_substitute_inplace(pivot.matrix(), x[row]);
             }
         }
 
@@ -635,14 +689,8 @@ template <class Tensor, class RHSVector, class XVector> class SparseLUSolver {
             // solve the diagonal pivot
             if constexpr (is_block) {
                 // backward substitution inside block
-                XVector& xb = x[row];
                 Tensor const& pivot = lu_matrix[diag_lu[row]];
-                for (Idx br = block_size - 1; br != -1; --br) {
-                    for (Idx bc = block_size - 1; bc > br; --bc) {
-                        xb(br) -= pivot(br, bc) * xb(bc);
-                    }
-                    xb(br) = xb(br) / pivot(br, br);
-                }
+                LUFactor::backward_substitute_inplace(pivot.matrix(), x[row]);
             } else {
                 x[row] = x[row] / lu_matrix[diag_lu[row]];
             }

tests/cpp_unit_tests/math_solver/test_sparse_lu_solver.cpp

@@ -39,11 +39,66 @@ template <class T> void check_result(std::vector<T> const& x, std::vector<T> con
     }
 }
 
+template <class MatrixActual, class MatrixExpected>
+void check_matrix_result(MatrixActual const& actual, MatrixExpected const& expected) {
+    REQUIRE(actual.rows() == expected.rows());
+    REQUIRE(actual.cols() == expected.cols());
+    CHECK((actual - expected).cwiseAbs().maxCoeff() < numerical_tolerance);
+}
+
+using Matrix3 = Eigen::Matrix<double, 3, 3, Eigen::ColMajor>;
+using RowMajorMatrix3 = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>;
+
+std::vector<double> scalar_lu_test_data() {
+    // [4 1 5
+    //  3 7 f
+    //  2 f 6]
+    return {
+        4.0, 1.0, 5.0, // row 0
+        3.0, 7.0, 0.0, // row 1
+        2.0, 0.0, 6.0  // row 2
+    };
+}
+
+Matrix3 scalar_lu_test_matrix() {
+    auto const data = scalar_lu_test_data();
+    Eigen::Map<RowMajorMatrix3 const> const matrix{data.data()};
+    return matrix;
+}
+
 // test block calculation with 2*2
 using Tensor = Eigen::Array<double, 2, 2, Eigen::ColMajor>;
 using Array = Eigen::Array<double, 2, 1, Eigen::ColMajor>;
 } // namespace
 
+TEST_CASE("Dense LU factor") {
+    SUBCASE("Dense inverse") {
+        using LUFactor = DenseLUFactor<Matrix3>;
+
+        Matrix3 const matrix = scalar_lu_test_matrix();
+        Matrix3 const expected_inverse = (Matrix3{
+                                              {42.0, -6.0, -35.0},
+                                              {-18.0, 14.0, 15.0},
+                                              {-14.0, 2.0, 25.0},
+                                          } /
+                                          80.0); // cofactor matrix divided by determinant
+        Matrix3 lu_matrix = matrix;
+        LUFactor::BlockPerm block_perm{};
+        bool const use_pivot_perturbation = false;
+        bool has_pivot_perturbation = false;
+
+        LUFactor::factorize_block_in_place(lu_matrix, block_perm, epsilon, use_pivot_perturbation,
+                                           has_pivot_perturbation);
+        Matrix3 const factorized_lu_matrix = lu_matrix;
+        Matrix3 const inverse = LUFactor::dense_inverse(lu_matrix, block_perm);
+
+        CHECK(has_pivot_perturbation == false);
+        check_matrix_result(lu_matrix, factorized_lu_matrix);
+        check_matrix_result(inverse, expected_inverse);
+        check_matrix_result(matrix * inverse, Matrix3::Identity());
+    }
+}
+
 TEST_CASE("Test Sparse LU solver") {
     // 3 * 3 matrix, with diagonal, two fill-ins
     /// x x x
@@ -58,11 +113,7 @@ TEST_CASE("Test Sparse LU solver") {
         // [4 1 5        3          21
         //  3 7 f     * [-1]   =  [ 2 ]
         //  2 f 6]       2          18
-        std::vector<double> data = {
-            4, 1, 5, // row 0
-            3, 7, 0, // row 1
-            2, 0, 6  // row 2
-        };
+        std::vector<double> data = scalar_lu_test_data();
         std::vector<double> const rhs = {21, 2, 18};
         std::vector<double> const x_ref = {3, -1, 2};
         std::vector<double> x(3, 0.0);