矩阵运算

#include "stdafx.h"
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include "MatrixAlgo.h"

//对称正定矩阵的求逆
int bssgj(double a[], int n)
{
 int i,j,k,m;
 double w,g,*b;
 b=(double *)malloc(n*sizeof(double));
 for(k=0; k<=n-1; k++)
 {
  w=a[0];
  if(fabs(w)+1.0==1.0)
  {
   free(b);
   printf("fail\n");
   return(-2);
  }
  m=n-k-1;
  for(i=1; i<=n-1; i++)
  {
   g=a[i*n];
   b[i]=g/w;
   if (i<=m)
    b[i]=-b[i];
   for (j=1; j<=i; j++)
    a[(i-1)*n+j-1]=a[i*n+j]+g*b[j];
  }
  a[n*n-1]=1.0/w;
  for (i=1; i<=n-1; i++)
   a[(n-1)*n+i-1]=b[i];
 }
 for (i=0; i<=n-2; i++)
  for (j=i+1; j<=n-1; j++)
   a[i*n+j]=a[j*n+i];
 free(b);
 return(2);
}

//求矩阵秩的全选主元高斯消去法
int brank(double a[], int m, int n)
{
 int i,j,k,nn,is,js,l,ll,u,v;
 double q,d;
 nn=m;
 if(m>=n)
  nn=n;
 k=0;
 for (l=0; l<=nn-1; l++)
 {
  q=0.0;
  for (i=l; i<=m-1; i++)
   for (j=l; j<=n-1; j++)
   {
    ll=i*n+j;
    d=fabs(a[ll]);
    if (d>q)
    {
     q=d;
     is=i;
     js=j;
    }
   }
  if (q+1.0==1.0)
   return(k);
  k=k+1;
  if (is!=l)
  {
   for (j=l; j<=n-1; j++)
   {
    u=l*n+j;
    v=is*n+j;
    d=a[u];
    a[u]=a[v];
    a[v]=d;
   }
  }
  if (js!=l)
  {
   for (i=l; i<=m-1; i++)
   {
    u=i*n+js;
    v=i*n+l;
    d=a[u];
    a[u]=a[v];
    a[v]=d;
   }
  }
  ll=l*n+l;
  for (i=l+1; i<=n-1; i++)
  {
   d=a[i*n+l]/a[ll];
   for (j=l+1; j<=n-1; j++)
   {
    u=i*n+j;
    a[u]=a[u]-d*a[l*n+j];
   }
  }
 }
 return(k);
}

//求广义逆的奇异值分解法
int bginv(double a[], int m, int n, double aa[], double eps, double u[], double v[], int ka)
{
 int i,j,k,l,t,p,q,f;
 i=bmuav(a,m,n,u,v,eps,ka);
 if (i<0)
  return(-1);
 j=n;
 if (m<n)
  j=m;
 j=j-1;
 k=0;
 while ((k<=j)&&(a[k*n+k]!=0.0))
  k=k+1;
 k=k-1;
 for (i=0; i<=n-1; i++)
  for (j=0; j<=m-1; j++)
  {
   t=i*m+j;
   aa[t]=0.0;
   for (l=0; l<=k; l++)
   {
    f=l*n+i;
    p=j*m+l;
    q=l*n+l;
    aa[t]=aa[t]+v[f]*u[p]/a[q];
   }
  }
 return(1);
}

//求行列式值的全选主元高斯消去法
double bsdet(double a[], int n)
{
 int i,j,k,is,js,l,u,v;
 double f,det,q,d;
 f=1.0;
 det=1.0;
 for (k=0; k<=n-2; k++)
 {
  q=0.0;
  for (i=k; i<=n-1; i++)
   for (j=k; j<=n-1; j++)
   {
    l=i*n+j;
    d=fabs(a[l]);
    if (d>q)
    {
     q=d;
     is=i;
     js=j;
    }
   }
  if (q+1.0==1.0)
  {
   det=0.0;
   return(det);
  }
  if (is!=k)
  {
   f=-f;
   for (j=k; j<=n-1; j++)
   {
    u=k*n+j;
    v=is*n+j;
    d=a[u];
    a[u]=a[v];
    a[v]=d;
   }
  }
  if (js!=k)
  {
   f=-f;
   for (i=k; i<=n-1; i++)
   {
    u=i*n+js;
    v=i*n+k;
    d=a[u];
    a[u]=a[v];
    a[v]=d;
   }
  }
  l=k*n+k;
  det=det*a[l];
  for (i=k+1; i<=n-1; i++)
  {
   d=a[i*n+k]/a[l];
   for (j=k+1; j<=n-1; j++)
   {
    u=i*n+j;
    a[u]=a[u]-d*a[k*n+j];
   }
  }
 }
 det=f*det*a[n*n-1];
 return(det);
}

