高斯消元法求线性方程组

#define SWAP(x,y) if((x)!=(y)) /
    { x = x+y; /
     y = x-y; /
      x = x-y; }

//  gauss-jordan消元法解线性方程组Ax = B
//  参数
//  a -- 输入系数矩阵，输出被其逆矩阵代替(n*n)
//      b -- 等号右边的矩阵B， 输出时被对应解向量代替(n*m)
//  n --  a的维数
//
/
void gauss_jordan(INOUT LM_REAL *a, INOUT LM_REAL *b, int n)
{
 int i,j,k,l,ll;
 int icol, irow;
 LM_REAL big, dum, pivinv;
 LM_REAL *inva = NULL;
 int m = 1;           //b为n*1
 int *ipiv = NULL;

 ipiv = (int*)malloc(n*sizeof(int));
 inva = (LM_REAL*)malloc(n*n*sizeof(LM_REAL));

 for(j=0; j<n; j++)
 {
  ipiv[j] = 0;
 }
 SetEye(inva,n);
 
 for(i=0; i<n; i++)
 {
  big = 0.0;
  for(j=0; j<n; j++)     //寻找主元(最大数)
  {
   if( ipiv[j] != 1)
    for(k=0; k<n; k++)
    {
     if(ipiv[k] == 0)
     {
      if(fabs(a[k+j*n]) >= big)
      {
       big = fabs( a[k+j*n] );
       irow = j;
       icol = k;
      }
     }//end if ipiv
    }
  }//end for j
  ++(ipiv[icol]);

  //实施消元
  if(irow != icol)
  {
   SWAP(b[irow], b[icol] );
   for(l=0; l<n; l++)
   {
    SWAP(a[l+irow*n], a[l+icol*n]);
    SWAP(inva[l+irow*n], inva[l+icol*n]);
   }
  }

  if (fabs(a[icol+icol*n]) < EPSILON)
   return;                    //error 奇异矩阵
  pivinv = 1.0 / a[icol+icol*n];
  b[icol] *= pivinv;
  for(l=0; l<n; l++)
  {
   a[l+icol*n] *= pivinv;
   inva[l+icol*n] *= pivinv;
  }
  for(ll=0; ll<n; ll++)
  {
   if(ll != icol)
   {
    dum = a[icol+ll*n];
    b[ll] -= b[icol] * dum;
    for(l=0; l<n; l++)
    {
     a[l+ll*n] -= a[l+icol*n] * dum;
     inva[l+ll*n] -= inva[l+icol*n] * dum;
    }
   }
  }//end for
 }//end for whole

 //更新a为逆矩阵
 for(i=0; i<n*n; i++)
  a[i] = inva[i];

 free(ipiv);
 free(inva);
}

//设置单位矩阵
void SetEye(LM_REAL *mat, int n)
{
 int i,j;
 for(i=0; i<n; i++)
 for(j=0; j<n; j++)
 {
  if(i==j)
   mat[j+i*n] = 1.0;
  else
   mat[j+i*n] = 0.0;
 }
}