home *** CD-ROM | disk | FTP | other *** search
/ Crawly Crypt Collection 1 / crawlyvol1.bin / apps / math / euler / source / matheh.c < prev    next >
C/C++ Source or Header  |  1992-09-08  |  24KB  |  1,000 lines
  1. #include <stdio.h>
  2. #include <stdlib.h>
  3. #include <string.h>
  4. #include <math.h>
  5. #include <float.h>
  6.  
  7. #include "header.h"
  8. #include "sysdep.h"
  9. #include "matheh.h"
  10.  
  11. char *ram;
  12. int *perm,*col,signdet,luflag=0;
  13. double **lumat,*c,det;
  14. complex **c_lumat,*c_c,c_det;
  15. int rank;
  16.  
  17. #define outofram() { output("Out of Memory!\n"); error=120; return; }
  18. #define wrong_arg() { output("Wrong arguments!\n"); error=1000; return; }
  19.  
  20. /***************** real linear systems *******************/
  21.  
  22. void lu (double *a, int n, int m)
  23. /***** lu
  24.     lu decomposition of a
  25. *****/
  26. {    int i,j,k,mm,j0,kh;
  27.     double *d,piv,temp,*temp1,zmax,help;
  28.     
  29.     if (!luflag)
  30.     {
  31.         /* get place for result c and move a to c */
  32.         c=(double *)ram;
  33.         ram+=(LONG)n*m*sizeof(double);
  34.         if (ram>ramend) outofram();
  35.         memmove((char *)c,(char *)a,(LONG)n*m*sizeof(double));
  36.     }
  37.     else c=a;
  38.         
  39.     /* inititalize lumat */
  40.     lumat=(double **)ram;
  41.     ram+=(LONG)n*sizeof(double *);
  42.     if (ram>ramend) outofram();
  43.     d=c; 
  44.     for (i=0; i<n; i++) { lumat[i]=d; d+=m; }
  45.     
  46.     /* get place for perm */
  47.     perm=(int *)ram;
  48.     ram+=(LONG)n*sizeof(int);
  49.     if (ram>ramend) outofram();
  50.     
  51.     /* get place for col */
  52.     col=(int *)ram;
  53.     ram+=(LONG)m*sizeof(int);
  54.     if (ram>ramend) outofram();
  55.     
  56.     /* d is a vector needed by the algorithm */
  57.     d=(double *)ram;
  58.     ram+=(LONG)n*sizeof(double);
  59.     if (ram>ramend) outofram();
  60.     
  61.     /* gauss algorithm */
  62.     if (!luflag)
  63.         for (k=0; k<n; k++)
  64.         {    perm[k]=k;
  65.             for (zmax=0.0, j=0; j<m; j++)
  66.                 if ( (help=fabs(lumat[k][j])) >zmax) zmax=help;
  67.             if (zmax==0.0) { error=130; return; }
  68.             d[k]=zmax;
  69.         }
  70.     else
  71.     {    for (k=0; k<n; k++) perm[k]=k;
  72.     }
  73.     signdet=1; rank=0; det=1.0; k=0;
  74.     for (kh=0; kh<m; kh++)
  75.     {    piv=luflag?fabs(lumat[k][kh]):(fabs(lumat[k][kh])/d[k]);
  76.         j0=k;
  77.         for (j=k+1; j<n; j++)
  78.         {    temp=luflag?fabs(lumat[j][kh]):(fabs(lumat[j][kh])/d[j]);
  79.             if (piv<temp)
  80.             {    piv=temp; j0=j;
  81.             }
  82.         }
  83.         if (piv<epsilon)
  84.         {    signdet=0;
  85.             if (luflag)
  86.             {    col[kh]=0;
  87.                 continue;
  88.             }
  89.             else
  90.             {    output("Determinant zero!\n"); 
  91.                 error=131; return;
  92.             }
  93.         }
  94.         else
  95.         {    col[kh]=1; rank++;
  96.             det*=lumat[j0][kh];
  97.         }
  98.         if (j0!=k)
  99.         {    signdet=-signdet;
  100.             mm=perm[j0]; perm[j0]=perm[k]; perm[k]=mm;
  101.             if (!luflag)
  102.             { temp=d[j0]; d[j0]=d[k]; d[k]=temp; }
  103.             temp1=lumat[j0]; lumat[j0]=lumat[k]; lumat[k]=temp1;
  104.         }
  105.         for (j=k+1; j<n; j++)
  106.             if (lumat[j][kh] != 0.0)
  107.             {    lumat[j][kh] /= lumat[k][kh];
  108.                 for (temp=lumat[j][kh], mm=kh+1; mm<m; mm++)
  109.                     lumat[j][mm]-=temp*lumat[k][mm];
  110.             }
  111.         k++;
  112.         if (k>=n) { kh++; break; }
  113.     }
  114.     if (k<n || kh<m)
  115.     {    signdet=0;
  116.         if (!luflag)
  117.         {    error=131; output("Determinant zero!\n"); 
  118.             return;
  119.         }
  120.     }
  121.     for (j=kh; j<m; j++) col[j]=0;
  122.     det=det*signdet;
  123. }
  124.  
  125. void solvesim (double *a, int n, double *rs, int m, double *res)
  126. /**** solvesim
  127.     solve simultanuously a linear system.
  128. ****/
  129. {    double **x,**b,*h,sum;
  130.     int i,k,l,j;
  131.     ram=newram; luflag=0;
  132.     lu(a,n,n); if (error) return;
  133.     
  134.     /* initialize x and b */
  135.     x=(double **)ram;
  136.     ram+=(LONG)n*sizeof(double *);
  137.     if (ram>ramend) outofram();
  138.     h=res; for (i=0; i<n; i++) { x[i]=h; h+=m; }
  139.     b=(double **)ram;
  140.     ram+=(LONG)n*sizeof(double *);
  141.     if (ram>ramend) outofram();
  142.     h=rs; for (i=0; i<n; i++) { b[i]=h; h+=m; }
  143.     
  144.     for (l=0; l<m; l++)
  145.     {    x[0][l]=b[perm[0]][l];
  146.         for (k=1; k<n; k++)
  147.         {    x[k][l]=b[perm[k]][l];
  148.             for (j=0; j<k; j++)
  149.                 x[k][l] -= lumat[k][j]*x[j][l];
  150.         }
  151.         x[n-1][l] /= lumat[n-1][n-1];
  152.         for (k=n-2; k>=0; k--)
  153.         {    for (sum=0.0, j=k+1; j<n; j++)
  154.                 sum+=lumat[k][j]*x[j][l];
  155.             x[k][l] = (x[k][l]-sum)/lumat[k][k];
  156.         }
  157.     }
  158. }
  159.  
  160. void make_lu (double *a, int n, int m, 
  161.     int **rows, int **cols, int *rankp,
  162.     double *detp)
  163. {    ram=newram;
  164.     luflag=1; lu(a,n,m); newram=ram;
  165.     if (error) return;
  166.     *rows=perm; *cols=col; *rankp=rank; *detp=det;
  167. }
  168.  
  169. void lu_solve (double *a, int n, double *rs, int m, double *res)
  170. {    double **x,**b,*h,sum,*d;
  171.     int i,k,l,j;
  172.     ram=newram;
  173.     
  174.     /* initialize x and b */
  175.     x=(double **)ram;
  176.     ram+=(LONG)n*sizeof(double *);
  177.     if (ram>ramend) outofram();
  178.     h=res; for (i=0; i<n; i++) { x[i]=h; h+=m; }
  179.  
  180.     b=(double **)ram;
  181.     ram+=(LONG)n*sizeof(double *);
  182.     if (ram>ramend) outofram();
  183.     h=rs; for (i=0; i<n; i++) { b[i]=h; h+=m; }
  184.     
  185.     /* inititalize lumat */
  186.     lumat=(double **)ram;
  187.     ram+=(LONG)n*sizeof(double *);
  188.     if (ram>ramend) outofram();
  189.     d=a; 
  190.     for (i=0; i<n; i++) { lumat[i]=d; d+=n; }
  191.     
  192.     for (l=0; l<m; l++)
  193.     {    x[0][l]=b[0][l];
  194.         for (k=1; k<n; k++)
  195.         {    x[k][l]=b[k][l];
  196.             for (j=0; j<k; j++)
  197.                 x[k][l] -= lumat[k][j]*x[j][l];
  198.         }
  199.         x[n-1][l] /= lumat[n-1][n-1];
  200.         for (k=n-2; k>=0; k--)
  201.         {    for (sum=0.0, j=k+1; j<n; j++)
  202.                 sum+=lumat[k][j]*x[j][l];
  203.             x[k][l] = (x[k][l]-sum)/lumat[k][k];
  204.         }
  205.     }
  206. }
  207.  
  208. /******************* complex linear systems **************/
  209.  
  210. void c_add (complex x, complex y, complex z)
  211. {    z[0]=x[0]+y[0];
  212.     z[1]=x[1]+y[1];
  213. }
  214.  
  215. void c_sub (complex x, complex y, complex z)
  216. {    z[0]=x[0]-y[0];
  217.     z[1]=x[1]-y[1];
  218. }
  219.  
  220. void c_mult (complex x, complex y, complex z)
  221. {    double h;
  222.     h=x[0]*y[0]-x[1]*y[1];
  223.     z[1]=x[0]*y[1]+x[1]*y[0];
  224.     z[0]=h;
  225. }
  226.  
  227. void c_div (complex x, complex y, complex z)
  228. {    double r,h;
  229.     r=y[0]*y[0]+y[1]*y[1];
  230.     h=(x[0]*y[0]+x[1]*y[1])/r;
  231.     z[1]=(x[1]*y[0]-x[0]*y[1])/r;
  232.     z[0]=h;
  233. }
  234.  
  235. double c_abs (complex x)
  236. {    return sqrt(x[0]*x[0]+x[1]*x[1]);
  237. }
  238.  
  239. void c_copy (complex x, complex y)
  240. {    x[0]=y[0];
  241.     x[1]=y[1];
  242. }
  243.  
  244. void c_lu (double *a, int n, int m)
  245. /***** clu
  246.     lu decomposition of a
  247. *****/
  248. {    int i,j,k,mm,j0,kh;
  249.     double *d,piv,temp,zmax,help;
  250.     complex t,*h,*temp1;
  251.     
  252.     if (!luflag)
  253.     {    /* get place for result c and move a to c */
  254.         c_c=(complex *)ram;
  255.         ram+=(LONG)n*m*sizeof(complex);
  256.         if (ram>ramend) outofram();
  257.         memmove((char *)c_c,(char *)a,(LONG)n*m*sizeof(complex));
  258.     }
  259.     else c_c=(complex *)a;
  260.     
  261.     /* inititalize c_lumat */
  262.     c_lumat=(complex **)ram;
  263.     ram+=(LONG)n*sizeof(complex *);
  264.     if (ram>ramend) outofram();
  265.     h=c_c; 
  266.     for (i=0; i<n; i++) { c_lumat[i]=h; h+=m; }
  267.     
  268.     /* get place for perm */
  269.     perm=(int *)ram;
  270.     ram+=(LONG)n*sizeof(int);
  271.     if (ram>ramend) outofram();
  272.     
  273.     /* get place for col */
  274.     col=(int *)ram;
  275.     ram+=(LONG)m*sizeof(int);
  276.     if (ram>ramend) outofram();
  277.     
  278.     /* d is a vector needed by the algorithm */
  279.     d=(double *)ram;
  280.     ram+=(LONG)n*sizeof(double);
  281.     if (ram>ramend) outofram();
  282.     
  283.     /* gauss algorithm */
  284.     if (!luflag)
  285.         for (k=0; k<n; k++)
  286.         {    perm[k]=k;
  287.             for (zmax=0.0, j=0; j<m; j++)
  288.                 if ( (help=c_abs(c_lumat[k][j])) >zmax) zmax=help;
  289.             if (zmax==0.0) { error=130; return; }
  290.             d[k]=zmax;
  291.         }
  292.     else
  293.     {    for (k=0; k<n; k++) perm[k]=k;
  294.     }
  295.     signdet=1; rank=0; c_det[0]=1.0; c_det[1]=0.0; k=0;
  296.     for (kh=0; kh<m; kh++)
  297.     {    piv=luflag?c_abs(c_lumat[k][kh]):(c_abs(c_lumat[k][kh])/d[k]);
  298.         j0=k;
  299.         for (j=k+1; j<n; j++)
  300.         {    temp=luflag?c_abs(c_lumat[j][kh]):(c_abs(c_lumat[j][kh])/d[j]);
  301.             if (piv<temp)
  302.             {    piv=temp; j0=j;
  303.             }
  304.         }
  305.         if (piv<epsilon)
  306.         {    signdet=0;
  307.             if (luflag)
  308.             {    col[kh]=0;
  309.                 continue;
  310.             }
  311.             else
  312.             {    output("Determinant zero!\n"); 
  313.                 error=131; return;
  314.             }
  315.         }
  316.         else
  317.         {    col[kh]=1; rank++;
  318.             c_mult(c_det,c_lumat[j0][kh],c_det);
  319.         }
  320.         if (j0!=k)
  321.         {    signdet=-signdet;
  322.             mm=perm[j0]; perm[j0]=perm[k]; perm[k]=mm;
  323.             if (!luflag)
  324.             {    temp=d[j0]; d[j0]=d[k]; d[k]=temp; }
  325.             temp1=c_lumat[j0]; c_lumat[j0]=c_lumat[k]; 
  326.                 c_lumat[k]=temp1;
  327.         }
  328.         for (j=k+1; j<n; j++)
  329.             if (c_lumat[j][kh][0] != 0.0 || c_lumat[j][kh][1]!=0.0)
  330.             {    c_div(c_lumat[j][kh],c_lumat[k][kh],c_lumat[j][kh]);
  331.                 for (mm=kh+1; mm<m; mm++)
  332.                 {    c_mult(c_lumat[j][kh],c_lumat[k][mm],t);
  333.                     c_sub(c_lumat[j][mm],t,c_lumat[j][mm]);
  334.                 }
  335.             }
  336.         k++;
  337.         if (k>=n) { kh++; break; }
  338.     }
  339.     if (k<n || kh<m)
  340.     {    signdet=0;
  341.         if (!luflag)
  342.         {    error=131; output("Determinant zero!\n"); 
  343.             return;
  344.         }
  345.     }
  346.     for (j=kh; j<m; j++) col[j]=0;
  347.     c_det[0]=c_det[0]*signdet; c_det[1]=c_det[1]*signdet;
  348. }
  349.  
  350. void c_solvesim (double *a, int n, double *rs, int m, double *res)
  351. /**** solvesim
  352.     solve simultanuously a linear system.
  353. ****/
  354. {    complex **x,**b,*h;
  355.     complex sum,t;
  356.     int i,k,l,j;
  357.     ram=newram;
  358.     luflag=0; c_lu(a,n,n); if (error) return;
  359.     
  360.     /* initialize x and b */
  361.     x=(complex **)ram;
  362.     ram+=(LONG)n*sizeof(complex *);
  363.     if (ram>ramend) outofram();
  364.     h=(complex *)res; for (i=0; i<n; i++) { x[i]=h; h+=m; }
  365.     
  366.     b=(complex **)ram;
  367.     ram+=(LONG)n*sizeof(complex *);
  368.     if (ram>ramend) outofram();
  369.     h=(complex *)rs; for (i=0; i<n; i++) { b[i]=h; h+=m; }
  370.     
  371.     for (l=0; l<m; l++)
  372.     {    c_copy(x[0][l],b[perm[0]][l]);
  373.         for (k=1; k<n; k++)
  374.         {    c_copy(x[k][l],b[perm[k]][l]);
  375.             for (j=0; j<k; j++)
  376.             {    c_mult(c_lumat[k][j],x[j][l],t);
  377.                 c_sub(x[k][l],t,x[k][l]);
  378.             }
  379.         }
  380.         c_div(x[n-1][l],c_lumat[n-1][n-1],x[n-1][l]);
  381.         for (k=n-2; k>=0; k--)
  382.         {    sum[0]=0; sum[1]=0.0;
  383.             for (j=k+1; j<n; j++)
  384.             {    c_mult(c_lumat[k][j],x[j][l],t);
  385.                 c_add(sum,t,sum);
  386.             }
  387.             c_sub(x[k][l],sum,t);
  388.             c_div(t,c_lumat[k][k],x[k][l]);
  389.         }
  390.     }
  391. }
  392.  
  393. void cmake_lu (double *a, int n, int m,
  394.     int **rows, int **cols, int *rankp,
  395.     double *detp, double *detip)
  396. {    ram=newram;
  397.     luflag=1; c_lu(a,n,m); newram=ram;
  398.     if (error) return;
  399.     *rows=perm; *cols=col; *rankp=rank; 
  400.     *detp=c_det[0]; *detip=c_det[1];
  401. }
  402.  
  403. void clu_solve (double *a, int n, double *rs, int m, double *res)
  404. /**** solvesim
  405.     solve simultanuously a linear system.
  406. ****/
  407. {    complex **x,**b,*h;
  408.     complex sum,t;
  409.     int i,k,l,j;
  410.     ram=newram;
  411.     
  412.     /* initialize x and b */
  413.     x=(complex **)ram;
  414.     ram+=(LONG)n*sizeof(complex *);
  415.     if (ram>ramend) outofram();
  416.     h=(complex *)res; for (i=0; i<n; i++) { x[i]=h; h+=m; }
  417.     
  418.     b=(complex **)ram;
  419.     ram+=(LONG)n*sizeof(complex *);
  420.     if (ram>ramend) outofram();
  421.     h=(complex *)rs; for (i=0; i<n; i++) { b[i]=h; h+=m; }
  422.     
  423.     /* inititalize c_lumat */
  424.     c_lumat=(complex **)ram;
  425.     ram+=(LONG)n*sizeof(complex *);
  426.     if (ram>ramend) outofram();
  427.     h=(complex *)a; 
  428.     for (i=0; i<n; i++) { c_lumat[i]=h; h+=n; }
  429.     
  430.     for (l=0; l<m; l++)
  431.     {    c_copy(x[0][l],b[0][l]);
  432.         for (k=1; k<n; k++)
  433.         {    c_copy(x[k][l],b[k][l]);
  434.             for (j=0; j<k; j++)
  435.             {    c_mult(c_lumat[k][j],x[j][l],t);
  436.                 c_sub(x[k][l],t,x[k][l]);
  437.             }
  438.         }
  439.         c_div(x[n-1][l],c_lumat[n-1][n-1],x[n-1][l]);
  440.         for (k=n-2; k>=0; k--)
  441.         {    sum[0]=0; sum[1]=0.0;
  442.             for (j=k+1; j<n; j++)
  443.             {    c_mult(c_lumat[k][j],x[j][l],t);
  444.                 c_add(sum,t,sum);
  445.             }
  446.             c_sub(x[k][l],sum,t);
  447.             c_div(t,c_lumat[k][k],x[k][l]);
  448.         }
  449.     }
  450. }
  451.  
  452. /************** fft *****************/
  453.  
  454. int nn;
  455. complex *ff,*zz,*fh;
  456.  
  457. void rfft (LONG m0, LONG p0, LONG q0, LONG n)
  458. /***** rfft 
  459.     make a fft on x[m],x[m+q0],...,x[m+(p0-1)*q0] (p points).
  460.     one has xi_p0 = xi_n^n = zz[n] ; i.e., p0*n=nn.
  461. *****/
  462. {    LONG p,q,m,l;
  463.     LONG mh,ml;
  464.     int found=0;
  465.     complex sum,h;
  466.     if (p0==1) return;
  467.     if (test_key()==escape) { error=301; return; }
  468.     if (p0%2==0) { p=p0/2; q=2; }
  469.     else
  470.     {    q=3;
  471.         while (q*q<=p0)
  472.         {    if (p0%q==0) 
  473.             {    found=1; break; }
  474.             q+=2;
  475.         }
  476.         if (found) p=p0/q;
  477.         else { q=p0; p=1; }
  478.     }
  479.     if (p>1) for (m=0; m<q; m++) 
  480.         rfft((m0+m*q0)%nn,p,q*q0,nn/p);
  481.     mh=m0;
  482.     for (l=0; l<p0; l++)
  483.     {    ml=l%p;
  484.         c_copy(sum,ff[(m0+ml*q*q0)%nn]);
  485.         for (m=1; m<q; m++)
  486.         {    c_mult(ff[(m0+(m+ml*q)*q0)%nn],zz[(n*l*m)%nn],h);
  487.             c_add(sum,h,sum);
  488.         }
  489.         sum[0]/=q; sum[1]/=q;
  490.         c_copy(fh[mh],sum);
  491.         mh+=q0; if (mh>=nn) mh-=nn;
  492.     }
  493.     for (l=0; l<p0; l++)
  494.     {    c_copy(ff[m0],fh[m0]);
  495.         m0+=q0; if (m0>=nn) mh-=nn;
  496.     }
  497. }
  498.  
  499. void fft (double *a, int n, int signum)
  500. {    complex z;
  501.     double h;
  502.     int i;
  503.     
  504.     if (n==0) return;
  505.     nn=n;
  506.  
  507.     ram=newram;
  508.     ff=(complex *)a;
  509.     zz=(complex *)ram;
  510.     ram+=n*sizeof(complex);
  511.     fh=(complex *)ram;
  512.     ram+=n*sizeof(complex);
  513.     if (ram>ramend) outofram();
  514.     
  515.     /* compute zz[k]=e^{-2*pi*i*k/n}, k=0,...,n-1 */
  516.     h=2*M_PI/n; z[0]=cos(h); z[1]=signum*sin(h);
  517.     zz[0][0]=1; zz[0][1]=0;
  518.     for (i=1; i<n; i++)
  519.     {    if (i%16) { zz[i][0]=cos(i*h); zz[i][1]=signum*sin(i*h); }
  520.         else c_mult(zz[i-1],z,zz[i]);
  521.     }
  522.     rfft(0,n,1,1);
  523.     if (signum==1)
  524.         for (i=0; i<n; i++)
  525.         {    ff[i][0]*=n; ff[i][1]*=n;
  526.         }
  527. }
  528.  
  529. /************** bauhuber algorithm ***************/
  530.  
  531. #define ITERMAX 200
  532. #define EPS (64*DBL_EPSILON)
  533. #define QR 0.1
  534. #define QI 0.8
  535. #define EPSROOT (64*epsilon)
  536. #define BETA (2096*EPSROOT)
  537.  
  538. void quadloes (double ar, double ai, double br, double bi,
  539.     double cr, double ci, double *treal, double *timag)
  540. {    double pr,pi,qr,qi,h;
  541.     pr=br*br-bi*bi; pi=2*br*bi;
  542.     qr=ar*cr-ai*ci; qi=ar*ci+ai*cr;
  543.     pr=pr-4*qr; pi=pi-4*qi;
  544.     h=sqrt(pr*pr+pi*pi);
  545.     qr=h+pr; if (qr<0.0) qr=0; 
  546.     qr=sqrt(qr/2);
  547.     qi=h-pr; if (qi<0.0) qi=0; 
  548.     qi=sqrt(qi/2);
  549.     if (pi<0.0) qi=-qi;
  550.     h=qr*br+qi*bi;
  551.     if (h>0.0) { qr=-qr; qi=-qi; }
  552.     pr=qr-br; pi=qi-bi;
  553.     h=pr*pr+pi*pi;
  554.     *treal=2*(cr*pr+ci*pi)/h;
  555.     *timag=2*(ci*pr-cr*pi)/h;
  556. }
  557.  
  558. int cxdiv (double ar, double ai, double br, double bi,
  559.     double *cr, double *ci)
  560. {    double temp;
  561.     if (br==0.0 && bi==0.0) return 1;
  562.     if (fabs(br)>fabs(bi))
  563.     {    temp=bi/br; br=temp*bi+br;
  564.         *cr=(ar+temp*ai)/br;
  565.         *ci=(ai-temp*ar)/br;
  566.     }
  567.     else
  568.     {    temp=br/bi; bi=temp*br+bi;
  569.         *cr=(temp*ar+ai)/bi;
  570.         *ci=(temp*ai-ar)/bi;
  571.     }
  572.     return 0;
  573. }
  574.  
  575. double cxxabs (double ar, double ai)
  576. {    if (ar==0.0) return fabs(ai);
  577.     if (ai==0.0) return fabs(ar);
  578.     return sqrt(ai*ai+ar*ar);
  579. }
  580.  
  581. void chorner (int n, int iu, double *ar, double *ai,
  582.     double xr, double xi, double *pr, double *pi,
  583.     double *p1r, double *p1i, double *p2r, double *p2i,
  584.     double *rf1)
  585. {    register int i,j;
  586.     int i1;
  587.     double temp,hh,tempr=0.0,tempi=0.0;
  588.     *pr=ar[n]; *pi=ai[n];
  589.     *p1r=*p2r=0.0; *p1i=*p2i=0.0;
  590.     *rf1=cxxabs(*pr,*pi);
  591.     i1=n-iu;
  592.     for (j=n-iu,i=n-1; i>=iu; i--,j--)
  593.     {    if (i<n-1)
  594.         {    tempr=*p1r; tempi=*p1i;
  595.             *p1r=*p1r * xr - *p1i * xi;
  596.             *p1i=*p1i * xr + tempr * xi;
  597.         }
  598.         *p1r+=*pr; *p1i+=*pi;
  599.         temp=*pr;
  600.         *pr=*pr * xr - *pi * xi + ar[i];
  601.         *pi=*pi * xr + temp * xi + ai[i];
  602.         temp=cxxabs(*p1r,*p1i);
  603.         hh=cxxabs(*pr,*pi); if (hh>temp) temp=hh;
  604.         if (temp>*rf1)
  605.         {    *rf1=temp; i1=j-1;
  606.         }
  607.         if (i<n-1)
  608.         {    temp=*p2r;
  609.             *p2r=*p2r * xr - *p2i * xi + tempr*2;
  610.             *p2i=*p2i * xr + temp * xi + tempi*2;
  611.         }
  612.     }
  613.     temp=cxxabs(xr,xi);
  614.     if (temp!=0.0)
  615.         *rf1=pow(temp,(double)i1)*(i1+1);
  616.     else
  617.         *rf1=cxxabs(*p1r,*p1i);
  618.     *rf1*=EPS;
  619. }
  620.  
  621. void scpoly (int n, double *ar, double *ai, double *scal)
  622. {    double p,h;
  623.     int i;
  624.     *scal=0.0;
  625.     p=cxxabs(ar[n],ai[n]);
  626.     for (i=0; i<n; i++)
  627.     {    ai[i]/=p; ar[i]/=p;
  628.         h=pow(cxxabs(ar[i],ai[i]),1.0/(n-i));
  629.         if (h>*scal) *scal=h;
  630.     }
  631.     ar[n]/=p; ai[n]/=p;
  632.     if (*scal==0.0) *scal=1.0;
  633.     for (p=1.0,i=n-1; i>=0; i--)
  634.     {    p*= *scal;
  635.         ar[i]/=p; ai[i]/=p;
  636.     }
  637. }
  638.  
  639. void bauroot (int n, int iu, double *ar, double *ai, double *x0r,
  640.     double *x0i)
  641. {    int iter=0,i=0,aborted=0;
  642.     double xoldr,xoldi,xnewr,xnewi,h,h1,h2,h3,h4,dzmax,dzmin,
  643.         dxr=1,dxi=0,tempr,tempi,abs_pold,abs_pnew,abs_p1new,
  644.         temp,ss,u,v,
  645.         pr,pi,p1r,p1i,p2r,p2i,abs_pnoted=-1;
  646.         
  647.     dxr=dxi=xoldr=xoldi=0.0;
  648.     if (n-iu==1)
  649.     {    quadloes(0.0,0.0,ar[n],ai[n],
  650.             ar[n-1],ai[n-1],x0r,x0i);
  651.         goto stop;
  652.     }
  653.     if (n-iu==2)
  654.     {    quadloes(ar[n],ai[n],ar[n-1],ai[n-1],
  655.             ar[n-2],ai[n-2],x0r,x0i);
  656.         goto stop;
  657.     }
  658.     xnewr=*x0r; xnewi=*x0i;
  659.     chorner(n,iu,ar,ai,xnewr,xnewi,&pr,&pi,&p1r,&p1i,&p2r,&p2i,&ss);
  660.     iter++;
  661.     abs_pnew=cxxabs(pr,pi);
  662.     if (abs_pnew==0) goto stop;
  663.     abs_pold=abs_pnew;
  664.     dzmin=BETA*(1+cxxabs(xnewr,xnewi));
  665.     while (!aborted)
  666.     {    abs_p1new=cxxabs(p1r,p1i);
  667.         iter++;
  668.         if (abs_pnew>abs_pold) /* Spiraling */
  669.         {    i=0;
  670.             temp=dxr;
  671.             dxr=QR*dxr-QI*dxi;
  672.             dxi=QR*dxi+QI*temp;
  673.         }
  674.         else /* Newton step */
  675.         {    
  676.             dzmax=1.0+cxxabs(xnewr,xnewi);
  677.             h1=p1r*p1r-p1i*p1i-pr*p2r+pi*p2i;
  678.             h2=2*p1r*p1i-pr*p2i-pi*p2r;
  679.             if (abs_p1new>10*ss && cxxabs(h1,h2)>100*ss*ss)
  680.                 /* do a Newton step */
  681.             {    i++;
  682.                 if (i>2) i=2;
  683.                 tempr=pr*p1r-pi*p1i;
  684.                 tempi=pr*p1i+pi*p1r;
  685.                 cxdiv(-tempr,-tempi,h1,h2,&dxr,&dxi);
  686.                 if (cxxabs(dxr,dxi)>dzmax)
  687.                 {    temp=dzmax/cxxabs(dxr,dxi);
  688.                     dxr*=temp; dxi*=temp;
  689.                     i=0;
  690.                 }
  691.                 if (i==2 && cxxabs(dxr,dxi)<dzmin/EPSROOT &&
  692.                     cxxabs(dxr,dxi)>0)
  693.                 {    i=0;
  694.                     cxdiv(xnewr-xoldr,xnewi-xoldi,dxr,dxi,&h3,&h4);
  695.                     h3+=1;
  696.                     h1=h3*h3-h4*h4;
  697.                     h2=2*h3*h4;
  698.                     cxdiv(dxr,dxi,h1,h2,&h3,&h4);
  699.                     if (cxxabs(h3,h4)<50*dzmin)
  700.                     {    dxr+=h3; dxi+=h4;
  701.                     }
  702.                 }
  703.                 xoldr=xnewr; xoldi=xnewi;
  704.                 abs_pold=abs_pnew;
  705.             }
  706.             else /* saddle point, minimize into direction pr+i*pi */
  707.             {    i=0;
  708.                 h=dzmax/abs_pnew;
  709.                 dxr=h*pr; dxi=h*pi;
  710.                 xoldr=xnewr; xoldi=xnewi;
  711.                 abs_pold=abs_pnew;
  712.                 do
  713.                 {    chorner(n,iu,ar,ai,xnewr+dxr,xnewi+dxi,&u,&v,
  714.                         &h,&h1,&h2,&h3,&h4);
  715.                     dxr*=2; dxi*=2;
  716.                 }
  717.                 while (fabs(cxxabs(u,v)/abs_pnew-1)<EPSROOT);
  718.             }
  719.         } /* end of Newton step */
  720.         xnewr=xoldr+dxr;
  721.         xnewi=xoldi+dxi;
  722.         dzmin=BETA*(1+cxxabs(xoldr,xoldi));
  723.         chorner(n,iu,ar,ai,xnewr,xnewi,&pr,&pi,
  724.             &p1r,&p1i,&p2r,&p2i,&ss);
  725.         abs_pnew=cxxabs(pr,pi);
  726.         if (abs_pnew==0.0) break;
  727.         if (cxxabs(dxr,dxi)<dzmin && abs_pnew<1e-5
  728.             && iter>5) break;
  729.         if (iter>ITERMAX)
  730.         {    iter=0;
  731.             if (abs_pnew<=abs_pnoted) break;
  732.             abs_pnoted=abs_pnew;
  733.             if (test_key()==escape) { error=700; return; }
  734.         }
  735.     }
  736.     *x0r=xnewr; *x0i=xnewi;
  737.     stop: ;
  738. /*
  739.     chorner(n,iu,ar,ai,*x0r,*x0i,&pr,&pi,&p1r,&p1i,&p2r,&p2i,&ss);
  740.     abs_pnew=cxxabs(pr,pi);
  741.     printf("%20.5e +i* %20.5e, %20.5e\n",
  742.         *x0r,*x0i,abs_pnew);
  743. */
  744. }
  745.  
  746. static void polydiv (int n, int iu, double *ar, double *ai,
  747.     double x0r, double x0i)
  748. {    int i;
  749.     for (i=n-1; i>iu; i--)
  750.     {    ar[i]+=ar[i+1]*x0r-ai[i+1]*x0i;
  751.         ai[i]+=ai[i+1]*x0r+ar[i+1]*x0i;
  752.     }
  753. }
  754.  
  755. void bauhuber (double *p, int n, double *result, int all,
  756.     double startr, double starti)
  757. {    double *ar,*ai,scalefak=1.0;
  758.     int i;
  759.     double x0r,x0i;
  760.     ram=newram;
  761.     if (ram+2*(n+1)*sizeof(double)>ramend) outofram();
  762.     ar=(double *)ram;
  763.     ai=ar+n+1;
  764.     for (i=0; i<=n; i++)
  765.     {    ar[i]=p[2*i];
  766.         ai[i]=p[2*i+1];
  767.     }
  768. /*    scpoly(n,ar,ai,&scalefak); */
  769.     /* scalefak=1; */
  770.     x0r=startr; x0i=starti;
  771.     for (i=0; i<(all?n:1); i++)
  772.     {    bauroot(n,i,ar,ai,&x0r,&x0i);
  773.         ar[i]=scalefak*x0r;
  774.         ai[i]=scalefak*x0i;
  775.         if (error) 
  776.         {    output("Bauhuber-Iteration failed!\n"); 
  777.             error=311; return;
  778.         }
  779.         polydiv(n,i,ar,ai,x0r,x0i);
  780.         x0i=-x0i;
  781.     }
  782.     for (i=0; i<n; i++)
  783.     {    result[2*i]=ar[i]; result[2*i+1]=ai[i];
  784.     }
  785. }
  786.  
  787. /**************** tridiagonalization *********************/
  788.  
  789. double **mg;
  790.  
  791. void tridiag ( double *a, int n, int **rows)
  792. /***** tridiag
  793.     tridiag. a with n rows and columns.
  794.     r[] contains the new indices of the rows.
  795. *****/
  796. {    char *ram=newram,rh;
  797.     double **m,maxi,*mh,lambda,h;
  798.     int i,j,ipiv,ik,jk,k,*r;
  799.     
  800.     /* make a pointer array to the rows of m : */
  801.     m=(double **)ram; ram+=n*sizeof(double *);
  802.     if (ram>ramend) outofram();
  803.     for (i=0; i<n; i++) { m[i]=a; a+=n; }
  804.     r=(int *)ram; ram+=n*sizeof(double *);
  805.     if (ram>ramend) outofram();
  806.     for (i=0; i<n; i++) r[i]=i;
  807.     
  808.     /* start algorithm : */
  809.     for (j=0; j<n-2; j++) /* need only go the (n-2)-th column */
  810.     {    /* determine pivot */
  811.         jk=r[j]; maxi=fabs(m[j+1][jk]); ipiv=j+1;
  812.         for (i=j+2; i<n; i++)
  813.         {    h=fabs(m[i][jk]);
  814.             if (h>maxi) { maxi=h; ipiv=i; }
  815.         }
  816.         if (maxi<epsilon) continue;
  817.         /* exchange with pivot : */
  818.         if (ipiv!=j+1)
  819.         {    mh=m[j+1]; m[j+1]=m[ipiv]; m[ipiv]=mh;
  820.             rh=r[j+1]; r[j+1]=r[ipiv]; r[ipiv]=rh;
  821.         }
  822.         /* zero elements */
  823.         for (i=j+2; i<n; i++)
  824.         {    jk=r[j]; m[i][jk]=lambda=-m[i][jk]/m[j+1][jk];
  825.             for (k=j+1; k<n; k++) 
  826.             {    ik=r[k]; m[i][ik]+=lambda*m[j+1][ik];
  827.             }
  828.             /* same for columns */
  829.             jk=r[j+1]; ik=r[i];
  830.             for (k=0; k<n; k++) m[k][jk]-=lambda*m[k][ik];
  831.         }
  832.     }
  833.     *rows=r; mg=m;
  834. }
  835.  
  836. complex **cmg;
  837.  
  838. void ctridiag ( double *ca, int n, int **rows)
  839. /***** tridiag
  840.     tridiag. a with n rows and columns.
  841.     r[] contains the new indices of the rows.
  842. *****/
  843. {    char *ram=newram,rh;
  844.     complex **m,*mh,lambda,*a=(complex *)ca,help;
  845.     double maxi,h;
  846.     int i,j,ipiv,ik,jk,k,*r;
  847.     
  848.     /* make a pointer array to the rows of m : */
  849.     m=(complex **)ram; ram+=n*sizeof(double *);
  850.     if (ram>ramend) outofram();
  851.     for (i=0; i<n; i++) { m[i]=a; a+=n; }
  852.     r=(int *)ram; ram+=n*sizeof(complex *);
  853.     if (ram>ramend) outofram();
  854.     for (i=0; i<n; i++) r[i]=i;
  855.     
  856.     /* start algorithm : */
  857.     for (j=0; j<n-2; j++) /* need only go the (n-2)-th column */
  858.     {    /* determine pivot */
  859.         jk=r[j]; maxi=c_abs(m[j+1][jk]); ipiv=j+1;
  860.         for (i=j+2; i<n; i++)
  861.         {    h=c_abs(m[i][jk]);
  862.             if (h>maxi) { maxi=h; ipiv=i; }
  863.         }
  864.         if (maxi<epsilon) continue;
  865.         /* exchange with pivot : */
  866.         if (ipiv!=j+1)
  867.         {    mh=m[j+1]; m[j+1]=m[ipiv]; m[ipiv]=mh;
  868.             rh=r[j+1]; r[j+1]=r[ipiv]; r[ipiv]=rh;
  869.         }
  870.         /* zero elements */
  871.         for (i=j+2; i<n; i++)
  872.         {    jk=r[j];
  873.             c_div(m[i][jk],m[j+1][jk],lambda);
  874.             lambda[0]=-lambda[0]; lambda[1]=-lambda[1];
  875.             c_copy(m[i][jk],lambda);
  876.             for (k=j+1; k<n; k++) 
  877.             {    ik=r[k];
  878.                 c_mult(lambda,m[j+1][ik],help);
  879.                 c_add(m[i][ik],help,m[i][ik]);
  880.             }
  881.             /* same for columns */
  882.             jk=r[j+1]; ik=r[i];
  883.             for (k=0; k<n; k++)
  884.             {    c_mult(lambda,m[k][ik],help);
  885.                 c_sub(m[k][jk],help,m[k][jk]);
  886.             }
  887.         }
  888.     }
  889.     *rows=r; cmg=m;
  890. }
  891.  
  892. void charpoly (double *m1, int n, double *p)
  893. /***** charpoly
  894.     compute the chracteristic polynomial of m.
  895. *****/
  896. {    int i,j,k,*r;
  897.     double **m,h1,h2;
  898.     tridiag(m1,n,&r); m=mg; /* unusual global variable handling */
  899.     /* compute the p_n rekursively : */
  900.     m[0][r[0]]=-m[0][r[0]]; /* first one is x-a(0,0). */
  901.     for (j=1; j<n; j++)
  902.     {    m[0][r[j]]=-m[0][r[j]];
  903.         for (k=1; k<=j; k++)
  904.         {    h1=-m[k][r[j]]; h2=m[k][r[k-1]]; 
  905.             for (i=0; i<k; i++) 
  906.                 m[i][r[j]]=m[i][r[j]]*h2+m[i][r[k-1]]*h1;
  907.             m[k][r[j]]=h1;
  908.         }
  909.         for (i=0; i<j; i++) m[i+1][r[j]]+=m[i][r[j-1]];
  910.     }
  911.     for (i=0; i<n; i++) p[i]=m[i][r[n-1]];
  912.     p[n]=1.0;
  913. }
  914.  
  915. void ccharpoly (double *m1, int n, double *p)
  916. /***** charpoly
  917.     compute the chracteristic polynomial of m.
  918. *****/
  919. {    int *r,i,j,k;
  920.     complex **m,h1,h2,g1,g2,*pc=(complex *)p;
  921.     ctridiag(m1,n,&r); m=cmg; /* unusual global variable handling */
  922.     /* compute the p_n rekursively : */
  923.     m[0][r[0]][0]=-m[0][r[0]][0];
  924.     m[0][r[0]][1]=-m[0][r[0]][1]; /* first one is x-a(0,0). */
  925.     for (j=1; j<n; j++)
  926.     {    m[0][r[j]][0]=-m[0][r[j]][0];
  927.         m[0][r[j]][1]=-m[0][r[j]][1];
  928.         for (k=1; k<=j; k++)
  929.         {    h1[0]=-m[k][r[j]][0]; h1[1]=-m[k][r[j]][1]; 
  930.             c_copy(h2,m[k][r[k-1]]); 
  931.             for (i=0; i<k; i++) 
  932.             {    c_mult(h2,m[i][r[j]],g1);
  933.                 c_mult(h1,m[i][r[k-1]],g2);
  934.                 c_add(g1,g2,m[i][r[j]]);
  935.             }
  936.             c_copy(m[k][r[j]],h1);
  937.         }
  938.         for (i=0; i<j; i++) 
  939.         {    c_add(m[i+1][r[j]],m[i][r[j-1]],m[i+1][r[j]]);
  940.         }
  941.     }
  942.     for (i=0; i<n; i++) c_copy(pc[i],m[i][r[n-1]]);
  943.     pc[n][0]=1.0; pc[n][1]=0.0;
  944. }
  945.  
  946. /***************** jacobi-givens eigenvalues **************/
  947.  
  948. double rotate (double *m, int j, int k, int n)
  949. {
  950.     double theta,t,s,c,tau,h,pivot;
  951.     int l;
  952.     pivot=*mat(m,n,j,k);
  953.     if (fabs(pivot)<epsilon) return 0;
  954.     theta=(*mat(m,n,j,j)-*mat(m,n,k,k))/(2*pivot);
  955.     t=1/(fabs(theta)+sqrt(1+theta*theta));
  956.     if (theta<0) t=-t;
  957.     c=1/sqrt(1+t*t); s=t*c;
  958.     tau=s/(1+c);
  959.     for (l=0; l<n; l++)
  960.     {
  961.         if (l==j || l==k) continue;
  962.         h=*mat(m,n,l,j);
  963.         *mat(m,n,j,l)=*mat(m,n,l,j)=h+s*(*mat(m,n,l,k)-tau*h);
  964.         *mat(m,n,l,k)-=s*(h+tau* *mat(m,n,l,k));
  965.         *mat(m,n,k,l)=*mat(m,n,l,k);
  966.     }
  967.     *mat(m,n,j,j)+=t*pivot;
  968.     *mat(m,n,k,k)-=t*pivot;
  969.     *mat(m,n,j,k)=*mat(m,n,k,j)=0;
  970.     return fabs(pivot);
  971. }
  972.  
  973. void mjacobi (header *hd)
  974. {
  975.     header *st=hd,*result,*hd1;
  976.     double *m,max,neumax,*mr;
  977.     int r,c,i,j;
  978.     hd=getvalue(hd); if (error) return;
  979.     if (hd->type!=s_matrix) wrong_arg();
  980.     getmatrix(hd,&r,&c,&m);
  981.     if (r!=c) wrong_arg();
  982.     if (r<2) { moveresult(st,hd); return; }
  983.     hd1=new_matrix(r,r,""); if (error) return;
  984.     m=matrixof(hd1);
  985.     memmove(m,matrixof(hd),(long)r*r*sizeof(double));
  986.     while(1)
  987.     {
  988.         max=0.0;
  989.         for (i=0; i<r-1; i++)
  990.             for (j=i+1; j<r; j++)
  991.                 if ((neumax=rotate(m,i,j,r))>max)
  992.                     max=neumax;
  993.         if (max<epsilon) break;
  994.     }
  995.     result=new_matrix(1,r,""); if (error) return;
  996.     mr=matrixof(result);
  997.     for (i=0; i<r; i++) *mr++=*mat(m,r,i,i);
  998.     moveresult(st,result);
  999. }
  1000.