//一般实矩阵的奇异值分解
int bmuav(double a[], int m, int n, double u[], double v[], double eps, int ka)
{
 int i,j,k,l,it,ll,kk,ix,iy,mm,nn,iz,m1,ks;
 double d,dd,t,sm,sm1,em1,sk,ek,b,c,shh,fg[2],cs[2];
 double *s,*e,*w;
 s=(double *)malloc(ka*sizeof(double));
 e=(double *)malloc(ka*sizeof(double));
 w=(double *)malloc(ka*sizeof(double));
 it=60;
 k=n;
 if (m-1<n)
  k=m-1;
 l=m;
 if (n-2<m)
  l=n-2;
 if (l<0)
  l=0;
 ll=k;
 if (l>k)
  ll=l;
 if (ll>=1)
 {
  for (kk=1; kk<=ll; kk++)
  {
   if (kk<=k)
   {
    d=0.0;
    for (i=kk; i<=m; i++)
    {
     ix=(i-1)*n+kk-1;
     d=d+a[ix]*a[ix];
    }
    s[kk-1]=sqrt(d);
    if (s[kk-1]!=0.0)
    {
     ix=(kk-1)*n+kk-1;
     if (a[ix]!=0.0)
     {
      s[kk-1]=fabs(s[kk-1]);
      if (a[ix]<0.0)
       s[kk-1]=-s[kk-1];
     }
     for (i=kk; i<=m; i++)
     {
      iy=(i-1)*n+kk-1;
      a[iy]=a[iy]/s[kk-1];
     }
     a[ix]=1.0+a[ix];
    }
    s[kk-1]=-s[kk-1];
   }
   if (n>=kk+1)
   {
    for (j=kk+1; j<=n; j++)
    {
     if ((kk<=k)&&(s[kk-1]!=0.0))
     {
      d=0.0;
      for (i=kk; i<=m; i++)
      {
       ix=(i-1)*n+kk-1;
       iy=(i-1)*n+j-1;
       d=d+a[ix]*a[iy];
      }
      d=-d/a[(kk-1)*n+kk-1];
      for (i=kk; i<=m; i++)
      {
       ix=(i-1)*n+j-1;
       iy=(i-1)*n+kk-1;
       a[ix]=a[ix]+d*a[iy];
      }
     }
     e[j-1]=a[(kk-1)*n+j-1];
    }
   }
   if (kk<=k)
   {
    for (i=kk; i<=m; i++)
    {
     ix=(i-1)*m+kk-1;
     iy=(i-1)*n+kk-1;
     u[ix]=a[iy];
    }
   }
   if (kk<=l)
   {
    d=0.0;
    for (i=kk+1; i<=n; i++)
     d=d+e[i-1]*e[i-1];
    e[kk-1]=sqrt(d);
    if (e[kk-1]!=0.0)
    {
     if (e[kk]!=0.0)
     {
      e[kk-1]=fabs(e[kk-1]);
      if (e[kk]<0.0)
       e[kk-1]=-e[kk-1];
     }
     for (i=kk+1; i<=n; i++)
      e[i-1]=e[i-1]/e[kk-1];
     e[kk]=1.0+e[kk];
    }
    e[kk-1]=-e[kk-1];
    if ((kk+1<=m)&&(e[kk-1]!=0.0))
    {
     for (i=kk+1; i<=m; i++)
      w[i-1]=0.0;
     for (j=kk+1; j<=n; j++)
      for (i=kk+1; i<=m; i++)
       w[i-1]=w[i-1]+e[j-1]*a[(i-1)*n+j-1];
     for (j=kk+1; j<=n; j++)
      for (i=kk+1; i<=m; i++)
      {
       ix=(i-1)*n+j-1;
       a[ix]=a[ix]-w[i-1]*e[j-1]/e[kk];
      }
    }
    for (i=kk+1; i<=n; i++)
     v[(i-1)*n+kk-1]=e[i-1];
   }
  }
 }
 mm=n;
 if (m+1<n)
  mm=m+1;
 if (k<n)
  s[k]=a[k*n+k];
 if (m<mm)
  s[mm-1]=0.0;
 if (l+1<mm)
  e[l]=a[l*n+mm-1];
 e[mm-1]=0.0;
 nn=m;
 if (m>n)
  nn=n;
 if (nn>=k+1)
 {
  for (j=k+1; j<=nn; j++)
  {
   for (i=1; i<=m; i++)
    u[(i-1)*m+j-1]=0.0;
   u[(j-1)*m+j-1]=1.0;
  }
 }
 if (k>=1)
 {
  for (ll=1; ll<=k; ll++)
  {
   kk=k-ll+1;
   iz=(kk-1)*m+kk-1;
   if (s[kk-1]!=0.0)
   {
    if (nn>=kk+1)
     for (j=kk+1; j<=nn; j++)
     {
      d=0.0;
      for (i=kk; i<=m; i++)
      {
       ix=(i-1)*m+kk-1;
       iy=(i-1)*m+j-1;
       d=d+u[ix]*u[iy]/u[iz];
      }
      d=-d;
      for (i=kk; i<=m; i++)
      {
       ix=(i-1)*m+j-1;
       iy=(i-1)*m+kk-1;
       u[ix]=u[ix]+d*u[iy];
      }
     }
    for (i=kk; i<=m; i++)
    {
     ix=(i-1)*m+kk-1;
     u[ix]=-u[ix];
    }
    u[iz]=1.0+u[iz];
    if (kk-1>=1)
     for (i=1; i<=kk-1; i++)
      u[(i-1)*m+kk-1]=0.0;
   }
   else
   {
    for (i=1; i<=m; i++)
     u[(i-1)*m+kk-1]=0.0;
    u[(kk-1)*m+kk-1]=1.0;
   }
  }
 }
 for (ll=1; ll<=n; ll++)
 {
  kk=n-ll+1;
  iz=kk*n+kk-1;
  if ((kk<=l)&&(e[kk-1]!=0.0))
  {
   for (j=kk+1; j<=n; j++)
   {
    d=0.0;
    for (i=kk+1; i<=n; i++)
    {
     ix=(i-1)*n+kk-1;
     iy=(i-1)*n+j-1;
     d=d+v[ix]*v[iy]/v[iz];
    }
    d=-d;
    for (i=kk+1; i<=n; i++)
    {
     ix=(i-1)*n+j-1;
     iy=(i-1)*n+kk-1;
     v[ix]=v[ix]+d*v[iy];
    }
   }
  }
  for (i=1; i<=n; i++)
   v[(i-1)*n+kk-1]=0.0;
  v[iz-n]=1.0;
 }
 for (i=1; i<=m; i++)
  for (j=1; j<=n; j++)
   a[(i-1)*n+j-1]=0.0;
 m1=mm;
 it=60;
 while (1==1)
 {
  if (mm==0)
  {
   ppp(a,e,s,v,m,n);
   free(s);
   free(e);
   free(w);
   return(1);
  }
  if (it==0)
  {
   ppp(a,e,s,v,m,n);
   free(s);
   free(e);
   free(w);
   return(-1);
  }
  kk=mm-1;
  while ((kk!=0)&&(fabs(e[kk-1])!=0.0))
  {
   d=fabs(s[kk-1])+fabs(s[kk]);
   dd=fabs(e[kk-1]);
   if (dd>eps*d)
    kk=kk-1;
   else
    e[kk-1]=0.0;
  }
  if (kk==mm-1)
  {
   kk=kk+1;
   if (s[kk-1]<0.0)
   {
    s[kk-1]=-s[kk-1];
    for (i=1; i<=n; i++)
    {
     ix=(i-1)*n+kk-1;
     v[ix]=-v[ix];
    }
   }
   while ((kk!=m1)&&(s[kk-1]<s[kk]))
   {
    d=s[kk-1];
    s[kk-1]=s[kk];
    s[kk]=d;
    if (kk<n)
     for (i=1; i<=n; i++)
     {
      ix=(i-1)*n+kk-1;
      iy=(i-1)*n+kk;
      d=v[ix];
      v[ix]=v[iy];
      v[iy]=d;
     }
    if (kk<m)
     for (i=1; i<=m; i++)
     {
      ix=(i-1)*m+kk-1;
      iy=(i-1)*m+kk;
      d=u[ix];
      u[ix]=u[iy];
      u[iy]=d;
     }
    kk=kk+1;
   }
   it=60;
   mm=mm-1;
  }
  else
  {
   ks=mm;
   while ((ks>kk)&&(fabs(s[ks-1])!=0.0))
   {
    d=0.0;
    if (ks!=mm)
     d=d+fabs(e[ks-1]);
    if (ks!=kk+1)
     d=d+fabs(e[ks-2]);
    dd=fabs(s[ks-1]);
    if (dd>eps*d)
     ks=ks-1;
    else
     s[ks-1]=0.0;
   }
   if (ks==kk)
   {
    kk=kk+1;
    d=fabs(s[mm-1]);
    t=fabs(s[mm-2]);
    if (t>d)
     d=t;
    t=fabs(e[mm-2]);
    if (t>d)
     d=t;
    t=fabs(s[kk-1]);
    if (t>d)
     d=t;
    t=fabs(e[kk-1]);
    if (t>d)
     d=t;
    sm=s[mm-1]/d;
    sm1=s[mm-2]/d;
    em1=e[mm-2]/d;
    sk=s[kk-1]/d;
    ek=e[kk-1]/d;
    b=((sm1+sm)*(sm1-sm)+em1*em1)/2.0;
    c=sm*em1;
    c=c*c;
    shh=0.0;
    if ((b!=0.0)||(c!=0.0))
    {
     shh=sqrt(b*b+c);
     if (b<0.0)
      shh=-shh;
     shh=c/(b+shh);
    }
    fg[0]=(sk+sm)*(sk-sm)-shh;
    fg[1]=sk*ek;
    for (i=kk; i<=mm-1; i++)
    {
     sss(fg,cs);
     if (i!=kk)
      e[i-2]=fg[0];
     fg[0]=cs[0]*s[i-1]+cs[1]*e[i-1];
     e[i-1]=cs[0]*e[i-1]-cs[1]*s[i-1];
     fg[1]=cs[1]*s[i];
     s[i]=cs[0]*s[i];
     if ((cs[0]!=1.0)||(cs[1]!=0.0))
      for (j=1; j<=n; j++)
      {
       ix=(j-1)*n+i-1;
       iy=(j-1)*n+i;
       d=cs[0]*v[ix]+cs[1]*v[iy];
       v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
       v[ix]=d;
      }
     sss(fg,cs);
     s[i-1]=fg[0];
     fg[0]=cs[0]*e[i-1]+cs[1]*s[i];
     s[i]=-cs[1]*e[i-1]+cs[0]*s[i];
     fg[1]=cs[1]*e[i];
     e[i]=cs[0]*e[i];
     if (i<m)
      if ((cs[0]!=1.0)||(cs[1]!=0.0))
       for (j=1; j<=m; j++)
       {
        ix=(j-1)*m+i-1;
        iy=(j-1)*m+i;
        d=cs[0]*u[ix]+cs[1]*u[iy];
        u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
        u[ix]=d;
       }
    }
    e[mm-2]=fg[0];
    it=it-1;
   }
   else
   {
    if (ks==mm)
    {
     kk=kk+1;
     fg[1]=e[mm-2];
     e[mm-2]=0.0;
     for (ll=kk; ll<=mm-1; ll++)
     {
      i=mm+kk-ll-1;
      fg[0]=s[i-1];
      sss(fg,cs);
      s[i-1]=fg[0];
      if (i!=kk)
      {
       fg[1]=-cs[1]*e[i-2];
       e[i-2]=cs[0]*e[i-2];
      }
      if ((cs[0]!=1.0)||(cs[1]!=0.0))
       for (j=1; j<=n; j++)
       {
        ix=(j-1)*n+i-1;
        iy=(j-1)*n+mm-1;
        d=cs[0]*v[ix]+cs[1]*v[iy];
        v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
        v[ix]=d;
       }
     }
    }
    else
    {
     kk=ks+1;
     fg[1]=e[kk-2];
     e[kk-2]=0.0;
     for (i=kk; i<=mm; i++)
     {
      fg[0]=s[i-1];
      sss(fg,cs);
      s[i-1]=fg[0];
      fg[1]=-cs[1]*e[i-1];
      e[i-1]=cs[0]*e[i-1];
      if ((cs[0]!=1.0)||(cs[1]!=0.0))
       for (j=1; j<=m; j++)
       {
        ix=(j-1)*m+i-1;
        iy=(j-1)*m+kk-2;
        d=cs[0]*u[ix]+cs[1]*u[iy];
        u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
        u[ix]=d;
       }
     }
    }
   }
  }
 }
}

