Merge pull request #1359 from PowerGridModel/feature/links-part-3

Merge links on nodes: Naive Gauss elimination and internal loads

Author: figueroa1395

power_grid_model_c/power_grid_model/include/power_grid_model/link_solver.hpp

@@ -319,4 +319,54 @@ inline std::vector<std::vector<DoubleComplex>> set_projection_system(Idx free_in
     return projection_system;
 };
 
+inline void naive_gauss_elimination(std::vector<std::vector<DoubleComplex>>& system) {
+    auto const system_size = narrow_cast<Idx>(std::ssize(system));
+
+    // we skip pivoting since the matrix system is mostly diagonally dominant
+    // and given the real power systems topology of super nodes, this should not introduce
+    // any numerical instabilities in practice
+
+    // forward elimination
+    for (Idx const column : IdxRange{system_size}) {
+        auto const pivot = system[column][column];
+        for (Idx const row : IdxRange{column + Idx{1}, system_size}) {
+            auto& row_col_value = system[row][column];
+            row_col_value /= -pivot;
+            for (Idx const column_part : IdxRange{column + Idx{1}, system_size + Idx{1}}) {
+                system[row][column_part] += row_col_value * system[column][column_part];
+            }
+        }
+    }
+
+    // backward substitution
+    system[system_size - 1][system_size] /= system[system_size - 1][system_size - 1];
+    for (Idx const row : IdxRange{system_size - 1} | std::views::reverse) {
+        auto element_sum = DoubleComplex{};
+        for (Idx const column : IdxRange{row + Idx{1}, system_size}) {
+            element_sum -= system[row][column] * system[column][system_size];
+        }
+        system[row][system_size] = (system[row][system_size] + element_sum) / system[row][row];
+    }
+};
+
+inline std::vector<DoubleComplex> compute_internal_loads(SolutionSet const& solution_set,
+                                                         std::vector<std::vector<DoubleComplex>> const& system) {
+    auto const number_of_rows = narrow_cast<Idx>(solution_set.extended_rhs.size());
+    auto const number_of_columns = narrow_cast<Idx>(system.size());
+    std::vector<DoubleComplex> internal_loads(number_of_rows);
+
+    for (auto const row : IdxRange{number_of_rows}) {
+        internal_loads[row] = solution_set.extended_rhs[row];
+        auto sum_value = DoubleComplex{};
+        for (auto const column : IdxRange{number_of_columns}) {
+            solution_set.dfs_matrix.get_value(row, column).transform([&sum_value, &system, column](IntS value) {
+                sum_value += static_cast<DoubleComplex>(value) * system[column].back();
+                return value;
+            });
+        }
+        internal_loads[row] -= sum_value;
+    }
+
+    return internal_loads;
+};
 } // namespace power_grid_model::link_solver::detail