全选主元高斯消去法 C++ 代码

上一篇博客的代码中用到了线性代数方程组的求解功能，当时用的是徐士良《C常用算法程序集》里面的全选主元高斯消去法的函数 cagaus 。

徐士良这本书写的还是很实用的，里面的代码都属于拿来就可以用的。但是有一个缺点就是代码没有注释，里面的变量名取的也很随意，经常是 i、j、k、a、b、c 一类的。所以里面的代码读起来挺费劲。趁着周末有点时间，我把徐士良书里的 cagaus 做了个整理，加上了必要的注释，变量命名也尽量的与变量的用途相关。

全选主元高斯消去法属于计算效率比较低的算法，但是鲁棒性非常好。通常用在规模比较小的线性代数方程组的计算上。

inline static void swapCols(double A[], int N, int col1, int col2)
{
    for (int row = 0; row <= N-1; row++)
    {
        int p = row * N + col1;
        int q = row * N + col2;
        double t = A[p];
        A[p] = A[q];
        A[q] = t;
    }
}

inline static void swap(double &a, double &b)
{
    double t = a;
    a = b;
    b = t;
}

/**
 * @brief cagaus 全选主元高斯消去法求解线性代数方程组 Ax = b
 * @note 这个代码改编自徐世良编著的《C常用算法程序集》
 * @param A[] 线性代数方程组的系数矩阵，要求系数矩阵是 n * n 方阵，行优先存储。
 * @param b[] n 个元素的数组。
 * @param perm[] 辅助数组，n 个元素。函数利用这个数组来记录选主元过程中的位置交换。
 * @param n 线性代数方程组的阶数。
 * @param x[] 计算的结果。
 * @return true 表示计算成功，false 则表明系数矩阵奇异
 */
bool cagaus(double A[],double b[], int perm[], int N, double x[])
{    
    for (int row = 0; row <= N - 2; row++) //依次找每一行的主元，这个主元不一定在当前行。
    {
        double pivot = 0.0; //主元
        int pivot_row; //主元所在的行
        for (int i = row; i <= N - 1; i++) //全选主元，即选择矩阵中最大的一个，转化为找最大值问题
        {
            for (int j = row; j <= N - 1; j++)
            {
                double t = fabs(A[i * N + j]); //当然是绝对值最大一个，这里用到了数学库绝对值函数fabs()
                if (t > pivot)
                {
                    pivot = t;
                    perm[row] = j; //第 row 行的主元所在的列
                    pivot_row = i; //主元所在的行
                }
            }
        }
        if (pivot + 1.0 == 1.0)
        {
            return false; //主元为 0， 说明是奇异矩阵
        }
        else
        {
            if (perm[row] != row) //主元不在当前列，所以要进行列交换
            {
                swapCols(A, N, row, perm[row]);
            }
            if (pivot_row != row) //主元不在当前行，所以要进行行交换
            {
                for (int j = row; j <= N-1; j++)
                {
                    int p = row * N + j;
                    int q = pivot_row * N + j;
                    swap(A[p], A[q]);
                }
                swap(b[row], b[pivot_row]);
            }
        }

        pivot = A[row * N + row]; //归一化 start
        for (int j = row + 1; j <= N-1; j++)
        {
            int p = row * N + j;
            A[p] = A[p] / pivot;
        }
        b[row] = b[row] / pivot; //归一化 end

        for (int i = row + 1; i <= N - 1; i++) //消元
        {
            for (int j = row + 1; j <= N - 1; j++)
            {
                int p = i * N + j;
                A[p] = A[p] - A[i * N + row] * A[row * N + j];
            }
            b[i] = b[i] - A[i * N + row] * b[row];
        }
    }
    double d = A[(N-1) * N + N - 1]; // 最后一行的主元
    if (d + 1.0 == 1.0)
    {
        return false; // 如果这个主元是 0，说明是奇异矩阵
    }
    x[N - 1] = b[N - 1] / d; //计算解向量的最后一个分量
    for (int i = N - 2; i >= 0; i--) //回代过程
    {
        double t = 0.0;
        for (int j = i + 1; j <= N - 1; j++)
        {
            t = t + A[i * N + j] * x[j];
        }
        x[i] = b[i] - t;
    }
    perm[N - 1] = N - 1;
    for (int k = N - 1; k >= 0; k--) //恢复解向量，交换位置的地方再交换回来
    {
        if (perm[k] != k)
        {
            swap(x[k], x[perm[k]]);
        }
    }
    return true;
}