void ppp(double a[], double e[], double s[], double v[], int m, int n)
{
 int i,j,p,q;
 double d;
 if (m>=n)
  i=n;
 else
  i=m;
 for (j=1; j<=i-1; j++)
 {
  a[(j-1)*n+j-1]=s[j-1];
  a[(j-1)*n+j]=e[j-1];
 }
 a[(i-1)*n+i-1]=s[i-1];
 if (m<n)
  a[(i-1)*n+i]=e[i-1];
 for (i=1; i<=n-1; i++)
  for (j=i+1; j<=n; j++)
  {
   p=(i-1)*n+j-1;
   q=(j-1)*n+i-1;
   d=v[p];
   v[p]=v[q];
   v[q]=d;
  }
 return;
}

void sss(double fg[2], double cs[2])
{
 double r,d;
 if ((fabs(fg[0])+fabs(fg[1]))==0.0)
 {
  cs[0]=1.0;
  cs[1]=0.0;
  d=0.0;
 }
 else
 {
  d=sqrt(fg[0]*fg[0]+fg[1]*fg[1]);
  if (fabs(fg[0])>fabs(fg[1]))
  {
   d=fabs(d);
   if (fg[0]<0.0)
    d=-d;
  }
  if (fabs(fg[1])>=fabs(fg[0]))
  {
   d=fabs(d);
   if (fg[1]<0.0)
    d=-d;
  }
  cs[0]=fg[0]/d;
  cs[1]=fg[1]/d;
 }
 r=1.0;
 if (fabs(fg[0])>fabs(fg[1]))
  r=cs[1];
 else
  if (cs[0]!=0.0)
   r=1.0/cs[0];
 fg[0]=d;
 fg[1]=r;
 return;
}

