ガウス・ジョルダン法

参考
改訂第3版 C言語によるはじめてのアルゴリズム入門 
河西 朝雄 (著) p98

行列の基本操作は以下にまとめてあるが、この記事の下の方にもコピペした。
https://note.com/alchan/n/neaf2b8b9a9e7

        public static double[,] GaussianJordanEliminationSolve(double[,] A, double[] b)
       {
           int row_count = A.GetLength(0);//row
           int col_count = A.GetLength(1);//col

           var ex = AddCol(A, b);

           for (int pivot_index = 0; pivot_index < row_count; pivot_index++)
           {
               //行交換実行
               if (ex[pivot_index, pivot_index] == 0)
               {
                   _pivoting(ex, pivot_index);
               }

               //ピボット行のピボット要素を１にするため
               //ピボット行を全部ピボット要素で除算            
               var w = 1 / ex[pivot_index, pivot_index];
               for (int col_index = 0; col_index < col_count + 1; col_index++)
               {
                   ex[pivot_index, col_index] *= w;
               }

               //ピボット列のピボット行以外の項を上も下も0に掃き出す
               //ピボット列以降の項は修正される
               for (int row_index = 0; row_index < row_count; row_index++)
               {
                   //pivot行以外は死なす
                   if (row_index == pivot_index) { continue; }

                   //ピボット列のピボット行の項の以外の項
                   double leading_coefficient = ex[row_index, pivot_index];
                   //ピボット列より左は0になっているはず
                   for (int col_index = pivot_index; col_index < col_count + 1; col_index++)
                   {
                       ex[row_index, col_index] -= ex[pivot_index, col_index] * leading_coefficient;
                   }
               }
           }
           return ex;
       }

      public static double[,] AddCol(double[,] matrix, double[] new_col)
       {
           int row_count = matrix.GetLength(0);//rowの個数
           int col_count = matrix.GetLength(1);//colの個数

           double[,] ret = new double[row_count, col_count+1];

           for (int row_index = 0; row_index < row_count; row_index++)
           {
               for (int col_index = 0; col_index < col_count+1; col_index++)
               {
                   if (col_index == col_count)
                   {
                       ret[row_index, col_index] = new_col[row_index];
                   }
                   else
                   {
                       ret[row_index, col_index] = matrix[row_index, col_index];
                   }
               }
           }
           return ret;
       }

       public static int _pivoting(double[,] A, int pivot_index)
       {
           var abs_max_index = _get_pivoting_index(A, pivot_index);
           if (pivot_index == abs_max_index) { return 0; }

           if(ChangeRow(A, pivot_index, abs_max_index)==0)
           {
               //Changeしてない
               return 0;
           }
           else
           {
               //Changeした
               //pivot交換した
               return 1;
           }
       }
         public static int _get_pivoting_index(double[,] A, int pivot_index)
       {
           //pivot列の
           //pivot+1行以降で
           //絶対値最大の行のインデックスを返す

           int row_count = A.GetLength(0);//rowの個数

           double abs_max = 0;
           int abs_max_row_index = pivot_index;

           //pivotエレメントは最後の一つまで走査される場合と
           //一つ手前で止まる場合がある
           //if (A.GetLength(0) - 1 < abs_max_row_index) { abs_max_row_index = pivot_index; }

           for (int row_index = pivot_index; row_index < row_count; row_index++)
           {
               var temp = Math.Abs(A[row_index, pivot_index]);
               if (abs_max < temp)
               {
                   abs_max = temp;
                   abs_max_row_index = row_index;
               }
           }

           return abs_max_row_index;
       }