00001 #include <math.h>
00002 #define RADIX 2.0
00003 void balanc(double **a, int n)
00004 {
00005 int last,j,i;
00006 double s,r,g,f,c,sqrdx;
00007
00008 sqrdx=RADIX*RADIX;
00009 last=0;
00010 while (last == 0) {
00011 last=1;
00012 for (i=1;i<=n;i++) {
00013 r=c=0.0;
00014 for (j=1;j<=n;j++)
00015 if (j != i) {
00016 c += fabs(a[j][i]);
00017 r += fabs(a[i][j]);
00018 }
00019 if (c && r) {
00020 g=r/RADIX;
00021 f=1.0;
00022 s=c+r;
00023 while (c<g) {
00024 f *= RADIX;
00025 c *= sqrdx;
00026 }
00027 g=r*RADIX;
00028 while (c>g) {
00029 f /= RADIX;
00030 c /= sqrdx;
00031 }
00032 if ((c+r)/f < 0.95*s) {
00033 last=0;
00034 g=1.0/f;
00035 for (j=1;j<=n;j++) a[i][j] *= g;
00036 for (j=1;j<=n;j++) a[j][i] *= f;
00037 }
00038 }
00039 }
00040 }
00041
00042 }
00043
00044
00045 #define SWAP(g,h) {y=(g);(g)=(h);(h)=y;}
00046
00047 void elmhes(double **a, int n)
00048
00049
00050
00051
00052
00053 {
00054 int m,j,i;
00055 double y,x;
00056
00057 for (m=2;m<n;m++) {
00058 x=0.0;
00059 i=m;
00060 for (j=m;j<=n;j++) {
00061 if (fabs(a[j][m-1]) > fabs(x)) {
00062 x=a[j][m-1];
00063 i=j;
00064 }
00065 }
00066 if (i != m) {
00067 for (j=m-1;j<=n;j++) SWAP(a[i][j],a[m][j])
00068 for (j=1;j<=n;j++) SWAP(a[j][i],a[j][m])
00069 }
00070 if (x) {
00071 for (i=m+1;i<=n;i++) {
00072 if (y=a[i][m-1]) {
00073 y /= x;
00074 a[i][m-1]=y;
00075 for (j=m;j<=n;j++)
00076 a[i][j] -= y*a[m][j];
00077 for (j=1;j<=n;j++)
00078 a[j][m] += y*a[j][i];
00079 }
00080 }
00081 }
00082 }
00083
00084 }
00085
00086 #define SIGN(a,b) ((b) > 0 ? fabs(a) : -fabs(a))
00087
00088 void hqr(double **a, int n, double wr[], double wi[])
00089
00090 {
00091 int nn,m,l,k,j,its,i,mmin;
00092 double z,y,x,w,v,u,t,s,r,q,p,anorm;
00093
00094 anorm=fabs(a[1][1]);
00095 for (i=2;i<=n;i++)
00096 for (j=(i-1);j<=n;j++)
00097 anorm += fabs(a[i][j]);
00098 nn=n;
00099 t=0.0;
00100
00101 while (nn >= 1) {
00102 its=0;
00103 do {
00104 for (l=nn;l>=2;l--) {
00105 s=fabs(a[l-1][l-1])+fabs(a[l][l]);
00106 if (s == 0.0) s=anorm;
00107 if (fabs(a[l][l-1]) + s == s) break;
00108 }
00109 x=a[nn][nn];
00110 if (l == nn) {
00111 wr[nn]=x+t;
00112 wi[nn--]=0.0;
00113 } else {
00114 y=a[nn-1][nn-1];
00115 w=a[nn][nn-1]*a[nn-1][nn];
00116 if (l == (nn-1)) {
00117 p=0.5*(y-x);
00118 q=p*p+w;
00119 z=sqrt(fabs(q));
00120 x += t;
00121 if (q >= 0.0) {
00122 z=p+SIGN(z,p);
00123 wr[nn-1]=wr[nn]=x+z;
00124 if (z) wr[nn]=x-w/z;
00125 wi[nn-1]=wi[nn]=0.0;
00126 } else {
00127 wr[nn-1]=wr[nn]=x+p;
00128 wi[nn-1]= -(wi[nn]=z);
00129 }
00130 nn -= 2;
00131 } else {
00132 if (its == 50) break;
00133 if (its == 10 || its == 20) {
00134 t += x;
00135 for (i=1;i<=nn;i++) a[i][i] -= x;
00136 s=fabs(a[nn][nn-1])+fabs(a[nn-1][nn-2]);
00137 y=x=0.75*s;
00138 w = -0.4375*s*s;
00139 }
00140 ++its;
00141 for (m=(nn-2);m>=l;m--) {
00142 z=a[m][m];
00143 r=x-z;
00144 s=y-z;
00145 p=(r*s-w)/a[m+1][m]+a[m][m+1];
00146 q=a[m+1][m+1]-z-r-s;
00147 r=a[m+2][m+1];
00148 s=fabs(p)+fabs(q)+fabs(r);
00149 p /= s;
00150 q /= s;
00151 r /= s;
00152 if (m == l) break;
00153 u=fabs(a[m][m-1])*(fabs(q)+fabs(r));
00154 v=fabs(p)*(fabs(a[m-1][m-1])+fabs(z)+fabs(a[m+1][m+1]));
00155 if (u+v == v) break;
00156 }
00157 for (i=m+2;i<=nn;i++) {
00158 a[i][i-2]=0.0;
00159 if (i != (m+2)) a[i][i-3]=0.0;
00160 }
00161 for (k=m;k<=nn-1;k++) {
00162 if (k != m) {
00163 p=a[k][k-1];
00164 q=a[k+1][k-1];
00165 r=0.0;
00166 if (k != (nn-1)) r=a[k+2][k-1];
00167 if (x=fabs(p)+fabs(q)+fabs(r)) {
00168 p /= x;
00169 q /= x;
00170 r /= x;
00171 }
00172 }
00173 if (s=SIGN(sqrt(p*p+q*q+r*r),p)) {
00174 if (k == m) {
00175 if (l != m)
00176 a[k][k-1] = -a[k][k-1];
00177 } else
00178 a[k][k-1] = -s*x;
00179 p += s;
00180 x=p/s;
00181 y=q/s;
00182 z=r/s;
00183 q /= p;
00184 r /= p;
00185 for (j=k;j<=nn;j++) {
00186 p=a[k][j]+q*a[k+1][j];
00187 if (k != (nn-1)) {
00188 p += r*a[k+2][j];
00189 a[k+2][j] -= p*z;
00190 }
00191 a[k+1][j] -= p*y;
00192 a[k][j] -= p*x;
00193 }
00194 mmin = nn<k+3 ? nn : k+3;
00195 for (i=l;i<=mmin;i++) {
00196 p=x*a[i][k]+y*a[i][k+1];
00197 if (k != (nn-1)) {
00198 p += z*a[i][k+2];
00199 a[i][k+2] -= p*r;
00200 }
00201 a[i][k+1] -= p*q;
00202 a[i][k] -= p;
00203 }
00204 }
00205 }
00206 }
00207 }
00208 } while (l < nn-1);
00209 }
00210 }