//复矩阵相乘
void bcmul(double ar[], double ai[], double br[], double bi[], int m, int n, int k, double cr[], double ci[])
{
 int i,j,l,u,v,w;
 double p,q,s;
 for (i=0; i<=m-1; i++)
  for (j=0; j<=k-1; j++)
  {
   u=i*k+j;
   cr[u]=0.0;
   ci[u]=0.0;
   for (l=0; l<=n-1; l++)
   {
    v=i*n+l;
    w=l*k+j;
    p=ar[v]*br[w];
    q=ai[v]*bi[w];
    s=(ar[v]+ai[v])*(br[w]+bi[w]);
    cr[u]=cr[u]+p-q;
    ci[u]=ci[u]+s-p-q;
   }
  }
 return;
}

//实矩阵相乘
void brmul(double a[], double b[], int m, int n, int k, double c[])
{
 int i,j,l,u;
 for (i=0; i<=m-1; i++)
  for (j=0; j<=k-1; j++)
  {
   u=i*k+j;
   c[u]=0.0;
   for (l=0; l<=n-1; l++)
    c[u]=c[u]+a[i*n+l]*b[l*k+j];
  }
 return;
}

//一般实矩阵的QR分解
int bmaqr(double a[], int m, int n, double q[])
{
 int i,j,k,l,nn,p,jj;
 double u,alpha,w,t;
 if (m<n)
 {
  printf("fail\n");
  return(0);
 }
 for (i=0; i<=m-1; i++)
  for (j=0; j<=m-1; j++)
  {
   l=i*m+j;
   q[l]=0.0;
   if (i==j)
    q[l]=1.0;
  }
 nn=n;
 if (m==n)
  nn=m-1;
 for (k=0; k<=nn-1; k++)
 {
  u=0.0;
  l=k*n+k;
  for (i=k; i<=m-1; i++)
  {
   w=fabs(a[i*n+k]);
   if (w>u)
    u=w;
  }
  alpha=0.0;
  for (i=k; i<=m-1; i++)
  {
   t=a[i*n+k]/u;
   alpha=alpha+t*t;
  }
  if (a[l]>0.0)
   u=-u;
  alpha=u*sqrt(alpha);
  if (fabs(alpha)+1.0==1.0)
  {
   printf("fail\n");
   return(0);
  }
  u=sqrt(2.0*alpha*(alpha-a[l]));
  if ((u+1.0)!=1.0)
  {
   a[l]=(a[l]-alpha)/u;
   for (i=k+1; i<=m-1; i++)
   {
    p=i*n+k;
    a[p]=a[p]/u;
   }
   for (j=0; j<=m-1; j++)
   {
    t=0.0;
    for (jj=k; jj<=m-1; jj++)
     t=t+a[jj*n+k]*q[jj*m+j];
    for (i=k; i<=m-1; i++)
    {
     p=i*m+j;
     q[p]=q[p]-2.0*t*a[i*n+k];
    }
   }
   for (j=k+1; j<=n-1; j++)
   {
    t=0.0;
    for (jj=k; jj<=m-1; jj++)
     t=t+a[jj*n+k]*a[jj*n+j];
    for (i=k; i<=m-1; i++)
    {
     p=i*n+j;
     a[p]=a[p]-2.0*t*a[i*n+k];
    }
   }
   a[l]=alpha;
   for (i=k+1; i<=m-1; i++)
    a[i*n+k]=0.0;
  }
 }
 for (i=0; i<=m-2; i++)
  for (j=i+1; j<=m-1;j++)
  {
   p=i*m+j;
   l=j*m+i;
   t=q[p];
   q[p]=q[l];
   q[l]=t;
  }
 return(1);
}

