Line data    Source code

       1             : /*************************************************************************
       2             :  *
       3             :  *  Project
       4             :  *                         _____ _____  __  __ _____
       5             :  *                        / ____|  __ \|  \/  |  __ \
       6             :  *  ___  _ __   ___ _ __ | |  __| |__) | \  / | |__) |
       7             :  * / _ \| '_ \ / _ \ '_ \| | |_ |  ___/| |\/| |  ___/
       8             :  *| (_) | |_) |  __/ | | | |__| | |    | |  | | |
       9             :  * \___/| .__/ \___|_| |_|\_____|_|    |_|  |_|_|
      10             :  *      | |
      11             :  *      |_|
      12             :  *
      13             :  * Copyright (C) Akiel Aries, <akiel@akiel.org>, et al.
      14             :  *
      15             :  * This software is licensed as described in the file LICENSE, which
      16             :  * you should have received as part of this distribution. The terms
      17             :  * among other details are referenced in the official documentation
      18             :  * seen here : https://akielaries.github.io/openGPMP/ along with
      19             :  * important files seen in this project.
      20             :  *
      21             :  * You may opt to use, copy, modify, merge, publish, distribute
      22             :  * and/or sell copies of the Software, and permit persons to whom
      23             :  * the Software is furnished to do so, under the terms of the
      24             :  * LICENSE file. As this is an Open Source effort, all implementations
      25             :  * must be of the same methodology.
      26             :  *
      27             :  *
      28             :  *
      29             :  * This software is distributed on an AS IS basis, WITHOUT
      30             :  * WARRANTY OF ANY KIND, either express or implied.
      31             :  *
      32             :  ************************************************************************/
      33             : #include <cmath>
      34             : #include <cstdint>
      35             : #include <iostream>
      36             : #include <openGPMP/linalg/linsys.hpp>
      37             : #include <utility>
      38             : #include <vector>
      39             : 
      40          20 : gpmp::linalg::LinSys::LinSys(const std::vector<std::vector<double>> &mat)
      41          20 :     : matrix(mat) {
      42          20 :     num_rows = matrix.size();
      43          20 :     num_cols = matrix[0].size();
      44          20 : }
      45             : 
      46             : // display the augmented matrix
      47           0 : void gpmp::linalg::LinSys::display_mtx() const {
      48           0 :     for (int i = 0; i < num_rows; ++i) {
      49           0 :         for (int j = 0; j < num_cols; ++j) {
      50           0 :             std::cout << matrix[i][j] << " ";
      51             :         }
      52           0 :         std::cout << std::endl;
      53             :     }
      54           0 : }
      55             : 
      56           0 : void gpmp::linalg::LinSys::display(
      57             :     const std::vector<std::vector<double>> &mat) const {
      58           0 :     uint32_t rows = mat.size();
      59           0 :     uint32_t cols = mat[0].size();
      60             : 
      61           0 :     for (uint32_t i = 0; i < rows; ++i) {
      62           0 :         for (uint32_t j = 0; j < cols; ++j) {
      63           0 :             std::cout << mat[i][j] << " ";
      64             :         }
      65           0 :         std::cout << std::endl;
      66             :     }
      67           0 : }
      68             : 
      69             : // solve the linear system using Gaussian Elimination
      70           1 : std::vector<double> gpmp::linalg::LinSys::solve_gauss() {
      71             :     // temp matrix because our result is a vector of solutions
      72           1 :     std::vector<std::vector<double>> temp_mtx = matrix;
      73             : 
      74           3 :     for (int i = 0; i < num_rows - 1; ++i) {
      75             :         // perform partial pivot
      76           2 :         int pivotRow = i;
      77           5 :         for (int k = i + 1; k < num_rows; ++k) {
      78           3 :             if (std::abs(temp_mtx[k][i]) > std::abs(temp_mtx[pivotRow][i]))
      79           1 :                 pivotRow = k;
      80             :         }
      81           2 :         std::swap(temp_mtx[i], temp_mtx[pivotRow]);
      82             : 
      83             :         // eliminate entries below pivot
      84           5 :         for (int k = i + 1; k < num_rows; ++k) {
      85           3 :             double factor = temp_mtx[k][i] / temp_mtx[i][i];
      86          14 :             for (int j = i; j < num_cols; ++j) {
      87          11 :                 temp_mtx[k][j] -= factor * temp_mtx[i][j];
      88             :             }
      89             :         }
      90             :     }
      91             : 
      92             :     // back-substitution
      93           1 :     std::vector<double> solutions(num_rows, 0);
      94             : 
      95           4 :     for (int i = num_rows - 1; i >= 0; --i) {
      96           3 :         double sum = 0.0;
      97           6 :         for (int j = i + 1; j < num_cols - 1; ++j) {
      98           3 :             sum += temp_mtx[i][j] * solutions[j];
      99             :         }
     100           3 :         solutions[i] = (temp_mtx[i][num_cols - 1] - sum) / temp_mtx[i][i];
     101             :     }
     102             : 
     103           2 :     return solutions;
     104           1 : }
     105             : 
     106             : // calculate the determinant of the matrix
     107           7 : double gpmp::linalg::LinSys::determinant() const {
     108             :     // assuming square matrix
     109           7 :     if (num_rows != num_cols) {
     110           0 :         std::cerr << "Error: Determinant is undefined for non-square matrices."
     111           0 :                   << std::endl;
     112           0 :         return 0.0;
     113             :     }
     114             : 
     115           7 :     std::vector<std::vector<double>> temp_mtx = matrix;
     116           7 :     double det = 1.0;
     117             : 
     118          15 :     for (int i = 0; i < num_rows - 1; ++i) {
     119          20 :         for (int k = i + 1; k < num_rows; ++k) {
     120          12 :             double factor = temp_mtx[k][i] / temp_mtx[i][i];
     121          46 :             for (int j = i; j < num_cols; ++j) {
     122          34 :                 temp_mtx[k][j] -= factor * temp_mtx[i][j];
     123             :             }
     124             :         }
     125             :     }
     126             : 
     127          22 :     for (int i = 0; i < num_rows; ++i) {
     128          15 :         det *= temp_mtx[i][i];
     129             :     }
     130             : 
     131           7 :     return det;
     132           7 : }
     133             : 
     134             : // invert the matrix
     135           0 : void gpmp::linalg::LinSys::invert_mtx() {
     136             :     // assuming square matrix
     137           0 :     if (num_rows != num_cols) {
     138             :         std::cerr
     139           0 :             << "Error: Matrix inversion is undefined for non-square matrices."
     140           0 :             << std::endl;
     141           0 :         return;
     142             :     }
     143             : 
     144             :     std::vector<std::vector<double>> identity(
     145           0 :         num_rows,
     146           0 :         std::vector<double>(num_cols, 0.0));
     147             : 
     148             :     // augment the matrix with the identity matrix
     149           0 :     for (int i = 0; i < num_rows; ++i) {
     150           0 :         matrix[i].insert(matrix[i].end(),
     151           0 :                          identity[i].begin(),
     152           0 :                          identity[i].end());
     153             :     }
     154             : 
     155             :     // perform Gaussian elimination
     156           0 :     solve_gauss();
     157             : 
     158             :     // separate the inverse matrix
     159           0 :     for (int i = 0; i < num_rows; ++i) {
     160           0 :         matrix[i].erase(matrix[i].begin(), matrix[i].begin() + num_rows);
     161             :     }
     162           0 : }
     163             : 
     164             : std::pair<std::vector<std::vector<double>>, std::vector<std::vector<double>>>
     165           1 : gpmp::linalg::LinSys::lu_decomp() {
     166           1 :     std::vector<std::vector<double>> L(num_rows,
     167           2 :                                        std::vector<double>(num_cols, 0.0));
     168           1 :     std::vector<std::vector<double>> U(num_rows,
     169           2 :                                        std::vector<double>(num_cols, 0.0));
     170             : 
     171           5 :     for (int i = 0; i < num_rows; ++i) {
     172             :         // Set diagonal elements of L to 1
     173           4 :         L[i][i] = 1.0;
     174             : 
     175          14 :         for (int j = i; j < num_cols; ++j) {
     176          10 :             U[i][j] = matrix[i][j];
     177          20 :             for (int k = 0; k < i; ++k) {
     178          10 :                 U[i][j] -= L[i][k] * U[k][j];
     179             :             }
     180             :         }
     181             : 
     182          10 :         for (int j = i + 1; j < num_rows; ++j) {
     183           6 :             L[j][i] = matrix[j][i];
     184          10 :             for (int k = 0; k < i; ++k) {
     185           4 :                 L[j][i] -= L[j][k] * U[k][i];
     186             :             }
     187           6 :             L[j][i] /= U[i][i];
     188             :         }
     189             :     }
     190             : 
     191           2 :     return {L, U};
     192           1 : }
     193             : 
     194             : // solve the linear system using LU decomposition
     195           0 : void gpmp::linalg::LinSys::solve_lu() {
     196           0 :     lu_decomp();
     197             : 
     198             :     // forward substitution
     199           0 :     for (int i = 0; i < num_rows - 1; ++i) {
     200           0 :         for (int j = i + 1; j < num_rows; ++j) {
     201           0 :             matrix[j][num_cols - 1] -= matrix[j][i] * matrix[i][num_cols - 1];
     202             :         }
     203             :     }
     204             : 
     205             :     // back-substitution
     206           0 :     for (int i = num_rows - 1; i >= 0; --i) {
     207           0 :         for (int j = i + 1; j < num_cols - 1; ++j) {
     208           0 :             matrix[i][num_cols - 1] -= matrix[i][j] * matrix[j][num_cols - 1];
     209             :         }
     210           0 :         matrix[i][num_cols - 1] /= matrix[i][i];
     211             :     }
     212           0 : }
     213             : 
     214           0 : void gpmp::linalg::LinSys::solve_cholesky() {
     215             :     // assuming symmetric and positive-definite matrix
     216           0 :     if (!is_symmetric()) {
     217             :         std::cerr << "Error: Cholesky decomposition is only applicable to"
     218           0 :                      "symmetric positive-definite matrices\n";
     219             :     }
     220             : 
     221           0 :     for (int i = 0; i < num_rows; ++i) {
     222           0 :         for (int j = 0; j <= i; ++j) {
     223           0 :             if (i == j) {
     224           0 :                 double sum = 0.0;
     225           0 :                 for (int k = 0; k < j; ++k) {
     226           0 :                     sum += std::pow(matrix[j][k], 2);
     227             :                 }
     228           0 :                 matrix[j][j] = std::sqrt(matrix[j][j] - sum);
     229             :             }
     230             : 
     231             :             else {
     232           0 :                 double sum = 0.0;
     233           0 :                 for (int k = 0; k < j; ++k) {
     234           0 :                     sum += matrix[i][k] * matrix[j][k];
     235             :                 }
     236           0 :                 matrix[i][j] = (matrix[i][j] - sum) / matrix[j][j];
     237             :             }
     238             :         }
     239             :     }
     240             : 
     241             :     // forward substitution
     242           0 :     for (int i = 0; i < num_rows; ++i) {
     243           0 :         double sum = 0.0;
     244           0 :         for (int j = 0; j < i; ++j) {
     245           0 :             sum += matrix[i][j] * matrix[num_cols - 1][j];
     246             :         }
     247           0 :         matrix[num_cols - 1][i] =
     248           0 :             (matrix[num_cols - 1][i] - sum) / matrix[i][i];
     249             :     }
     250             : 
     251             :     // back-substitution
     252           0 :     for (int i = num_rows - 1; i >= 0; --i) {
     253           0 :         double sum = 0.0;
     254           0 :         for (int j = i + 1; j < num_cols - 1; ++j) {
     255           0 :             sum += matrix[j][i] * matrix[i][num_cols - 1];
     256             :         }
     257           0 :         matrix[i][num_cols - 1] =
     258           0 :             (matrix[i][num_cols - 1] - sum) / matrix[i][i];
     259             :     }
     260           0 : }
     261             : 
     262             : // solve the linear system using Jacobi iteration
     263           0 : void gpmp::linalg::LinSys::solve_jacobi(int maxIterations, double tolerance) {
     264           0 :     std::vector<std::vector<double>> x(num_rows, std::vector<double>(1, 0.0));
     265           0 :     std::vector<std::vector<double>> xOld = x;
     266             : 
     267           0 :     for (int iter = 0; iter < maxIterations; ++iter) {
     268           0 :         for (int i = 0; i < num_rows; ++i) {
     269           0 :             double sum = 0.0;
     270           0 :             for (int j = 0; j < num_cols - 1; ++j) {
     271           0 :                 if (j != i) {
     272           0 :                     sum += matrix[i][j] * xOld[j][0];
     273             :                 }
     274             :             }
     275           0 :             x[i][0] = (matrix[i][num_cols - 1] - sum) / matrix[i][i];
     276             :         }
     277             : 
     278             :         // check convergence
     279           0 :         double maxDiff = 0.0;
     280           0 :         for (int i = 0; i < num_rows; ++i) {
     281           0 :             double diff = std::abs(x[i][0] - xOld[i][0]);
     282           0 :             if (diff > maxDiff) {
     283           0 :                 maxDiff = diff;
     284             :             }
     285             :         }
     286             : 
     287             :         // converged
     288           0 :         if (maxDiff < tolerance) {
     289           0 :             break;
     290             :         }
     291             : 
     292           0 :         xOld = x;
     293             :     }
     294             : 
     295             :     // copy the solution back to the augmented matrix
     296           0 :     for (int i = 0; i < num_rows; ++i) {
     297           0 :         matrix[i][num_cols - 1] = x[i][0];
     298             :     }
     299           0 : }
     300             : 
     301             : // check if the matrix is symmetric and positive-definite
     302           2 : bool gpmp::linalg::LinSys::is_symmetric() const {
     303             :     // assuming square matrix
     304           2 :     if (num_rows != num_cols) {
     305           0 :         return false;
     306             :     }
     307             : 
     308             :     // check symmetry
     309           8 :     for (int i = 0; i < num_rows; ++i) {
     310          24 :         for (int j = 0; j < num_cols; ++j) {
     311             :             // if (matrix[i][j] != matrix[j][i]) {
     312          36 :             if (fabs(matrix[i][j] - matrix[j][i]) >
     313          18 :                 std::numeric_limits<double>::epsilon()) {
     314           0 :                 return false;
     315             :             }
     316             :         }
     317             :     }
     318             : 
     319             :     // check positive definiteness
     320           6 :     for (int i = 0; i < num_rows; ++i) {
     321           5 :         std::vector<std::vector<double>> submatrix(i + 1,
     322          10 :                                                    std::vector<double>(i + 1));
     323          14 :         for (int j = 0; j <= i; ++j) {
     324          28 :             for (int k = 0; k <= i; ++k) {
     325          19 :                 submatrix[j][k] = matrix[j][k];
     326             :             }
     327             :         }
     328             : 
     329           5 :         double det = LinSys(submatrix).determinant();
     330           5 :         if (det <= 0.0) {
     331           1 :             return false;
     332             :         }
     333           5 :     }
     334             : 
     335           1 :     return true;
     336             : }
     337             : 
     338           1 : double gpmp::linalg::LinSys::frobenius_norm() const {
     339           1 :     double sum = 0.0;
     340           4 :     for (int i = 0; i < num_rows; ++i) {
     341          12 :         for (int j = 0; j < num_cols; ++j) {
     342           9 :             sum += std::pow(matrix[i][j], 2);
     343             :         }
     344             :     }
     345           1 :     return std::sqrt(sum);
     346             : }
     347             : 
     348             : // calculate the 1-norm of the matrix
     349           1 : double gpmp::linalg::LinSys::one_norm() const {
     350           1 :     double maxColSum = 0.0;
     351           4 :     for (int j = 0; j < num_cols; ++j) {
     352           3 :         double colSum = 0.0;
     353          12 :         for (int i = 0; i < num_rows; ++i) {
     354           9 :             colSum += std::abs(matrix[i][j]);
     355             :         }
     356           3 :         maxColSum = std::max(maxColSum, colSum);
     357             :     }
     358           1 :     return maxColSum;
     359             : }
     360             : 
     361             : // calculate the infinity norm of the matrix
     362           1 : double gpmp::linalg::LinSys::inf_norm() const {
     363           1 :     double maxRowSum = 0.0;
     364           4 :     for (int i = 0; i < num_rows; ++i) {
     365           3 :         double rowSum = 0.0;
     366          12 :         for (int j = 0; j < num_cols; ++j) {
     367           9 :             rowSum += std::abs(matrix[i][j]);
     368             :         }
     369           3 :         maxRowSum = std::max(maxRowSum, rowSum);
     370             :     }
     371           1 :     return maxRowSum;
     372             : }
     373             : 
     374             : // perform Gram-Schmidt orthogonalization
     375           0 : void gpmp::linalg::LinSys::gram_schmidt() {
     376             :     std::vector<std::vector<double>> orthoBasis(
     377           0 :         num_rows,
     378           0 :         std::vector<double>(num_cols, 0.0));
     379             : 
     380           0 :     for (int j = 0; j < num_cols; ++j) {
     381           0 :         for (int i = 0; i < num_rows; ++i) {
     382           0 :             double projection = 0.0;
     383           0 :             for (int k = 0; k < i; ++k) {
     384           0 :                 projection += (matrix[i][j] * orthoBasis[k][j]) /
     385           0 :                               (orthoBasis[k][j] * orthoBasis[k][j]);
     386             :             }
     387           0 :             orthoBasis[i][j] = matrix[i][j] - projection;
     388             :         }
     389             :     }
     390             : 
     391             :     // Replace the matrix with the orthogonalized basis
     392           0 :     matrix = orthoBasis;
     393           0 : }
     394             : 
     395             : // check if the matrix is diagonally dominant
     396           2 : bool gpmp::linalg::LinSys::diagonally_dominant() const {
     397           5 :     for (int i = 0; i < num_rows; ++i) {
     398           4 :         double diagonalValue = std::abs(matrix[i][i]);
     399           4 :         double rowSum = 0.0;
     400          16 :         for (int j = 0; j < num_cols; ++j) {
     401          12 :             if (j != i) {
     402           8 :                 rowSum += std::abs(matrix[i][j]);
     403             :             }
     404             :         }
     405           4 :         if (diagonalValue <= rowSum) {
     406           1 :             return false;
     407             :         }
     408             :     }
     409           1 :     return true;
     410             : }
     411             : 
     412             : // check if the system is consistent
     413           2 : bool gpmp::linalg::LinSys::is_consistent() const {
     414           5 :     for (int i = 0; i < num_rows; ++i) {
     415           4 :         bool allZeros = true;
     416           7 :         for (int j = 0; j < num_cols - 1; ++j) {
     417           6 :             if (fabs(matrix[i][j]) > std::numeric_limits<double>::epsilon()) {
     418             : 
     419           3 :                 allZeros = false;
     420           3 :                 break;
     421             :             }
     422             :         }
     423           5 :         if (allZeros && fabs(matrix[i][num_cols - 1]) >
     424           1 :                             std::numeric_limits<double>::epsilon()) {
     425           1 :             return false;
     426             :         }
     427             :     }
     428           1 :     return true;
     429             : }
     430             : 
     431             : // check if the system is homogeneous
     432           2 : bool gpmp::linalg::LinSys::is_homogeneous() const {
     433           5 :     for (int i = 0; i < num_rows; ++i) {
     434           8 :         if (fabs(matrix[i][num_cols - 1]) >
     435           4 :             std::numeric_limits<double>::epsilon()) {
     436           1 :             return false;
     437             :         }
     438             :     }
     439           1 :     return true;
     440             : }