home *** CD-ROM | disk | FTP | other *** search
/ vis-ftp.cs.umass.edu / vis-ftp.cs.umass.edu.tar / vis-ftp.cs.umass.edu / pub / Software / ASCENDER / NRroutines.c < prev    next >
C/C++ Source or Header  |  1996-04-25  |  11KB  |  583 lines
  1. #include "../polygons.h"
  2.  
  3. #define TINY 1.0e-20;
  4.  
  5. void nrerror(error_text)
  6. char error_text[];
  7. {
  8.     void exit();
  9.  
  10.     fprintf(stderr,"Numerical Recipes run-time error...\n");
  11.     fprintf(stderr,"%s\n",error_text);
  12.     fprintf(stderr,"...now exiting to system...\n");
  13.     exit(1);
  14. }
  15.  
  16.  
  17.  
  18. float *vector(int nl,int nh)
  19. {
  20.     float *v;
  21.  
  22.     v=(float *)malloc((unsigned) (nh-nl+1)*sizeof(float));
  23.     if (!v) nrerror("allocation failure in vector()");
  24.     return v-nl;
  25. }
  26.  
  27. int *ivector(int nl,int nh)
  28. {
  29.     int *v;
  30.  
  31.     v=(int *)malloc((unsigned) (nh-nl+1)*sizeof(int));
  32.     if (!v) nrerror("allocation failure in ivector()");
  33.     return v-nl;
  34. }
  35.  
  36. double *dvector(int nl,int nh)
  37. {
  38.     double *v;
  39.  
  40.     v=(double *)malloc((unsigned) (nh-nl+1)*sizeof(double));
  41.     if (!v) nrerror("allocation failure in dvector()");
  42.     return v-nl;
  43. }
  44.  
  45.  
  46.  
  47. float **matrix(int nrl,int nrh,int ncl,int nch)
  48. {
  49.     int i;
  50.     float **m;
  51.  
  52.     m=(float **) malloc((unsigned) (nrh-nrl+1)*sizeof(float*));
  53.     if (!m) nrerror("allocation failure 1 in matrix()");
  54.     m -= nrl;
  55.  
  56.     for(i=nrl;i<=nrh;i++) {
  57.         m[i]=(float *) malloc((unsigned) (nch-ncl+1)*sizeof(float));
  58.         if (!m[i]) nrerror("allocation failure 2 in matrix()");
  59.         m[i] -= ncl;
  60.     }
  61.     return m;
  62. }
  63.  
  64. double **dmatrix(int nrl,int nrh,int ncl,int nch)
  65. {
  66.     int i;
  67.     double **m;
  68.  
  69.     m=(double **) malloc((unsigned) (nrh-nrl+1)*sizeof(double*));
  70.     if (!m) nrerror("allocation failure 1 in dmatrix()");
  71.     m -= nrl;
  72.  
  73.     for(i=nrl;i<=nrh;i++) {
  74.         m[i]=(double *) malloc((unsigned) (nch-ncl+1)*sizeof(double));
  75.         if (!m[i]) nrerror("allocation failure 2 in dmatrix()");
  76.         m[i] -= ncl;
  77.     }
  78.     return m;
  79. }
  80.  
  81. int **imatrix(int nrl,int nrh,int ncl,int nch)
  82. {
  83.     int i,**m;
  84.  
  85.     m=(int **)malloc((unsigned) (nrh-nrl+1)*sizeof(int*));
  86.     if (!m) nrerror("allocation failure 1 in imatrix()");
  87.     m -= nrl;
  88.  
  89.     for(i=nrl;i<=nrh;i++) {
  90.         m[i]=(int *)malloc((unsigned) (nch-ncl+1)*sizeof(int));
  91.         if (!m[i]) nrerror("allocation failure 2 in imatrix()");
  92.         m[i] -= ncl;
  93.     }
  94.     return m;
  95. }
  96.  
  97.  
  98.  
  99. float **submatrix(float **a,int oldrl,int oldrh,int oldcl,
  100.           int oldch,int newrl,int newcl)
  101. {
  102.     int i,j;
  103.     float **m;
  104.  
  105.     m=(float **) malloc((unsigned) (oldrh-oldrl+1)*sizeof(float*));
  106.     if (!m) nrerror("allocation failure in submatrix()");
  107.     m -= newrl;
  108.  
  109.     for(i=oldrl,j=newrl;i<=oldrh;i++,j++) m[j]=a[i]+oldcl-newcl;
  110.  
  111.     return m;
  112. }
  113.  
  114.  
  115.  
  116. void free_vector(float *v,int nl,int nh)
  117. {
  118.     free((char*) (v+nl));
  119. }
  120.  
  121. void free_ivector(int *v,int nl,int nh)
  122. {
  123.     free((char*) (v+nl));
  124. }
  125.  
  126. void free_dvector(double *v,int nl,int nh)
  127. {
  128.     free((char*) (v+nl));
  129. }
  130.  
  131.  
  132.  
  133. void free_matrix(float **m,int nrl,int nrh,int ncl,int nch)
  134. {
  135.     int i;
  136.  
  137.     for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
  138.     free((char*) (m+nrl));
  139. }
  140.  
  141. void free_dmatrix(double **m,int nrl,int nrh,int ncl,int nch)
  142. {
  143.     int i;
  144.  
  145.     for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
  146.     free((char*) (m+nrl));
  147. }
  148.  
  149. void free_imatrix(int **m,int nrl,int nrh,int ncl,int nch)
  150. {
  151.     int i;
  152.  
  153.     for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
  154.     free((char*) (m+nrl));
  155. }
  156.  
  157.  
  158.  
  159. void free_submatrix(float **b,int nrl,int nrh,int ncl,int nch)
  160. {
  161.     free((char*) (b+nrl));
  162. }
  163.  
  164.  
  165.  
  166. float **convert_matrix(float *a,int nrl,int nrh,int ncl,int nch)
  167. {
  168.     int i,j,nrow,ncol;
  169.     float **m;
  170.  
  171.     nrow=nrh-nrl+1;
  172.     ncol=nch-ncl+1;
  173.     m = (float **) malloc((unsigned) (nrow)*sizeof(float*));
  174.     if (!m) nrerror("allocation failure in convert_matrix()");
  175.     m -= nrl;
  176.     for(i=0,j=nrl;i<=nrow-1;i++,j++) m[j]=a+ncol*i-ncl;
  177.     return m;
  178. }
  179.  
  180.  
  181.  
  182. void free_convert_matrix(float **b,int nrl,int nrh,int ncl,int nch)
  183. {
  184.     free((char*) (b+nrl));
  185. }
  186.  
  187.  
  188.  
  189.  
  190. void ludcmp(float **a,int n,int *indx,float *d)
  191. {
  192.     int i,imax,j,k;
  193.     float big,dum,sum,temp;
  194.     float *vv,*vector();
  195.     void nrerror(),free_vector();
  196.  
  197.     vv=vector(1,n);
  198.     *d=1.0;
  199.     for (i=1;i<=n;i++) {
  200.         big=0.0;
  201.         for (j=1;j<=n;j++)
  202.             if ((temp=fabs(a[i][j])) > big) big=temp;
  203.         if (big == 0.0) nrerror("Singular matrix in routine LUDCMP");
  204.         vv[i]=1.0/big;
  205.     }
  206.     for (j=1;j<=n;j++) {
  207.         for (i=1;i<j;i++) {
  208.             sum=a[i][j];
  209.             for (k=1;k<i;k++) sum -= a[i][k]*a[k][j];
  210.             a[i][j]=sum;
  211.         }
  212.         big=0.0;
  213.         for (i=j;i<=n;i++) {
  214.             sum=a[i][j];
  215.             for (k=1;k<j;k++)
  216.                 sum -= a[i][k]*a[k][j];
  217.             a[i][j]=sum;
  218.             if ( (dum=vv[i]*fabs(sum)) >= big) {
  219.                 big=dum;
  220.                 imax=i;
  221.             }
  222.         }
  223.         if (j != imax) {
  224.             for (k=1;k<=n;k++) {
  225.                 dum=a[imax][k];
  226.                 a[imax][k]=a[j][k];
  227.                 a[j][k]=dum;
  228.             }
  229.             *d = -(*d);
  230.             vv[imax]=vv[j];
  231.         }
  232.         indx[j]=imax;
  233.         if (a[j][j] == 0.0) a[j][j]=TINY;
  234.         if (j != n) {
  235.             dum=1.0/(a[j][j]);
  236.             for (i=j+1;i<=n;i++) a[i][j] *= dum;
  237.         }
  238.     }
  239.     free_vector(vv,1,n);
  240. }
  241.  
  242. void bksub(int ne,int nb,int jf,int k1,int k2,float ***c)
  243. {
  244.     int nbf,im,kp,k,j,i;
  245.     float xx;
  246.  
  247.     nbf=ne-nb;
  248.     im=1;
  249.     for (k=k2;k>=k1;k--) {
  250.         if (k == k1) im=nbf+1;
  251.         kp=k+1;
  252.         for (j=1;j<=nbf;j++) {
  253.             xx=c[j][jf][kp];
  254.             for (i=im;i<=ne;i++)
  255.                 c[i][jf][k] -= c[i][j][k]*xx;
  256.         }
  257.     }
  258.     for (k=k1;k<=k2;k++) {
  259.         kp=k+1;
  260.         for (i=1;i<=nb;i++) c[i][1][k]=c[i+nbf][jf][k];
  261.         for (i=1;i<=nbf;i++) c[i+nb][1][k]=c[i][jf][kp];
  262.     }
  263. }
  264.  
  265.  
  266. void lubksb(float **a,int n,int *indx,float b[])
  267. {
  268.     int i,ii=0,ip,j;
  269.     float sum;
  270.  
  271.     for (i=1;i<=n;i++) {
  272.         ip=indx[i];
  273.         sum=b[ip];
  274.         b[ip]=b[i];
  275.         if (ii)
  276.             for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j];
  277.         else if (sum) ii=i;
  278.         b[i]=sum;
  279.     }
  280.     for (i=n;i>=1;i--) {
  281.         sum=b[i];
  282.         for (j=i+1;j<=n;j++) sum -= a[i][j]*b[j];
  283.         b[i]=sum/a[i][i];
  284.     }
  285. }
  286.  
  287.  
  288. #define ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);\
  289.     a[k][l]=h+s*(g-h*tau);
  290.  
  291. void jacobi(a,n,d,v,nrot)
  292. float **a,d[],**v;
  293. int n,*nrot;
  294. {
  295.     int j,iq,ip,i;
  296.     float tresh,theta,tau,t,sm,s,h,g,c,*b,*z,*vector();
  297.     void nrerror(),free_vector();
  298.  
  299.     b=vector(1,n);
  300.     z=vector(1,n);
  301.     for (ip=1;ip<=n;ip++) {
  302.         for (iq=1;iq<=n;iq++) v[ip][iq]=0.0;
  303.         v[ip][ip]=1.0;
  304.     }
  305.     for (ip=1;ip<=n;ip++) {
  306.         b[ip]=d[ip]=a[ip][ip];
  307.         z[ip]=0.0;
  308.     }
  309.     *nrot=0;
  310.     for (i=1;i<=50;i++) {
  311.         sm=0.0;
  312.         for (ip=1;ip<=n-1;ip++) {
  313.             for (iq=ip+1;iq<=n;iq++)
  314.                 sm += fabs(a[ip][iq]);
  315.         }
  316.         if (sm == 0.0) {
  317.             free_vector(z,1,n);
  318.             free_vector(b,1,n);
  319.             return;
  320.         }
  321.         if (i < 4)
  322.             tresh=0.2*sm/(n*n);
  323.         else
  324.             tresh=0.0;
  325.         for (ip=1;ip<=n-1;ip++) {
  326.             for (iq=ip+1;iq<=n;iq++) {
  327.                 g=100.0*fabs(a[ip][iq]);
  328.                 if (i > 4 && fabs(d[ip])+g == fabs(d[ip])
  329.                     && fabs(d[iq])+g == fabs(d[iq]))
  330.                     a[ip][iq]=0.0;
  331.                 else if (fabs(a[ip][iq]) > tresh) {
  332.                     h=d[iq]-d[ip];
  333.                     if (fabs(h)+g == fabs(h))
  334.                         t=(a[ip][iq])/h;
  335.                     else {
  336.                         theta=0.5*h/(a[ip][iq]);
  337.                         t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
  338.                         if (theta < 0.0) t = -t;
  339.                     }
  340.                     c=1.0/sqrt(1+t*t);
  341.                     s=t*c;
  342.                     tau=s/(1.0+c);
  343.                     h=t*a[ip][iq];
  344.                     z[ip] -= h;
  345.                     z[iq] += h;
  346.                     d[ip] -= h;
  347.                     d[iq] += h;
  348.                     a[ip][iq]=0.0;
  349.                     for (j=1;j<=ip-1;j++) {
  350.                         ROTATE(a,j,ip,j,iq)
  351.                     }
  352.                     for (j=ip+1;j<=iq-1;j++) {
  353.                         ROTATE(a,ip,j,j,iq)
  354.                     }
  355.                     for (j=iq+1;j<=n;j++) {
  356.                         ROTATE(a,ip,j,iq,j)
  357.                     }
  358.                     for (j=1;j<=n;j++) {
  359.                         ROTATE(v,j,ip,j,iq)
  360.                     }
  361.                     ++(*nrot);
  362.                 }
  363.             }
  364.         }
  365.         for (ip=1;ip<=n;ip++) {
  366.             b[ip] += z[ip];
  367.             d[ip]=b[ip];
  368.             z[ip]=0.0;
  369.         }
  370.     }
  371.     nrerror("Too many iterations in routine JACOBI");
  372. }
  373.  
  374. #undef ROTATE
  375.  
  376. static float at,bt,ct;
  377. #define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? \
  378. (ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0))
  379.  
  380. static float maxarg1,maxarg2;
  381. #define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
  382.     (maxarg1) : (maxarg2))
  383. #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
  384.  
  385. void svdcmp(a,m,n,w,v)
  386. float **a,*w,**v;
  387. int m,n;
  388. {
  389.     int flag,i,its,j,jj,k,l,nm;
  390.     float c,f,h,s,x,y,z;
  391.     float anorm=0.0,g=0.0,scale=0.0;
  392.     float *rv1,*vector();
  393.     void nrerror(),free_vector();
  394.  
  395.     if (m < n) nrerror("SVDCMP: You must augment A with extra zero rows");
  396.     rv1=vector(1,n);
  397.     for (i=1;i<=n;i++) {
  398.         l=i+1;
  399.         rv1[i]=scale*g;
  400.         g=s=scale=0.0;
  401.         if (i <= m) {
  402.             for (k=i;k<=m;k++) scale += fabs(a[k][i]);
  403.             if (scale) {
  404.                 for (k=i;k<=m;k++) {
  405.                     a[k][i] /= scale;
  406.                     s += a[k][i]*a[k][i];
  407.                 }
  408.                 f=a[i][i];
  409.                 g = -SIGN(sqrt(s),f);
  410.                 h=f*g-s;
  411.                 a[i][i]=f-g;
  412.                 if (i != n) {
  413.                     for (j=l;j<=n;j++) {
  414.                         for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
  415.                         f=s/h;
  416.                         for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
  417.                     }
  418.                 }
  419.                 for (k=i;k<=m;k++) a[k][i] *= scale;
  420.             }
  421.         }
  422.         w[i]=scale*g;
  423.         g=s=scale=0.0;
  424.         if (i <= m && i != n) {
  425.             for (k=l;k<=n;k++) scale += fabs(a[i][k]);
  426.             if (scale) {
  427.                 for (k=l;k<=n;k++) {
  428.                     a[i][k] /= scale;
  429.                     s += a[i][k]*a[i][k];
  430.                 }
  431.                 f=a[i][l];
  432.                 g = -SIGN(sqrt(s),f);
  433.                 h=f*g-s;
  434.                 a[i][l]=f-g;
  435.                 for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
  436.                 if (i != m) {
  437.                     for (j=l;j<=m;j++) {
  438.                         for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
  439.                         for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
  440.                     }
  441.                 }
  442.                 for (k=l;k<=n;k++) a[i][k] *= scale;
  443.             }
  444.         }
  445.         anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
  446.     }
  447.     for (i=n;i>=1;i--) {
  448.         if (i < n) {
  449.             if (g) {
  450.                 for (j=l;j<=n;j++)
  451.                     v[j][i]=(a[i][j]/a[i][l])/g;
  452.                 for (j=l;j<=n;j++) {
  453.                     for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
  454.                     for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
  455.                 }
  456.             }
  457.             for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
  458.         }
  459.         v[i][i]=1.0;
  460.         g=rv1[i];
  461.         l=i;
  462.     }
  463.     for (i=n;i>=1;i--) {
  464.         l=i+1;
  465.         g=w[i];
  466.         if (i < n)
  467.             for (j=l;j<=n;j++) a[i][j]=0.0;
  468.         if (g) {
  469.             g=1.0/g;
  470.             if (i != n) {
  471.                 for (j=l;j<=n;j++) {
  472.                     for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
  473.                     f=(s/a[i][i])*g;
  474.                     for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
  475.                 }
  476.             }
  477.             for (j=i;j<=m;j++) a[j][i] *= g;
  478.         } else {
  479.             for (j=i;j<=m;j++) a[j][i]=0.0;
  480.         }
  481.         ++a[i][i];
  482.     }
  483.     for (k=n;k>=1;k--) {
  484.         for (its=1;its<=30;its++) {
  485.             flag=1;
  486.             for (l=k;l>=1;l--) {
  487.                 nm=l-1;
  488.                 if (fabs(rv1[l])+anorm == anorm) {
  489.                     flag=0;
  490.                     break;
  491.                 }
  492.                 if (fabs(w[nm])+anorm == anorm) break;
  493.             }
  494.             if (flag) {
  495.                 c=0.0;
  496.                 s=1.0;
  497.                 for (i=l;i<=k;i++) {
  498.                     f=s*rv1[i];
  499.                     if (fabs(f)+anorm != anorm) {
  500.                         g=w[i];
  501.                         h=PYTHAG(f,g);
  502.                         w[i]=h;
  503.                         h=1.0/h;
  504.                         c=g*h;
  505.                         s=(-f*h);
  506.                         for (j=1;j<=m;j++) {
  507.                             y=a[j][nm];
  508.                             z=a[j][i];
  509.                             a[j][nm]=y*c+z*s;
  510.                             a[j][i]=z*c-y*s;
  511.                         }
  512.                     }
  513.                 }
  514.             }
  515.             z=w[k];
  516.             if (l == k) {
  517.                 if (z < 0.0) {
  518.                     w[k] = -z;
  519.                     for (j=1;j<=n;j++) v[j][k]=(-v[j][k]);
  520.                 }
  521.                 break;
  522.             }
  523.             if (its == 30)  {
  524.                 w[1] = 0.0;
  525.                 return;
  526.             }
  527.             x=w[l];
  528.             nm=k-1;
  529.             y=w[nm];
  530.             g=rv1[nm];
  531.             h=rv1[k];
  532.             f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
  533.             g=PYTHAG(f,1.0);
  534.             f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
  535.             c=s=1.0;
  536.             for (j=l;j<=nm;j++) {
  537.                 i=j+1;
  538.                 g=rv1[i];
  539.                 y=w[i];
  540.                 h=s*g;
  541.                 g=c*g;
  542.                 z=PYTHAG(f,h);
  543.                 rv1[j]=z;
  544.                 c=f/z;
  545.                 s=h/z;
  546.                 f=x*c+g*s;
  547.                 g=g*c-x*s;
  548.                 h=y*s;
  549.                 y=y*c;
  550.                 for (jj=1;jj<=n;jj++) {
  551.                     x=v[jj][j];
  552.                     z=v[jj][i];
  553.                     v[jj][j]=x*c+z*s;
  554.                     v[jj][i]=z*c-x*s;
  555.                 }
  556.                 z=PYTHAG(f,h);
  557.                 w[j]=z;
  558.                 if (z) {
  559.                     z=1.0/z;
  560.                     c=f*z;
  561.                     s=h*z;
  562.                 }
  563.                 f=(c*g)+(s*y);
  564.                 x=(c*y)-(s*g);
  565.                 for (jj=1;jj<=m;jj++) {
  566.                     y=a[jj][j];
  567.                     z=a[jj][i];
  568.                     a[jj][j]=y*c+z*s;
  569.                     a[jj][i]=z*c-y*s;
  570.                 }
  571.             }
  572.             rv1[l]=0.0;
  573.             rv1[k]=f;
  574.             w[k]=x;
  575.         }
  576.     }
  577.     free_vector(rv1,1,n);
  578. }
  579.  
  580. #undef SIGN
  581. #undef MAX
  582. #undef PYTHAG
  583.