//复矩阵求逆的全选主元高斯-约当法
int bcinv(double ar[], double ai[], int n)
{
 int *is,*js,i,j,k,l,u,v,w;
 double p,q,s,t,d,b;
 is=(int *)malloc(n*sizeof(int));
 js=(int *)malloc(n*sizeof(int));
 for (k=0; k<=n-1; k++)
 {
  d=0.0;
  for (i=k; i<=n-1; i++)
   for (j=k; j<=n-1; j++)
   {
    u=i*n+j;
    p=ar[u]*ar[u]+ai[u]*ai[u];
    if (p>d)
    {
     d=p;
     is[k]=i;
     js[k]=j;
    }
   }
  if (d+1.0==1.0)
  {
   free(is);
   free(js);
   printf("err**not inv\n");
   return(0);
  }
  if (is[k]!=k)
   for (j=0; j<=n-1; j++)
   {
    u=k*n+j;
    v=is[k]*n+j;
    t=ar[u];
    ar[u]=ar[v];
    ar[v]=t;
    t=ai[u];
    ai[u]=ai[v];
    ai[v]=t;
   }
  if (js[k]!=k)
   for (i=0; i<=n-1; i++)
   {
    u=i*n+k;
    v=i*n+js[k];
    t=ar[u];
    ar[u]=ar[v];
    ar[v]=t;
    t=ai[u];
    ai[u]=ai[v];
    ai[v]=t;
   }
  l=k*n+k;
  ar[l]=ar[l]/d;
  ai[l]=-ai[l]/d;
  for (j=0; j<=n-1; j++)
   if (j!=k)
   {
    u=k*n+j;
    p=ar[u]*ar[l];
    q=ai[u]*ai[l];
    s=(ar[u]+ai[u])*(ar[l]+ai[l]);
    ar[u]=p-q;
    ai[u]=s-p-q;
   }
  for (i=0; i<=n-1; i++)
   if (i!=k)
   {
    v=i*n+k;
    for (j=0; j<=n-1; j++)
     if (j!=k)
     {
      u=k*n+j;
      w=i*n+j;
      p=ar[u]*ar[v];
      q=ai[u]*ai[v];
      s=(ar[u]+ai[u])*(ar[v]+ai[v]);
      t=p-q;
      b=s-p-q;
      ar[w]=ar[w]-t;
      ai[w]=ai[w]-b;
     }
   }
  for (i=0; i<=n-1; i++)
   if (i!=k)
   {
    u=i*n+k;
    p=ar[u]*ar[l];
    q=ai[u]*ai[l];
    s=(ar[u]+ai[u])*(ar[l]+ai[l]);
    ar[u]=q-p;
    ai[u]=p+q-s;
   }
 }
 for (k=n-1; k>=0; k--)
 {
  if (js[k]!=k)
   for (j=0; j<=n-1; j++)
   {
    u=k*n+j;
    v=js[k]*n+j;
    t=ar[u];
    ar[u]=ar[v];
    ar[v]=t;
    t=ai[u];
    ai[u]=ai[v];
    ai[v]=t;
   }
  if (is[k]!=k)
   for (i=0; i<=n-1; i++)
   {
    u=i*n+k;
    v=i*n+is[k];
    t=ar[u];
    ar[u]=ar[v];
    ar[v]=t;
    t=ai[u];
    ai[u]=ai[v];
    ai[v]=t;
   }
 }
 free(is); free(js);
 return(1);
}

