00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #include <stdio.h>
00014 #include <math.h>
00015 #include <AR/matrix.h>
00016
00017 int arVecTridiagonalize( ARMat *a, ARVec *d, ARVec *e )
00018 {
00019 ARVec wv1, wv2;
00020 double *v;
00021 double s, t, p, q;
00022 int dim;
00023 int i, j, k;
00024
00025 if( a->clm != a->row ) return(-1);
00026 if( a->clm != d->clm ) return(-1);
00027 if( a->clm != e->clm+1 ) return(-1);
00028 dim = a->clm;
00029
00030 for( k = 0; k < dim-2; k++ ) {
00031 v = &(a->m[k*dim]);
00032 d->v[k] = v[k];
00033
00034 wv1.clm = dim-k-1;
00035 wv1.v = &(v[k+1]);
00036 e->v[k] = arVecHousehold(&wv1);
00037 if( e->v[k] == 0.0 ) continue;
00038
00039 for( i = k+1; i < dim; i++ ) {
00040 s = 0.0;
00041 for( j = k+1; j < i; j++ ) {
00042 s += a->m[j*dim+i] * v[j];
00043 }
00044 for( j = i; j < dim; j++ ) {
00045 s += a->m[i*dim+j] * v[j];
00046 }
00047 d->v[i] = s;
00048 }
00049
00050 wv1.clm = wv2.clm = dim-k-1;
00051 wv1.v = &(v[k+1]);
00052 wv2.v = &(d->v[k+1]);
00053 t = arVecInnerproduct( &wv1, &wv2 ) / 2;
00054 for( i = dim-1; i > k; i-- ) {
00055 p = v[i];
00056 q = d->v[i] -= t*p;
00057 for( j = i; j < dim; j++ ) {
00058 a->m[i*dim+j] -= p*(d->v[j]) + q*v[j];
00059 }
00060 }
00061 }
00062
00063 if( dim >= 2) {
00064 d->v[dim-2] = a->m[(dim-2)*dim+(dim-2)];
00065 e->v[dim-2] = a->m[(dim-2)*dim+(dim-1)];
00066 }
00067
00068 if( dim >= 1 ) d->v[dim-1] = a->m[(dim-1)*dim+(dim-1)];
00069
00070 for( k = dim-1; k >= 0; k-- ) {
00071 v = &(a->m[k*dim]);
00072 if( k < dim-2 ) {
00073 for( i = k+1; i < dim; i++ ) {
00074 wv1.clm = wv2.clm = dim-k-1;
00075 wv1.v = &(v[k+1]);
00076 wv2.v = &(a->m[i*dim+k+1]);
00077 t = arVecInnerproduct( &wv1, &wv2 );
00078 for( j = k+1; j < dim; j++ ) a->m[i*dim+j] -= t * v[j];
00079 }
00080 }
00081 for( i = 0; i < dim; i++ ) v[i] = 0.0;
00082 v[k] = 1;
00083 }
00084
00085 return(0);
00086 }