【c++】高斯-约旦法求逆矩阵

void inverse(double A[N][N], double k, double inv_A[N][N]) {
    double t[N][N]{};
    int max{ 0 };
    double temp{ 0.0 };
    for (int i = 0;i != k;++i) {
        for (int j = 0;j != k;++j) {
            t[i][j] = A[i][j];
        }
    }
    //show(t,N,N);
    for (int i = 0;i != k;++i) {
        for (int j = 0;j != k;++j) {
            inv_A[i][j] = (i == j) ? (double)1 : 0;
        }
    }
    //show(inv_A, k, k);
    for (int col = 0;col != k;++col) {
        //寻找一列中最大的数
        max = col;
        for (int i = col+1;i != k;++i) {
            if (fabs(t[i][col]) > fabs(t[col][col])) {
                max = i;
            }
        }
       // std::cout << "max: " << max << '\n';
        //换行
        temp = 0.0;
        if(max!=col){
            for (int row = 0;row != k;++row) {
                temp = t[max][row];
                t[max][row] = t[col][row];
                t[col][row] = temp;
                temp = inv_A[max][row];
                inv_A[max][row] = inv_A[col][row];
                inv_A[col][row] = temp;
            }
        }
        std::cout << "inv: \n";
        show(inv_A, k, k);
        //将对角线元素先化为1
        temp = t[col][col];
        for (int i = 0;i != k;++i) {
            t[col][i] /= temp;
            inv_A[col][i] /= temp;
        }

        //消元，和高斯法解方程不同，这里的消元还要使对角线上方的元素为0
        for (int row = 0;row != k;++row) {
            if (row != col) {
                temp = t[row][col];
                for (int j = 0;j != k;++j) {
                    t[row][j] -= temp*t[col][j];
                    inv_A[row][j] -= temp*inv_A[col][j];
                }
            }
            else {
                continue;
            }
        }
       /*std::cout << "col: \n";
        show(inv_A, k, k);*/ 
    }
}

vec inverse(vec A) {
    int row{ static_cast<int> (A.size()) };
    int col{ static_cast<int>(A[0].size()) };
    vec B(row, vecRow(col));
    int point;
    double temp,m;
    //构建单位化矩阵
    for (int i = 0;i != row;++i) {
        for (int j = 0;j != col;++j) {
            if (i == j) {
                B[i][j] = 1.0;
            }
            else {
                B[i][j] = 0.0;
            }
        }
    }
    //一列一列来
    for (int k = 0;k != col;++k) {
        //选主元
        point = k;
        //寻找k列中绝对值最大的数的行point
        for (int r = k+1;r != row;++r) {
            if (fabs(A[point][k]) < fabs(A[r][k])) {
                point = r;
            }
        }
        //判断A是否为奇异矩阵
        //assert(fabs(A[point][k]) > 1e-6 && "A为奇异矩阵");
        //换行
        if(k!=point){
            for (int i = 0;i != col;++i) {
                temp = A[point][i];
                A[point][i] = A[k][i];
                A[k][i] = temp;
                temp = B[point][i];
                B[point][i] = B[k][i];
                B[k][i] = temp;
            }
        }
        assert(fabs(A[k][k]) > 1e-6 && "奇异矩阵");
        //将主对角线上的元素单位化
        temp = A[k][k];
        for (int i = 0;i != row;i++) {
            A[k][i] /= temp;
            B[k][i] /= temp;
        }
        //消去
        m = 0.0;
        for (int i = 0;i != row;++i) {
            if (i != k) {
                m = A[i][k];
                for (int j = 0;j != col;++j) {
                    A[i][j] -= m * A[k][j];
                    B[i][j] -= m * B[k][j];
                }
            }
        }
    }return B;
}

int main()
{
    vec A{ {0.012,0.010,0.167},
           {1.000,0.833,5.910},
           {3200 ,1200 ,4.200} };
    vecRow B{ 0.6781,12.1,981 };
    vecRow x;
    /*x = equationSolver(A, B);
    for (double i : x) {
        std::cout << i << "  ";
    }
    std::cout << "A:\n";
    show(A);*/
    vec invA;
    invA = inverse(A);
    std::cout << "A的逆矩阵：\n";
    show(invA);
    vec mul;
    mul = Multi2(A, invA);
    std::cout << "mul:\n";
    show(mul);

}