00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047 #include "eig.h"
00048
00049 using namespace std;
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063 static void
00064 Householder( int N, square_matrix& ma, valarray<double>& diag, double* neben)
00065 {
00066 double epsilon;
00067 int i, j, k;
00068 double h, sum, tmp, tmp2;
00069
00070 for (i = N-1; i > 0; --i)
00071 {
00072 h = 0.0;
00073 if (i == 1)
00074 neben[i] = ma[i][i-1];
00075 else
00076 {
00077 for (k = i-1, epsilon = 0.0; k >= 0; --k)
00078 epsilon += fabs(ma[i][k]);
00079
00080 if (epsilon == 0.0)
00081 neben[i] = ma[i][i-1];
00082 else
00083 {
00084 for(k = i-1, sum = 0.0; k >= 0; --k)
00085 {
00086 ma[i][k] /= epsilon;
00087 sum += ma[i][k]*ma[i][k];
00088 }
00089 tmp = (ma[i][i-1] > 0) ? -sqrt(sum) : sqrt(sum);
00090 neben[i] = epsilon*tmp;
00091 h = sum - ma[i][i-1]*tmp;
00092 ma[i][i-1] -= tmp;
00093 for (j = 0, sum = 0.0; j < i; ++j)
00094 {
00095 ma[j][i] = ma[i][j]/h;
00096 tmp = 0.0;
00097 for (k = j; k >= 0; --k)
00098 tmp += ma[j][k]*ma[i][k];
00099 for (k = j+1; k < i; ++k)
00100 tmp += ma[k][j]*ma[i][k];
00101 neben[j] = tmp/h;
00102 sum += neben[j] * ma[i][j];
00103 }
00104 sum /= 2.*h;
00105 for (j = 0; j < i; ++j)
00106 {
00107 neben[j] -= ma[i][j]*sum;
00108 tmp = ma[i][j];
00109 tmp2 = neben[j];
00110 for (k = j; k >= 0; --k)
00111 ma[j][k] -= (tmp*neben[k] + tmp2*ma[i][k]);
00112 }
00113 }
00114 }
00115 diag[i] = h;
00116 }
00117
00118 diag[0] = 0.0;
00119 neben[0] = 0.0;
00120
00121 for (i = 0; i < N; ++i)
00122 {
00123 if(diag[i] != 0.0)
00124 for (j = 0; j < i; ++j)
00125 {
00126 for (k = i-1, tmp = 0.0; k >= 0; --k)
00127 tmp += ma[i][k] * ma[k][j];
00128 for (k = i-1; k >= 0; --k)
00129 ma[k][j] -= tmp*ma[k][i];
00130 }
00131 diag[i] = ma[i][i];
00132 ma[i][i] = 1.0;
00133 for (k = i-1; k >= 0; --k)
00134 ma[k][i] = ma[i][k] = 0.0;
00135 }
00136 }
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152 static int
00153 QLalgo( int N, valarray<double>& diag, square_matrix& mq,
00154 int maxIter, double* neben)
00155 {
00156 int i, j, k, kp1, l;
00157 double tmp, diff, cneben, c1, c2, p;
00158 int iter;
00159
00160 neben[N-1] = 0.0;
00161 for (i = 0, iter = 0; i < N && iter < maxIter; ++i)
00162 do
00163 {
00164 for (j = i; j < N-1; ++j)
00165 {
00166 tmp = fabs(diag[j]) + fabs(diag[j+1]);
00167 if (fabs(neben[j]) + tmp == tmp)
00168 break;
00169 }
00170 if (j != i)
00171 {
00172 if (++iter > maxIter) return maxIter-1;
00173 diff = (diag[i+1]-diag[i])/neben[i]/2.0;
00174 if (diff >= 0)
00175 diff = diag[j] - diag[i] +
00176 neben[i] / (diff + sqrt(diff * diff + 1.0));
00177 else
00178 diff = diag[j] - diag[i] +
00179 neben[i] / (diff - sqrt(diff * diff + 1.0));
00180 c2 = c1 = 1.0;
00181 p = 0.0;
00182 for (k = j-1; k >= i; --k)
00183 {
00184 kp1 = k + 1;
00185 tmp = c2 * neben[k];
00186 cneben = c1 * neben[k];
00187 if (fabs(tmp) >= fabs(diff))
00188 {
00189 c1 = diff / tmp;
00190 c2 = 1. / sqrt(c1*c1 + 1.0);
00191 neben[kp1] = tmp / c2;
00192 c1 *= c2;
00193 }
00194 else
00195 {
00196 c2 = tmp / diff;
00197 c1 = 1. / sqrt(c2*c2 + 1.0);
00198 neben[kp1] = diff / c1;
00199 c2 *= c1;
00200 }
00201 tmp = (diag[k] - diag[kp1] + p) * c2 + 2.0 * c1 * cneben;
00202 diag[kp1] += tmp * c2 - p;
00203 p = tmp * c2;
00204 diff = tmp * c1 - cneben;
00205 for (l = N-1; l >= 0; --l)
00206 {
00207 tmp = mq[l][kp1];
00208 mq[l][kp1] = c2 * mq[l][k] + c1 * tmp;
00209 mq[l][k] = c1 * mq[l][k] - c2 * tmp;
00210 }
00211 }
00212 diag[i] -= p;
00213 neben[i] = diff;
00214 neben[j] = 0.0;
00215 }
00216 } while (j != i);
00217 return iter;
00218 }
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234 namespace eo {
00235 int
00236 eig( int N, const lower_triangular_matrix& C, valarray<double>& diag, square_matrix& Q,
00237 int niter)
00238 {
00239 int ret;
00240 int i, j;
00241
00242 if (niter == 0) niter = 30*N;
00243
00244 for (i=0; i < N; ++i)
00245 {
00246 vector<double>::const_iterator row = C[i];
00247 for (j = 0; j <= i; ++j)
00248 Q[i][j] = Q[j][i] = row[j];
00249 }
00250
00251 double* rgtmp = new double[N+1];
00252 Householder( N, Q, diag, rgtmp);
00253 ret = QLalgo( N, diag, Q, niter, rgtmp+1);
00254 delete [] rgtmp;
00255
00256 return ret;
00257 }
00258
00259 }