//托伯利兹矩阵求逆的特兰持方法
int btrch(double t[], double tt[], int n, double b[])
{
 int i,j,k;
 double a,s,*c,*r,*p;
 c=(double *)malloc(n*sizeof(double));
 r=(double *)malloc(n*sizeof(double));
 p=(double *)malloc(n*sizeof(double));
 if(fabs(t[0])+1.0==1.0)
 {
  free(c);
  free(r);
  free(p);
  printf("fail\n");
  return(-1);
 }
 a=t[0];
 c[0]=tt[1]/t[0];
 r[0]=t[1]/t[0];
 for (k=0; k<=n-3; k++)
 {
  s=0.0;
  for (j=1; j<=k+1; j++)
   s=s+c[k+1-j]*tt[j];
  s=(s-tt[k+2])/a;
  for (i=0; i<=k; i++)
   p[i]=c[i]+s*r[k-i];
  c[k+1]=-s;
  s=0.0;
  for (j=1; j<=k+1; j++)
   s=s+r[k+1-j]*t[j];
  s=(s-t[k+2])/a;
  for (i=0; i<=k; i++)
  {
   r[i]=r[i]+s*c[k-i];
   c[k-i]=p[k-i];
  }
  r[k+1]=-s;
  a=0.0;
  for (j=1; j<=k+2; j++)
   a=a+t[j]*c[j-1];
  a=t[0]-a;
  if (fabs(a)+1.0==1.0)
  {
   free(c);
   free(r);
   free(p);
   printf("fail\n");
   return(-1);
  }
 }
 b[0]=1.0/a;
 for (i=0; i<=n-2; i++)
 {
  k=i+1;
  j=(i+1)*n;
  b[k]=-r[i]/a;
  b[j]=-c[i]/a;
 }
 for (i=0; i<=n-1; i++)
  for (j=0; j<=n-2; j++)
  {
   k=(i+1)*n+j+1;
   b[k]=b[i*n+j]-c[i]*b[j+1];
   b[k]=b[k]+c[n-j-2]*b[n-i-1];
  }
 free(c);
 free(r);
 free(p);
 return(1);
}

