home *** CD-ROM | disk | FTP | other *** search
/ Power-Programmierung / CD2.mdf / c / library / dos / math / smooth11 / solve.c < prev    next >
Encoding:
C/C++ Source or Header  |  1988-03-30  |  7.4 KB  |  363 lines
  1. /*    decomp & solve - solution of linear system
  2.  
  3.  
  4.     AUTHORS
  5.         Forsythe, Malcolm, and Moler, from "Computer Methods for
  6.         Mathematical Computations"
  7.  
  8.         translated to C by J. R. Van Zandt
  9. */
  10.  
  11. #include <stdio.h>
  12. #include <math.h>
  13.  
  14. #define aa(i,j) a[ix[i]+j]        /* like a[i][j] or a[i*ndim+j] but faster */
  15.  
  16. double decomp(ndim,n,a,ipvt)    /* returns estimate of condition number  */
  17.     int ndim,    /* 2nd declared dimension of a (# columns) */
  18.     n;            /* rows 0...n-1 and columns 0...n-1 of a are used */
  19.     double a[];    /* two dimensional array of matrix coefficients */
  20.     int ipvt[];    /* pivot columns */
  21. {    double ek,t,anorm,ynorm,znorm,cond;
  22.     int nm1,i,j,k,kp1,kb,km1,m;
  23.     static double work[25];
  24.     static int ix[25];
  25.     if(n>25)
  26.         {fprintf(stderr,"decomp: matrix too big\n");
  27.         exit(1);
  28.         }
  29. /*
  30.     double *work;
  31.     int *ix;
  32.     work=malloc(n*sizeof(double));
  33.     ix=malloc(n*sizeof(int));
  34.     if(work==0 || ix==0)
  35.         {fprintf(stderr,"decomp: no workspace\n");
  36.         exit(1);
  37.         }
  38. */
  39.     for (i=0; i<n; i++)
  40.         ix[i]=i*ndim;
  41.     nm1=n-1;
  42.     ipvt[nm1]=0;
  43.     if(n==1)
  44.         {
  45. /*
  46.         free(work);
  47.         free(ix);
  48. */
  49.         if(a[0]==0.)
  50.             return 1.e32;
  51.         else
  52.             return 1.;
  53.         }
  54.             /*    compute 1-norm of a    */
  55.     anorm=0.;
  56.     for (j=0; j<n; j++)
  57.         {t=0.;
  58.         for (i=0; i<n; i++)
  59.             t += fabs(aa(i,j));
  60.         if(t>anorm)
  61.             anorm=t;
  62.         }
  63.                     /*    Gaussian elimination with partial pivoting */
  64.     for (k=0; k<nm1; k++)
  65.         {kp1=k+1;
  66.                     /*    Find pivot    */
  67.         m=k;
  68.         for (i=kp1; i<n; i++)
  69.             if(fabs(aa(i,k)) > fabs(aa(m,k)))
  70.                 m=i;
  71.         ipvt[k]=m;
  72.         t=aa(m,k);
  73. /*        printf("pivoting on a[%d][%d] = %8.4f\n",m,k,t);    */
  74.         if(m!=k)
  75.             {ipvt[nm1]=-ipvt[nm1];
  76.             aa(m,k)=aa(k,k);
  77.             aa(k,k)=t;
  78.             }
  79.                     /*    skip step if pivot is zero */
  80.         if(t!=0.)
  81.             {        /*    compute multipliers    */
  82.             for (i=kp1; i<n; i++)
  83.                 aa(i,k) = -aa(i,k)/t;
  84.                     /*    interchange and eliminate by columns    */
  85.             for (j=kp1; j<n; j++)
  86.                 {t=aa(m,j);
  87.                 aa(m,j) = aa(k,j);
  88.                 aa(k,j) = t;
  89.                 if(t != 0.)
  90.                     for (i=kp1; i<n; i++)
  91.                         aa(i,j) += aa(i,k)*t;
  92.                 }
  93.             }
  94. /*        showm("After pivoting",a,ndim,n);    */
  95.         }
  96.                     /*    
  97.                         cond = (1-norm of a)*(an estimate of 1-norm of
  98.                         a-inverse) estimate obtained by one step of
  99.                         inverse iteration for the small singular
  100.                         vector.  This involves solving two systems of
  101.                         equations, (a-transpose)*y = e and a*z=y there
  102.                         e is a vector of +1 or -1 chosen to cause
  103.                         growth in y.  Estimate = (1-norm of z)/(1-norm
  104.                         of y)
  105.                     */
  106.             
  107.                     /*    solve (a-transpose)*y - e    */
  108.     for (k=0; k<n; k++)
  109.         {t=0.;
  110.         if(k)
  111.             for (i=0; i<k; i++)
  112.                 t += aa(i,k)*work[i];
  113.         if(t<0.) ek = -1.; else ek=1.;
  114.         if (aa(k,k) == 0.)
  115.             {
  116. /*
  117.             free(work);
  118.             free(ix);
  119. */
  120.             return 1.e32;
  121.             }
  122.         work[k] = -(ek+t)/aa(k,k);
  123.         }
  124. /*    showv("decompose: work1",work,n);    */
  125.     for (k=n-2; k>=0; k--)
  126.         {t=0.;
  127.         for (i=k+1; i<n; i++)
  128.             t += aa(i,k)*work[k];
  129.         work[k] = t;
  130.         m=ipvt[k];
  131.         if (m!=k)
  132.             {t=work[m];
  133.             work[m] = work[k];
  134.             work[k]=t;
  135.             }
  136.         }
  137.     ynorm=0.;
  138.     for (i=0; i<n; i++)
  139.         ynorm += fabs(work[i]);
  140.             /*    solve a*z=y  */
  141.     solve(ndim, n, a, work, ipvt);
  142.     znorm=0.;
  143.     for (i=0; i<n; i++)
  144.         znorm += fabs(work[i]);
  145.             /*    estimate condition */
  146.     cond=anorm*znorm/ynorm;
  147.     if(cond<1.)
  148.         cond=1.;
  149. /*
  150.     free(work);
  151.     free(ix);
  152. */
  153.     return cond;
  154. }
  155.  
  156. double invert(ndim,n,a,x)    /* returns estimate of condition number of matrix */
  157.     int ndim,        /* 2nd declared dimension of a and x (# columns) */
  158.     n;                /* rows 0...n-1 and columns 0...n-1 of a are used */
  159.     double a[],        /*    matrix to be inverted    */
  160.     x[];            /*    resulting inverse    */
  161. {    int i, j;
  162.     double cond, condp1;
  163.     static double work[25];
  164.     static int ipvt[25];
  165.     if(n>25)
  166.         {fprintf(stderr,"invert: matrix too big\n");
  167.         exit(1);
  168.         }
  169. /*
  170.     double *work;
  171.     int *ipvt;
  172.     ipvt=malloc(n*sizeof(int));
  173.     work=malloc(n*sizeof(double));
  174.     if(ipvt==0 || work==0)
  175.         {fprintf(stderr,"invert: not enough memory\n");
  176.         exit(1);
  177.         }
  178. */
  179.     cond=decomp(ndim,n,a,ipvt);
  180. /*    printf("invert: cond = %f\n\n",cond);
  181.     printf("invert: ipvt\n");
  182.     for (i=0; i<n; i++)
  183.         printf("%8d \n",ipvt[i]);
  184. */
  185.     condp1=cond+1.;
  186.     if(condp1==cond)
  187.         {
  188. /*
  189.         free(work);
  190.         free(ipvt);
  191. */
  192.         return cond;
  193.         }
  194.     for (i=0; i<n; i++)
  195.         {for (j=0; j<n; j++)
  196.             work[j]=0.;
  197.         work[i]=1.;
  198. /*        showv("invert: RHS",work,n);    */
  199.         solve(ndim,n,a,work,ipvt);
  200.         for (j=0; j<n; j++)
  201.             x[j*ndim+i]=work[j];
  202.         }
  203. /*
  204.     free(work);
  205.     free(ipvt);
  206. */
  207.     return cond;
  208. }
  209.  
  210. solve (ndim,n,a,b,ipvt)
  211.     int ndim,        /*    2nd declared dimension of a (# columns)     */
  212.     n;        /* order of matrix a: rows 0...n-1 & columns 0...n-1 are used */
  213.     double a[],        /*    triangularized matrix obtained from decomp    */
  214.     b[];            /*    right hand side vector    */
  215.     int ipvt[];        /*    pivot vector obtained from decomp    */
  216. {    int kb,km1,nm1,kp1,i,k,m;
  217.     double t;
  218.     static int ix[25];
  219.     if(n>25)
  220.         {fprintf(stderr,"solve: matrix too big\n");
  221.         exit(1);
  222.         }
  223. /*
  224.     int *ix;
  225.     ix=malloc(n*sizeof(int));
  226.     if(ix==0)
  227.         {fprintf(stderr,"solve: no workspace\n");
  228.         exit(1);
  229.         }
  230. */
  231.     for (i=0; i<n; i++)
  232.         ix[i]=i*ndim;
  233. /*    showm("solve: decomposed matrix",a,ndim,n);
  234.     showv("solve: RHS",b,n);                        */
  235.             /*    forward elimination    */
  236.     if(n!=1)
  237.         {nm1=n-1;
  238.         for (k=0; k<nm1; k++)
  239.             {kp1=k+1;
  240.             m=ipvt[k];
  241.             t=b[m];
  242.             b[m]=b[k];
  243.             b[k]=t;
  244.             for (i=kp1; i<n; i++)
  245.                 b[i] += aa(i,k)*t;
  246.             }
  247. /*        showv("\nafter forward elimination",b,n);    */
  248.                     /*    back substitution */
  249.         for (k=nm1; k; k--)
  250.             {b[k] /= aa(k,k);
  251.             t=-b[k];
  252.             for (i=0; i<k; i++)
  253.                 b[i] += aa(i,k)*t;
  254.             }
  255.         }
  256.     b[0] /= a[0];
  257. /*    showv("\nafter back substitution",b,n);        */
  258. /*
  259.     free(ix);
  260. */
  261. }
  262.  
  263. #ifdef TEST
  264.  
  265. /*    sample program for decomp and solve */
  266.  
  267. main()
  268. {    double x[13][13], p[13][13], a[13][13], b[13], cond, condp1;
  269.     int    ipvt[13], i, j, k, n, ndim;
  270.  
  271.     ndim=13;
  272.     n=3;
  273.     a[0][0]=10.;
  274.     a[1][0]=-3.;
  275.     a[2][0]= 5.;
  276.     a[0][1]=-7.;
  277.     a[1][1]= 2.;
  278.     a[2][1]=-1.;
  279.     a[0][2]= 0.;
  280.     a[1][2]= 6.;
  281.     a[2][2]= 5.;
  282.     for (i=0; i<n; i++)
  283.         {for (j=0; j<n; j++)
  284.             printf("%8.4f ",a[i][j]);
  285.         printf("\n");
  286.         }
  287.     printf("\n");
  288.     cond=decomp(ndim,n,a,ipvt);
  289.     printf("cond = %f\n\n",cond);
  290.     printf("\nipvt\n");
  291.     for (i=0; i<n; i++)
  292.         printf("%8d \n",ipvt[i]);
  293.     condp1=cond+1.;
  294.     if(condp1==cond) exit();
  295.     b[0]=7.;
  296.     b[1]=4.;
  297.     b[2]=6.;
  298.     showv("RHS",b,n);
  299.     solve(ndim,n,a,b,ipvt);
  300.     showv("solution",b,n);
  301.     a[0][0]=10.;
  302.     a[1][0]=-3.;
  303.     a[2][0]= 5.;
  304.     a[0][1]=-7.;
  305.     a[1][1]= 2.;
  306.     a[2][1]=-1.;
  307.     a[0][2]= 0.;
  308.     a[1][2]= 6.;
  309.     a[2][2]= 5.;
  310.     showm("a, matrix to be inverted",a,ndim,n);
  311.     cond=invert(ndim,n,a,x);
  312.     printf("cond = %f\n\n",cond);
  313.     printf("ipvt\n");
  314.     for (i=0; i<n; i++)
  315.         printf("%8d \n",ipvt[i]);
  316.     condp1=cond+1.;
  317.     if(condp1==cond) exit();
  318.     showm("x, inverse",x,ndim,n);
  319.     a[0][0]=10.;
  320.     a[1][0]=-3.;
  321.     a[2][0]= 5.;
  322.     a[0][1]=-7.;
  323.     a[1][1]= 2.;
  324.     a[2][1]=-1.;
  325.     a[0][2]= 0.;
  326.     a[1][2]= 6.;
  327.     a[2][2]= 5.;
  328.     for (k=0; k<n; k++)
  329.         for (j=0; j<n; j++)
  330.             {p[k][j]=0.;
  331.             for (i=0; i<n; i++)
  332.                 p[k][j] += a[k][i]*x[i][j];
  333.             }
  334.     showm("a*x",p,ndim,n);
  335.     for (k=0; k<n; k++)
  336.         for (j=0; j<n; j++)
  337.             {p[k][j]=0.;
  338.             for (i=0; i<n; i++)
  339.                 p[k][j] += x[k][i]*a[i][j];
  340.             }
  341.     showm("x*a",p,ndim,n);
  342. }
  343.  
  344. showv(s,a,n) char *s; double a[]; int n;
  345. {    int i,j;
  346.     printf("%s\n",s);
  347.     for (i=0; i<n; i++)
  348.         printf("%8.4f \n",a[i]);
  349.     printf("\n");
  350. }
  351.  
  352. showm(s,a,ndim,n) char *s; double a[]; int ndim,n;
  353. {    int i,j;
  354.     printf("%s\n",s);
  355.     for (i=0; i<n; i++)
  356.         {for (j=0; j<n; j++)
  357.             printf("%8.4f ",a[i*ndim+j]);
  358.         printf("\n");
  359.         }
  360.     printf("\n");
  361. }
  362. #endif
  363.