求方阵逆

写了个求方阵逆的函数，因为不是方阵求的是伪逆，那个还要在追加代码有空再写。

需要我的matrixdet.h，不知道到我的优快云blog里找。我还没归类，所以需要的函数代码什么的到我的优快云blog自己找。

/***************************************************************************
  Copyright (C), ALL, Begtostudy. NERC. ZZU.
  File name:    matrix_invers.h
  Author:begtostudy Version:   Date: 2006.11.20
  Description:   
  Others:        
  Function List: 
  History:       
    1. Date:
       Author:
       Modification:
***************************************************************************/

//放在头文件matrix_invers.h里,今后我的文章里用的到，不要说你不知道
#include <math.h>
#include <memory.h>
#include <vector>
namespace jks
{
//////////////////////////////////////////////////////////////////////////

template<typename T>
double *ColPivot(T* a,T* b,int n,double* &x)
{
 int i,j,t,k;
 double p;
 if (x==NULL)
 {
  x=new double[n];
 }
#ifdef _WINBASE_ 
 else
  assert(!::IsBadWritePtr(x,n*sizeof(double)));
 assert(!::IsBadReadPtr(a,n*n*sizeof(T)));
 assert(!::IsBadReadPtr(b,n*sizeof(T)));
#endif

 T* c = new T[n*(n+1)];
 for (i=0;i<n;i++)
 {
  memcpy(c+i*(n+1),a+i*n,n*sizeof(T));
  *(c+i*(n+1)+n) = *(b+i);//!
 }
  
 for( i=0;i<=n-2;i++)
 {
  k=i;
  for(j=i+1;j<=n-1;j++)
   if(fabs(*(c+j*(n+1)+i))>(fabs(*(c+k*(n+1)+i))))
    k=j;
   if(k!=i)
    for( j=i;j<=n;j++ )
    {
     p=*(c+i*(n+1)+j);
     *(c+i*(n+1)+j)=*(c+k*(n+1)+j);
     *(c+k*(n+1)+j)=p;
    }
  for( j=i+1;j<=n-1;j++ )
  {
   p=(*(c+j*(n+1)+i))/(double)(*(c+i*(n+1)+i));
   for( t=i;t<=n;t++ )
    *(c+j*(n+1)+t)-=p*(*(c+i*(n+1)+t));
  }
 }
 for( i=n-1;i>=0;i--)
 {
  for( j=n-1;j>=i+1;j--)
   (*(c+i*(n+1)+n))-=x[j]*(*(c+i*(n+1)+j));
  x[i]=*(c+i*(n+1)+n)/(double)(*(c+i*(n+1)+i));
 }

 delete []c;
 
 return x;
}
template<class T>
void verticalcpy(T* toP,int toWidth,const T* fromP,int fromHeight,int fromWidth=1)
{
// assert(toP!=NULL);
// assert(fromP!=NULL);
// assert(fromHeight>0);

 if (fromWidth == toWidth)
 {
  std::copy(fromP, fromP + fromWidth * fromHeight, toP);
  return ;
 }
 
 int i,j;
 for (i=0;i<fromHeight;i++)
 {
  T* pt = toP+i*toWidth; 
  for (j=0;j<fromWidth;j++)
  {
   *pt++ = *fromP++;
  }
 }
}
template<typename T>
T* matix_reversal(T* p,int n)
{
// assert(!::IsBadReadPtr(p,n*n*sizeof(T)));

 T* pto = new T[n*n];
 int i;
 for (i=0;i<n;i++)
 {
  verticalcpy(pto+i,n,p+i*n,n);
 }

 memcpy(p,pto,n*n*sizeof(T)); 
   return p;

}
template<class T>
T* matrix_multiplication(T* lpA,T* lpB,T* lpC,
       unsigned short m,unsigned short s,unsigned short n)
{
// assert(lpC!=lpA && lpC!=lpB );
// assert(lpC!=NULL);
// assert(m!=0 && n!=0 && s!=0);
// assert(!::IsBadWritePtr(lpC,m*n*sizeof(T)));
// assert(!::IsBadReadPtr(lpA,m*s*sizeof(T)));
// assert(!::IsBadReadPtr(lpB,s*n*sizeof(T)));

 unsigned short i,j,k;
// for (i=0;i<m;i++)
//  for (j=0;j<n;j++)
//   *(lpC+i*n+j) = 0;
 memset(lpC,0,m*n*sizeof(T));

 for (i=0;i<m;i++)
 {
  for (j=0;j<n;j++)
  {
   for (k=0;k<s;k++)
   {
    *(lpC+i*n+j) += *(lpA+i*s+k) * *(lpB+k*n+j);
   // cerr<<*(lpA+i*s+k)<<'/t'<<*(lpB+k*n+j)<<endl;
   }
  }
 }

 return lpC;
}
inline double* matrix_inverse(double* p,int n,double *pto)
{
// assert(!::IsBadReadPtr(p,n*n*sizeof(double)));

 if (fabs(jks::matrix_det(p,n))<1e-20)
 {
  return NULL;
 }

 int i;

 if (pto==NULL)
 {
  pto = new double[n*n];
 }
// else
//  assert(!::IsBadWritePtr(pto,n*n*sizeof(double)));

 double *e = new double[n];

 for (i=0;i<n;i++)
 {
  memset(e,0,n*sizeof(double));
  e[i] = 1;
  double* t = pto+i*n;
  ColPivot(p,e,n,t);
 }
 delete []e;

 matix_reversal(p,n);

 return pto;
}
//////////////////////////////////////////////////////////////////////////
}

＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝示例

void test()
{
 double pp[3][3] = {     1 ,    2,     1,     1 ,   2   ,  2,     1 ,    4  ,   2 };
 double* p = pp[0];
// jks::Dump(p,3,3);

 double* tp = new double[3*3];
 if(jks::matrix_inverse(p,3,tp)==NULL)
 {
  cerr<<"erro!/n";
  return;
 }

// jks::Dump(tp,3,3);

 double* rp = new double[3*3];
 jks::matrix_multiplication(p,tp,rp,3,3,3);

// jks::Dump(rp,3,3);

 int i,j;
 for (i=0;i<3;i++)
 {
  for (j=0;j<3;j++)
  {
   cerr<<rp[i*3+j]<<' ';
  }cerr<<endl;
 }

delete[] rp;
}