//实短阵求逆的全选主元高斯-约当法
int brinv(double a[], int n)
{
 int *is,*js,i,j,k,l,u,v;
 double d,p;
 is=(int *)malloc(n*sizeof(int));
 js=(int *)malloc(n*sizeof(int));
 for (k=0; k<=n-1; k++)
 {
  d=0.0;
  for (i=k; i<=n-1; i++)
   for (j=k; j<=n-1; j++)
   {
    l=i*n+j;
    p=fabs(a[l]);
    if (p>d)
    {
     d=p;
     is[k]=i;
     js[k]=j;
    }
   }
  if (d+1.0==1.0)
  {
   free(is);
   free(js);
   printf("err**not inv\n");
   return(0);
  }
  if (is[k]!=k)
   for (j=0; j<=n-1; j++)
   {
    u=k*n+j;
    v=is[k]*n+j;
    p=a[u];
    a[u]=a[v];
    a[v]=p;
   }
  if (js[k]!=k)
   for (i=0; i<=n-1; i++)
   {
    u=i*n+k;
    v=i*n+js[k];
    p=a[u];
    a[u]=a[v];
    a[v]=p;
   }
  l=k*n+k;
  a[l]=1.0/a[l];
  for (j=0; j<=n-1; j++)
   if (j!=k)
   {
    u=k*n+j;
    a[u]=a[u]*a[l];
   }
  for (i=0; i<=n-1; i++)
   if (i!=k)
    for (j=0; j<=n-1; j++)
     if (j!=k)
     {
      u=i*n+j;
      a[u]=a[u]-a[i*n+k]*a[k*n+j];
     }
  for (i=0; i<=n-1; i++)
   if (i!=k)
   {
    u=i*n+k;
    a[u]=-a[u]*a[l];
   }
 }
 for (k=n-1; k>=0; k--)
 {
  if (js[k]!=k)
   for (j=0; j<=n-1; j++)
   {
    u=k*n+j;
    v=js[k]*n+j;
    p=a[u];
    a[u]=a[v];
    a[v]=p;
   }
  if (is[k]!=k)
   for (i=0; i<=n-1; i++)
   {
    u=i*n+k;
    v=i*n+is[k];
    p=a[u];
    a[u]=a[v];
    a[v]=p;
   }
 }
 free(is); free(js);
 return(1);
}

