C++ source code to solve simultaneous equations with Gauss elimination method(pivot).

gauss_elimination_pivot.cpp

/***********************************************************
  連立方程式の解法 ( ガウスの消去法（ピボット選択） )

    DATE        AUTHOR       VERSION
    2021.09.16  mk-mode.com  1.00 新規作成

  Copyright(C) 2021 mk-mode.com All Rights Reserved.
***********************************************************/
#include <cmath>     // for fabs
#include <cstdlib>   // for EXIT_XXXX
#include <iostream>
#include <vector>

namespace gauss_elimination_pivot {

/*
 * @brief      ピボット選択
 *
 * @param[ref] 行列（配列） mtx (vector<vector<double>>)
 * @param[in]  対象行
 * @return     true|false
 */
bool pivot(std::vector<std::vector<double>>& mtx, unsigned int k) {
  unsigned int n;      // 行数
  unsigned int pv;     // ピボット行
  unsigned int i;      // loop index
  double       v_max;  // k 列絶対値の最大値

  try {
    n = mtx.size();
    pv = k;
    v_max = std::fabs(mtx[k][k]);
    for (i = k + 1; i < n; ++i) {
      if (std::fabs(mtx[i][k]) > v_max) {
        pv = i;
        v_max = std::fabs(mtx[i][k]);
      }
    }
    if (k != pv) { swap(mtx[k], mtx[pv]); }
  } catch (...) {
    return false;
  }

  return true;
}

/*
 * @brief      ガウスの消去法
 *             * 値が NaN の場合は 0 とする
 *
 * @param[ref] 行列（配列） mtx (vector<vector<double>>)
 * @return     true|false
 */
bool solve_ge(std::vector<std::vector<double>>& mtx) {
  int i;     // loop インデックス
  int j;     // loop インデックス
  int k;     // loop インデックス
  int n;     // 元（行）の数
  double d;  // 計算用

  try {
    n = (int)mtx.size();

    // 前進消去
    for (k = 0; k < n - 1; ++k) {
      if (!pivot(mtx, k)) {
        std::cout << "[ERROR] Failed to pivot!" << std::endl;
        return false;
      }
      for (i = k + 1; i < n; ++i) {
        d = mtx[i][k] / mtx[k][k];
        for (j = k + 1; j <= n; ++j)
          mtx[i][j] -= mtx[k][j] * d;
      }
    }

    // 後退代入
    for (i = n - 1; i >= 0; --i) {
      d = mtx[i][n];
      for (j = i + 1; j < n; ++j)
        d -= mtx[i][j] * mtx[j][n];
      mtx[i][n] = d / mtx[i][i];
      //if (std::isnan(mtx[i][n])) { mtx[i][n] = 0.0;}
    }
  } catch (...) {
    return false;
  }

  return true;
}

}  // namespace gauss_elimination_pivot

int main(int argc, char* argv[]) {
  namespace ns = gauss_elimination_pivot;
  std::vector<std::vector<double>> mtx;  // 計算用行列
  // テストデータ（右端は b, その他左側正方行列は a）
  // * 3x3; 対角成分が 0 にならない例
  //mtx.push_back({ 2.0, -3.0,  1.0,  5.0});
  //mtx.push_back({ 1.0,  1.0, -1.0,  2.0});
  //mtx.push_back({ 3.0,  5.0, -7.0,  0.0});
  // * 4x4; 対角成分が 0 にならない例
  //mtx.push_back({ 1.0, -2.0,  3.0, -4.0,  5.0});
  //mtx.push_back({-2.0,  5.0,  8.0, -3.0,  9.0});
  //mtx.push_back({ 5.0,  4.0,  7.0,  1.0, -1.0});
  //mtx.push_back({ 9.0,  7.0,  3.0,  5.0,  4.0});
  // * 4x4; 対角成分が 0 になる例
  mtx.push_back({1.0, 2.0, 7.0, 6.0,  6.0});
  mtx.push_back({2.0, 4.0, 4.0, 2.0,  2.0});
  mtx.push_back({1.0, 8.0, 5.0, 2.0, 12.0});
  mtx.push_back({2.0, 4.0, 3.0, 3.0,  5.0});

  try {
    // 元行列確認
    for (auto &r: mtx) {
      for (auto &c: r) { std::cout << c << " "; }
      std::cout << std::endl;
    }

    // 計算（ガウスの消去法（ピボット選択））
    if (!ns::solve_ge(mtx)) {
      std::cout << "[ERROR] Failed to solve by the Gauss-Elimination(Pivot) method!"
                << std::endl;
      return EXIT_SUCCESS;
    }

    // 計算結果確認
    std::cout << "---" << std::endl;
    for (auto &r: mtx) {
      for (auto &c: r) { std::cout << c << " "; }
      std::cout << std::endl;
    }
  } catch (...) {
      std::cerr << "EXCEPTION!" << std::endl;
      return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}