martrix.cpp

该源码主要是快速完成矩阵的suv分解

// Martrix.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "iostream.h"
#include "math.h"




#define  dimention  5


 
int csstq(int n,double *b,double *c,double *q,double eps,int l)  
  { int i,j,k,m,it,u,v;
    double d,f,h,g,p,r,e,s;
    c[n-1]=0.0; d=0.0; f=0.0;
    for (j=0; j<=n-1; j++)
      { it=0;
        h=eps*(fabs(b[j])+fabs(c[j]));
        if (h>d) d=h;
        m=j;
        while ((m<=n-1)&&(fabs(c[m])>d)) m=m+1;
        if (m!=j)
          { do
              { if (it==l)
                  { printf("fail\n");
                    return(-1);
                  }
                it=it+1;
                g=b[j];
                p=(b[j+1]-g)/(2.0*c[j]);
                r=sqrt(p*p+1.0);
                if (p>=0.0) b[j]=c[j]/(p+r);
                else b[j]=c[j]/(p-r);
                h=g-b[j];
                for (i=j+1; i<=n-1; i++)
                  b[i]=b[i]-h;
                f=f+h; p=b[m]; e=1.0; s=0.0;
                for (i=m-1; i>=j; i--)
                  { g=e*c[i]; h=e*p;
                    if (fabs(p)>=fabs(c[i]))
                      { e=c[i]/p; r=sqrt(e*e+1.0);
                        c[i+1]=s*p*r; s=e/r; e=1.0/r;
                      }
                    else
		      { e=p/c[i]; r=sqrt(e*e+1.0);
                        c[i+1]=s*c[i]*r;
                        s=1.0/r; e=e/r;
                      }
                    p=e*b[i]-s*g;
                    b[i+1]=h+s*(e*g+s*b[i]);
                    for (k=0; k<=n-1; k++)
                      { u=k*n+i+1; v=u-1;
                        h=q[u]; q[u]=s*q[v]+e*h;
                        q[v]=e*q[v]-s*h;
                      }
                  }
                c[j]=s*p; b[j]=e*p;
              }
            while (fabs(c[j])>d);
          }
        b[j]=b[j]+f;
      }
    for (i=0; i<=n-1; i++)
      { k=i; p=b[i];
        if (i+1<=n-1)
          { j=i+1;
            while ((j<=n-1)&&(b[j]<=p))
              { k=j; p=b[j]; j=j+1;}
          }
        if (k!=i)
          { b[k]=b[i]; b[i]=p;
            for (j=0; j<=n-1; j++)
              { u=j*n+i; v=j*n+k;
                p=q[u]; q[u]=q[v]; q[v]=p;
              }
          }
      }
    return(1);
  }


  void cstrq(double *a,int n,double *q,double *b,double *c)  
  { int i,j,k,u;
    double h,f,g,h2;
    for (i=0; i<=n-1; i++)
    for (j=0; j<=n-1; j++)
      { u=i*n+j; q[u]=a[u];}
    for (i=n-1; i>=1; i--)
      { h=0.0;
        if (i>1)
          for (k=0; k<=i-1; k++)
            { u=i*n+k; h=h+q[u]*q[u];}
        if (h+1.0==1.0)
          { c[i]=0.0;
            if (i==1) c[i]=q[i*n+i-1];
            b[i]=0.0;
          }
        else
          { c[i]=sqrt(h);
            u=i*n+i-1;
            if (q[u]>0.0) c[i]=-c[i];
            h=h-q[u]*c[i];
            q[u]=q[u]-c[i];
            f=0.0;
            for (j=0; j<=i-1; j++)
              { q[j*n+i]=q[i*n+j]/h;
                g=0.0;
                for (k=0; k<=j; k++)
                  g=g+q[j*n+k]*q[i*n+k];
                if (j+1<=i-1)
                  for (k=j+1; k<=i-1; k++)
                    g=g+q[k*n+j]*q[i*n+k];
                c[j]=g/h;
                f=f+g*q[j*n+i];
              }
            h2=f/(h+h);
            for (j=0; j<=i-1; j++)
              { f=q[i*n+j];
                g=c[j]-h2*f;
                c[j]=g;
                for (k=0; k<=j; k++)
                  { u=j*n+k;
                    q[u]=q[u]-f*c[k]-g*q[i*n+k];
                  }
              }
            b[i]=h;
          }
      }
    for (i=0; i<=n-2; i++) c[i]=c[i+1];
    c[n-1]=0.0;
    b[0]=0.0;
    for (i=0; i<=n-1; i++)
      { if ((b[i]!=0.0)&&(i-1>=0))
          for (j=0; j<=i-1; j++)
            { g=0.0;
              for (k=0; k<=i-1; k++)
                g=g+q[i*n+k]*q[k*n+j];
              for (k=0; k<=i-1; k++)
                { u=k*n+j;
                  q[u]=q[u]-g*q[k*n+i];
                }
            }
        u=i*n+i;
        b[i]=q[u]; q[u]=1.0;
        if (i-1>=0)
          for (j=0; j<=i-1; j++)
            { q[i*n+j]=0.0; q[j*n+i]=0.0;}
      }
    return;
  }
//b=a*aT
void turnmultiply(double *a, int n,int m,double *b)
{
	int i,j,k;
	double value;
	for(i=0;i<n;i++)
		for(j=0;j<n;j++)
		{
			value=0;
			for(k=0;k<m;k++)
				value+=a[i*m+k]*a[j*m+k];
			b[i*n+j]=value;
		}
}

void paixu(double *a,int n)
 {
	 int k,i,j;
	 double temp,max;
	 for(i=0;i<n-1;i++)
	 {
		 max=-100000000000000000;
		 for(j=i;j<n;j++)
		 { 
              if(a[j]>max)
			  {
				  max=a[j];
				  k=j;
			  }
		 }
         temp=a[i];a[i]=a[k];a[k]=temp;
	 }
 }

int main(int argc, char* argv[])
{
	printf("Hello World!\n");
	

	int i,j;
	double a[25]={0,1,2,1,0,1,3,4,3,1,2,4,5,4,2,1,3,4,3,1,0,1,2,1,0};
	double aaT[25];
    turnmultiply(a,5,5,aaT);
	for(i=0;i<5;i++)
	{
		for(j=0;j<5;j++)
		{
          cout<<aaT[i*5+j]<<" ";
		}
		cout<<endl;
	}


	int k,l=60;
	float eps=0.000001;
	static double q[dimention*dimention],b[dimention],c[dimention];
   
	cstrq(aaT,dimention,q,b,c);
    k=csstq(dimention,b,c,q,eps,l);

	
    printf(" MAT A IS:\n");
    for (i=0; i<=dimention-1; i++)
      { for (j=0; j<=dimention-1; j++)
          printf("%13.7e ",aaT[i*dimention+j]);
        printf("\n");
      }


    printf("\n");
    if (k>0)
      { printf("MAT B IS:\n");
        for (i=0; i<=dimention-1; i++)
          printf("%13.7e ",b[i]);
        printf("\n\n");
        printf("MAT Q IS:\n");
        for (i=0; i<=dimention-1; i++)
          { for (j=0; j<=dimention-1; j++)
              printf("%13.7e ",q[i*dimention+j]);
            printf("\n");
          }
        printf("\n");
      }

	
	paixu(b,5);
	for(i=0;i<5;i++)
		cout<<b[i]<<endl;
		 
return 0;

}