bolster

CPU-solver

May 31st, 2011
1,224
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <stdlib.h>
  2. #include <stdio.h>
  3. #include <math.h>
  4.  
  5. #define MAT1 3
  6. #define TINY 1e-20
  7. #define a(i,j) a[(i)*MAT1+(j)]
  8.  
  9. void h_pivot_decomp(float *a, int *p, int *q){
  10.     int i,j,k;
  11.     int n=MAT1;
  12.     int pi,pj,tmp;
  13.     float max;
  14.     float ftmp;
  15.     for (k=0;k<n;k++){
  16.         pi=-1,pj=-1,max=0.0;
  17.         //find pivot in submatrix a(k:n,k:n)
  18.         for (i=k;i<n;i++) {
  19.             for (j=k;j<n;j++) {
  20.                 if (fabs(a(i,j))>max){
  21.                     max = fabs(a(i,j));
  22.                     pi=i;
  23.                     pj=j;
  24.                 }
  25.             }
  26.         }
  27.         //Swap Row
  28.         tmp=p[k];
  29.         p[k]=p[pi];
  30.         p[pi]=tmp;
  31.         for (j=0;j<n;j++){
  32.             ftmp=a(k,j);
  33.             a(k,j)=a(pi,j);
  34.             a(pi,j)=ftmp;
  35.         }
  36.         //Swap Col
  37.         tmp=q[k];
  38.         q[k]=q[pj];
  39.         q[pj]=tmp;
  40.         for (i=0;i<n;i++){
  41.             ftmp=a(i,k);
  42.             a(i,k)=a(i,pj);
  43.             a(i,pj)=ftmp;
  44.         }
  45.         //END PIVOT
  46.  
  47.         //check pivot size and decompose
  48.         if ((fabs(a(k,k))>TINY)){
  49.             for (i=k+1;i<n;i++){
  50.                 //Column normalisation
  51.                 ftmp=a(i,k)/=a(k,k);
  52.                 for (j=k+1;j<n;j++){
  53.                     //a(ik)*a(kj) subtracted from lower right submatrix elements
  54.                     a(i,j)-=(ftmp*a(k,j));
  55.                 }
  56.             }
  57.         }
  58.         //END DECOMPOSE
  59.     }
  60. }
  61.  
  62.  
  63. void h_solve(float *a, float *x, int *p, int *q){
  64.     //forward substitution; see  Golub, Van Loan 96
  65.     //And see http://www.cs.rutgers.edu/~richter/cs510/completePivoting.pdf
  66.     int i,ii=0,ip,j,tmp;
  67.     float ftmp;
  68.     float xtmp[MAT1];
  69.     //Swap rows (x=Px)
  70.     puts("x=Px Stage");
  71.     for (i=0; i<MAT1; i++){
  72.         xtmp[i]=x[p[i]]; //value that should be here
  73.         printf("x:%.1lf,q:%d\n",xtmp[i],q[i]);
  74.     }
  75.     //Lx=x
  76.     puts("Lx=x Stage");
  77.     for (i=0;i<MAT1;i++){
  78.         ftmp=xtmp[i];
  79.         if (ii != 0)
  80.             for (j=ii-1;j<i;j++)
  81.                 ftmp-=a(i,j)*xtmp[j];
  82.         else
  83.             if (ftmp!=0.0)
  84.                 ii=i+1;
  85.         xtmp[i]=ftmp;
  86.         printf("x:%.1lf,q:%d\n",xtmp[i],q[i]);
  87.     }
  88.     puts("Ux=x");
  89.     //backward substitution
  90.     //partially taken from Sourcebook on Parallel Computing p577
  91.     //solves Uy=z
  92.     xtmp[MAT1-1]/=a(MAT1-1,MAT1-1);
  93.     for (i=MAT1-2;i>=0;i--){
  94.         ftmp=xtmp[i];
  95.         for (j=i+1;j<MAT1;j++){
  96.             ftmp-=a(i,j)*xtmp[j];
  97.         }
  98.         xtmp[i]=(ftmp)/a(i,i);
  99.     }
  100.     for (i=0;i<MAT1;i++)
  101.         printf("x:%.1lf,q:%d\n",xtmp[i],q[i]);
  102.  
  103.     //Last bit
  104.     //solves x=Qy
  105.     puts("x=Qx Stage");
  106.     for (i=0;i<MAT1;i++){
  107.         x[i]=xtmp[q[i]];
  108.         printf("x:%.1lf,q:%d\n",x[i],q[i]);
  109.     }
  110. }
  111.  
  112.  
  113. void main(){
  114.     //3x3 Matrix
  115.     //float a[]={1,-2,3,2,-5,12,0,2,-10};
  116.     float a[]={1,3,-2,3,5,6,2,4,3};
  117.     float b[]={5,7,8};
  118.     //float a[]={1,2,3,2,-1,1,3,4,-1};
  119.     //float b[]={14,3,8};
  120.     //float a[]={1,-2,1,0,2,2,-2,4,2};
  121.     //float b[]={1,4,2};
  122.     int sig;
  123.     puts("Declared Stuff");
  124.  
  125.     //pivot array (not used currently)
  126.     int* p_pivot = (int *)malloc(sizeof(int)*MAT1);
  127.     int* q_pivot = (int *)malloc(sizeof(int)*MAT1);
  128.     puts("Starting Stuff");
  129.     for (unsigned int i=0; i<MAT1; i++){
  130.         p_pivot[i]=i;
  131.         q_pivot[i]=i;
  132.         printf("%.1lf|",b[i]);
  133.         for (unsigned int j=0;j<MAT1; j++){
  134.             printf("%.1lf,",a(i,j));
  135.         }
  136.         printf("|%d,%d",p_pivot[i],q_pivot[i]);
  137.         puts("");
  138.     }
  139.  
  140.     h_pivot_decomp(&a[0],p_pivot,q_pivot);
  141.     puts("After Pivot");
  142.     for (unsigned int i=0; i<MAT1; i++){
  143.         printf("%.1lf|",b[i]);
  144.         for (unsigned int j=0;j<MAT1; j++){
  145.             printf("%.1lf,",a(i,j));
  146.         }
  147.         printf("|%d,%d",p_pivot[i],q_pivot[i]);
  148.         puts("");
  149.     }
  150.  
  151.     h_solve(&a[0],&b[0],p_pivot,q_pivot);
  152.     puts("Finished Solve");
  153.  
  154.     for (unsigned int i=0; i<MAT1; i++){
  155.         printf("%.1lf|",b[i]);
  156.         for (unsigned int j=0;j<MAT1; j++){
  157.             printf("%.1lf,",a(i,j));
  158.         }
  159.         puts("");
  160.     }
  161. }
RAW Paste Data