//矩阵的三角分解
int blluu(double a[], int n, double l[], double u[])
{
 int i,j,k,w,v,ll;
 for (k=0; k<=n-2; k++)
 {
  ll=k*n+k;
  if (fabs(a[ll])+1.0==1.0)
  {
   printf("fail\n");
   return(0);
  }
  for (i=k+1; i<=n-1; i++)
  {
   w=i*n+k;
   a[w]=a[w]/a[ll];
  }
  for (i=k+1; i<=n-1; i++)
  {
   w=i*n+k;
   for (j=k+1; j<=n-1; j++)
   {
    v=i*n+j;
    a[v]=a[v]-a[w]*a[k*n+j];
   }
  }
 }
 for (i=0; i<=n-1; i++)
 {
  for (j=0; j<i; j++)
  {
   w=i*n+j;
   l[w]=a[w];
   u[w]=0.0;
  }
  w=i*n+i;
  l[w]=1.0;
  u[w]=a[w];
  for (j=i+1; j<=n-1; j++)
  {
   w=i*n+j;
   l[w]=0.0;
   u[w]=a[w];
  }
 }
 return(1);
}

//对称正定矩阵的乔里斯基分解与行列式的求值
int bchol(double a[], int n, double * det)
{
 int i,j,k,u,l;
 double d;
 if ((a[0]+1.0==1.0)||(a[0]<0.0))
 {
  printf("fail\n");
  return(-2);
 }
 a[0]=sqrt(a[0]);
 d=a[0];
 for (i=1; i<=n-1; i++)
 {
  u=i*n;
  a[u]=a[u]/a[0];
 }
 for (j=1; j<=n-1; j++)
 {
  l=j*n+j;
  for (k=0; k<=j-1; k++)
  {
   u=j*n+k;
   a[l]=a[l]-a[u]*a[u];
  }
  if ((a[l]+1.0==1.0)||(a[l]<0.0))
  {
   printf("fail\n");
   return(-2);
  }
  a[l]=sqrt(a[l]);
  d=d*a[l];
  for (i=j+1; i<=n-1; i++)
  {
   u=i*n+j;
   for (k=0; k<=j-1; k++)
    a[u]=a[u]-a[i*n+k]*a[j*n+k];
   a[u]=a[u]/a[l];
  }
 }
 *det=d*d;
 for (i=0; i<=n-2; i++)
  for (j=i+1; j<=n-1; j++)
   a[i*n+j]=0.0;
 return(2);
}

                              ----根据《C语言常用算法程序集》整理