c++ 矩阵求逆

//矩阵乘法
void pbgMatricMul(zdouble_t C[DN * DN], double A[DN * DN],double B[DN * DN])
{
    for (int32_t i = 0; i < DN; i++)
    {
        for (int32_t j = 0; j < DN; j++)
        {
            for (int32_t k = 0; k < DN; k++)
            {
                C[i * DN + j] += A[ i * DN + k] * B[k * DN + j];
            }
        }
    }

    //若绝对值小于10^-5,则置为0
    for (int32_t i = 0; i < DN * DN ; i++)
    {
        if (abs(C[i]) < 1e-5)
        {
            C[i] = 0;
        }
    }
}

//LUP分解
void CBackgroundSubtract::pbgLupDescomposition(zdouble_t A[DN * DN], zdouble_t L[DN * DN], zdouble_t U[DN * DN], int32_t P[DN])
{
    int32_t row = 0;
    for (int32_t i = 0;i < DN; i++)
    {
        P[i] = i;
    }
    for (int32_t i = 0; i < DN - 1;i++)
    {
        zdouble_t p = 0.0;
        for (int32_t j = i; j < DN; j++)
        {
            if (abs( A[j * DN + i]) > p)
            {
                p   = abs(A[j * DN + i]);
                row = j;
            }
        }
        if(0 == p)
        {
            //cout<< "矩阵奇异，无法计算逆" <<endl;
            return ;
        }

        //交换P[i]和P[row]
        int32_t tmp = P[i];
        P[i]        = P[row];
        P[row]      = tmp;

        zdouble_t tmp2 = 0.0;
        for (int32_t j = 0; j < DN; j++)
        {
            //交换A[i][j]和 A[row][j]
            tmp2 = A[i * DN + j];
            A[i * DN + j]   = A[row * DN + j];
            A[row * DN + j] = tmp2;
        }

        //以下同LU分解
        zdouble_t u = A[i * DN + i], l = 0.0;
        for (int32_t j = i + 1; j < DN; j++)
        {
            l = A[j * DN + i] / u;
            A[j * DN + i] = l;
            for (int32_t k = i + 1; k < DN; k ++)
            {
                A[j * DN + k] = A[j * DN + k] - A[i * DN + k] * l;
            }
        }
    }

    //构造L和U
    for (int32_t i = 0; i < DN; i++)
    {
        for (int32_t j = 0; j <= i; j++)
        {
            if (i != j)
            {
                L[i * DN + j] = A[i * DN + j];
            }
            else
            {
                L[i * DN + j] = 1;
            }
        }
        for (int32_t k = i; k < DN; k++)
        {
            U[i * DN + k] = A[i * DN + k];
        }
    }
}

//LUP求解方程
zdouble_t * pbgLupSolve(zdouble_t L[DN * DN], zdouble_t U[DN * DN], int P[DN], zdouble_t b[DN])
{
    zdouble_t *x=new double[DN]();
    zdouble_t *y=new double[DN]();

    //正向替换
    for (int32_t i = 0;i < DN;i++)
    {
        y[i] = b[P[i]];
        for (int32_t j = 0;j < i;j++)
        {
            y[i] = y[i] - L[i * DN + j] * y[j];
        }
    }
    //反向替换
    for (int32_t i = DN-1;i >= 0; i--)
    {
        x[i] = y[i];
        for (int32_t j = DN-1;j > i;j--)
        {
            x[i] = x[i] - U[i * DN + j] * x[j];
        }
        x[i] /= U[i * DN + i];
    }
    return x;
}

/*****************矩阵原地转置BEGIN********************/

/* 后继 */
int32_t CBackgroundSubtract::pbgGetNext(int32_t i, int32_t m, int32_t n)
{
    return (i % n) * m + i / n;
}

/* 前驱 */
int32_t CBackgroundSubtract::pbgGetPre(int32_t i, int32_t m, int32_t n)
{
    return (i % m) * n + i / m;
}

/* 处理以下标i为起点的环 */
void pbgMovedata(zdouble_t *mtx, int32_t i, int32_t m, int32_t n)
{
    zdouble_t temp = mtx[i]; // 暂存
    int32_t   cur = i;    // 当前下标
    int32_t   pre = pbgGetPre(cur, m, n);
    while(pre != i)
    {
        mtx[cur] = mtx[pre];
        cur = pre;
        pre = pbgGetPre(cur, m, n);
    }
    mtx[cur] = temp;
}

/* 转置，即循环处理所有环 */
void pbgTranspose(zdouble_t *mtx, int32_t m, int32_t n)
{
    for (int32_t i =0; i < m * n; ++i)
    {
        int32_t next = pbgGetNext(i, m, n);

        while(next > i) // 若存在后继小于i说明重复,就不进行下去了（只有不重复时进入while循环）
        {
            next = pbgGetNext(next, m, n);
        }
        if(next == i)  // 处理当前环
        {
            pbgMovedata(mtx, i, m, n);
        }
    }
}
/*****************矩阵原地转置END********************/

//LUP求逆(将每列b求出的各列x进行组装)
void pbgLupSolveInverse(zdouble_t inv_A[DN * DN], zdouble_t A[DN * DN])
{
    //创建矩阵A的副本，注意不能直接用A计算，因为LUP分解算法已将其改变
    zdouble_t *A_mirror   = new zdouble_t[DN * DN]();
    //zdouble_t *inv_A      = new zdouble_t[DN * DN]();//最终的逆矩阵（还需要转置）
    zdouble_t *inv_A_each = new zdouble_t[DN]();//矩阵逆的各列
    //double *B    =new double[DN*DN]();
    zdouble_t *b          =new zdouble_t[DN]();//b阵为B阵的列矩阵分量

    for(int32_t i = 0; i < DN; i++)
    {
        zdouble_t *L = new zdouble_t[DN * DN]();
        zdouble_t *U = new zdouble_t[DN * DN]();
        int32_t   *P = new int32_t[DN]();

        //构造单位阵的每一列
        for(int32_t j = 0; j < DN; j++)
        {
            b[j] = 0;
        }
        b[i] = 1;

        //每次都需要重新将A复制一份
        for(int32_t j = 0; j < DN * DN; j++)
        {
            A_mirror[j] = A[j];
        }

        pbgLupDescomposition(A_mirror, L, U, P);

        inv_A_each = pbgLupSolve (L, U, P, b);
        memcpy(inv_A + i * DN, inv_A_each, DN * sizeof(zdouble_t));//将各列拼接起来
    }
    pbgTranspose(inv_A, DN, DN);//由于现在根据每列b算出的x按行存储，因此需转置

   // return inv_A;
}