rtc: rtc: rtcEigenSystem.h Source File

00001 /*
00002  * Copyright (C) 2009
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: rtcEigenSystem.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-01-21 18:19:16 -0800 (Wed, 21 Jan 2009) $
00016  * changed by .: $Author: benjaminpitzer $
00017  * revision ...: $Revision: 14 $
00018  */
00019 #ifndef RTC_EIGENSYSTEM_H
00020 #define RTC_EIGENSYSTEM_H
00021 
00022 // puma includes
00023 #include <rtc/rtcException.h>
00024 #include "rtc/rtcMath.h"
00025 #include "rtc/rtcSMat.h"
00026 #include "rtc/rtcVec.h"
00027 
00028 #define PUMA_JACOBI_ROTATE(a,i,j,k,l) g=a.x[i*M+j];h=a.x[k*M+l];\
00029                                         a.x[i*M+j]=g-s*(h+g*tau);\
00030                                         a.x[k*M+l]=h+s*(g-h*tau);
00031 #define PUMA_RADIX 2.0
00032 #define PUMA_SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
00033 
00034 namespace rtc {
00035 
00037   // DECLARATIONS
00039 
00040   // Forward declarations
00041   template <class T, int M> class Vec;          // M-d vector
00042   template <class T, int M> class SMat;         // MxM Square Matrix
00043   template <class T, int M> class EigenSystem;  // Eigensystem
00044 
00046 
00059   template <class T, int M>
00060   class EigenSystem {
00061   public:
00062     // Constructors/Destructor
00063     EigenSystem(SMat<T,M>& a_);
00064     ~EigenSystem();
00065 
00066     // Eigen System routines for real symmetric matrices
00067 
00068     /*
00069      * Produces a vector of eigenvalues (D) and a matrix of eigenvectors (V) of
00070      * matrix A.
00071      */
00072     bool jacobi(Vec<T,M>& eigenvalues, SMat<T,M>& eigenvectors, int* nrot);
00073     bool jacobi(Vec<T,M>& eigenvalues, SMat<T,M>& eigenvectors);
00074     bool jacobi2(Vec<T,M>& eigenvalues, SMat<T,M>& eigenvectors) const;
00075     void eigenSort(Vec<T,M>& d, SMat<T,M>& v);
00076 
00077     // non-tested routines
00078     void householderReduce(Vec<T,M>& d, Vec<T,M>& e, bool vecs = true);
00079     void householderSolve(Vec<T,M>& d, Vec<T,M>& e, bool vecs = true);
00080     void balance();
00081     void hessenbergReduce();
00082     void qr(Vec<T,M>& wreal, Vec<T,M>& wimag);
00083 
00084   protected:
00085     void rotate(double& g,double& h,SMat<T,M>& a,
00086                 const int i,const int j,const int k,const int l,
00087                 const double s,const double tau) const;
00088     void rotateT(double& g,double& h,SMat<T,M>& a,
00089                  const int i,const int j,const int k,const int l,
00090                  const double s,const double tau) const;
00091     // Data
00092     SMat<T,M>& a;
00093   }; // end class EigenSystem<T,M>
00094 
00096   // DEFINITIONS
00098 
00100 
00101   // Constructor/Destructor
00102 
00105   template <class T, int M>
00106   inline EigenSystem<T,M>::EigenSystem(SMat<T,M>& a_) : a(a_) {
00107   }
00108 
00111   template <class T, int M>
00112   inline EigenSystem<T,M>::~EigenSystem() {
00113   }
00114 
00115   // Eigen System routines
00116 
00122   template <class T, int M>
00123   inline bool EigenSystem<T,M>::jacobi(Vec<T,M>& d, SMat<T,M>& v) {
00124     int nrot;
00125     return jacobi(d,v,&nrot);
00126   }
00127 
00134   template <class T, int M>
00135   inline bool EigenSystem<T,M>::jacobi(Vec<T,M>& d, SMat<T,M>& v, int* nrot) {
00136     int j,iq,ip,i;
00137     T tresh,theta,tau,t,sm,s,h,g,c;
00138     Vec<T,M> b;
00139     Vec<T,M> z;
00140 
00141     for (ip=0;ip<M;ip++) { // Initialize to the identity matrix.
00142       for (iq=0;iq<M;iq++) v.x[ip*M+iq]=0.0;
00143       v.x[ip*M+ip]=1.0;
00144     }
00145     for (ip=0;ip<M;ip++) { // Initialize b and d to the diagonal of a.
00146       b.x[ip]=d.x[ip]=a.x[ip*M+ip];
00147       z.x[ip]=0.0; // This vector will accumulate terms
00148     }
00149     *nrot=0;
00150     for (i=0;i<=50;i++) {
00151       sm=0.0;
00152       for (ip=0;ip<M-1;ip++) { // Sum off-diagonal elements
00153   for (iq=ip+1;iq<M;iq++) sm += fabs(a.x[ip*M+iq]);
00154       }
00155       if (sm == 0.0) return true;
00156       if (i < 4) tresh=0.2*sm/(M*M); // ...on the  rst three sweeps.
00157       else tresh=0.0; // ...thereafter.
00158       for (ip=0;ip<M-1;ip++) {
00159   for (iq=ip+1;iq<M;iq++) {
00160     g=100.0*fabs(a.x[ip*M+iq]);
00161 
00162     if (i > 4 && (T)(fabs(d.x[ip])+g) == (T)fabs(d.x[ip])
00163         && (T)(fabs(d.x[iq])+g) == (T)fabs(d.x[iq]))
00164       a.x[ip*M+iq]=0.0;
00165     else if (fabs(a.x[ip*M+iq]) > tresh) {
00166       h=d.x[iq]-d.x[ip];
00167       if ((T)(fabs(h)+g) == (T)fabs(h)) t=(a.x[ip*M+iq])/h;
00168       else {
00169         theta=0.5*h/(a.x[ip*M+iq]); // Equation (11.1.10).
00170         t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
00171         if (theta < 0.0) t = -t;
00172       }
00173       c=1.0/sqrt(1+t*t);
00174       s=t*c;
00175       tau=s/(1.0+c);
00176       h=t*a.x[ip*M+iq];
00177       z.x[ip] -= h;
00178       z.x[iq] += h;
00179       d.x[ip] -= h;
00180       d.x[iq] += h;
00181       a.x[ip*M+iq]=0.0;
00182       for (j=0;j<=ip-1;j++) { // Case of rotations 1 d j < p.
00183         PUMA_JACOBI_ROTATE(a,j,ip,j,iq)
00184     }
00185       for (j=ip+1;j<=iq-1;j++) { // Case of rotations p < j < q.
00186         PUMA_JACOBI_ROTATE(a,ip,j,j,iq)
00187     }
00188       for (j=iq+1;j<M;j++) { // Case of rotations q < j d n.
00189         PUMA_JACOBI_ROTATE(a,ip,j,iq,j)
00190     }
00191       for (j=0;j<M;j++) {
00192         PUMA_JACOBI_ROTATE(v,j,ip,j,iq)
00193     }
00194       ++(*nrot);
00195     }
00196   }
00197       }
00198       for (ip=0;ip<M;ip++) {
00199   b.x[ip] += z.x[ip];
00200   d.x[ip]=b.x[ip]; // Update d with the sum of tapq,
00201   z.x[ip]=0.0; // and reinitialize z.
00202       }
00203     }
00204     return false;
00205   }
00206 
00207   template <class T, int M>
00208   inline void EigenSystem<T,M>::rotate(double& g,double& h,SMat<T,M>& a,
00209                                        const int i,const int j,const int k,const int l,
00210                                        const double s,const double tau) const {
00211     g=a(i,j);
00212     h=a(k,l);
00213     a(i,j)=g-s*(h+g*tau);
00214     a(k,l)=h+s*(g-h*tau);
00215   }
00216 
00217   template <class T, int M>
00218   inline void EigenSystem<T,M>::rotateT(double& g,double& h,SMat<T,M>& a,
00219                                         const int i,const int j,const int k,const int l,
00220                                         const double s,const double tau) const {
00221     g=a(i,j);
00222     h=a(k,l);
00223     a(i,j)=static_cast<T>(g-s*(h+g*tau));
00224     a(k,l)=static_cast<T>(h+s*(g-h*tau));
00225   }
00226 
00227 
00228 
00238   template <class T, int M>
00239   inline bool EigenSystem<T,M>::jacobi2(Vec<T,M>& eigenvalues,
00240                                         SMat<T,M>& eigenvectors) const {
00241     static const int maxIter=100;
00242     const int& matrixDim = M;
00243 
00244     int j,iq,ip,i;
00245     double tresh, theta, tau, sm, g, h;
00246     T c,s,t;
00247 
00248     SMat<T,M> cloneMatrix(a);
00249     Vec<T,M> b;
00250     Vec<T,M> z(T(0));
00251 
00252     eigenvalues.set(T(0));
00253     eigenvectors.set(T(0));
00254 
00255     for (ip=0;ip<matrixDim;ip++) {
00256       eigenvectors(ip,ip)=static_cast<T>(1);
00257       eigenvalues[ip]=static_cast<T>(b[ip]=cloneMatrix(ip,ip));
00258     }
00259 
00260     for (i=0;i<maxIter;i++) {
00261       sm=0.0;
00262       for (ip=0;ip<matrixDim;ip++) {
00263         for(iq=ip+1;iq<matrixDim;iq++) {
00264           sm += fabs(cloneMatrix(ip,iq));
00265         }
00266       }
00267 
00268       if (sm == 0.0) {   // normal return!
00269 //        if(sort) {
00270 //          sort2<T,T> sorter(true,false); // descending order and columns sorter
00271 //          sorter.apply(eigenvalues,eigenvectors);
00272 //        }
00273 //
00274 //        if ((tmpParam.dimensions>0) &&
00275 //            (tmpParam.dimensions<eigenvalues.size())) {
00276 //          // reduce the dimensionality of the vector and matrices as requested
00277 //          eigenvalues.resize(tmpParam.dimensions,T(),true,false);
00278 //          eigenvectors.resize(eigenvectors.rows(),tmpParam.dimensions,T(),
00279 //                              true,false);
00280 //        }
00281 
00282         return true;
00283       }
00284 
00285       if (i < 3) {
00286         tresh= 0.2*sm/(matrixDim*matrixDim);
00287       } else {
00288         tresh=0.0;
00289       }
00290 
00291       for (ip=0;ip<matrixDim-1;ip++) {
00292         for (iq=ip+1;iq<matrixDim;iq++) {
00293           g = 100.0*fabs(cloneMatrix(ip,iq));
00294           if ((i > 3) && ((fabs(eigenvalues[ip])+g) == fabs(eigenvalues[ip]))
00295               && ((fabs(eigenvalues[iq])+g) == fabs(eigenvalues[iq]))) {
00296             cloneMatrix(ip,iq)=0.0;
00297           }
00298           else if (fabs(cloneMatrix(ip,iq)) > tresh) {
00299             h=eigenvalues[iq]-eigenvalues[ip];
00300             if ((fabs(h)+g) == fabs(h)) {
00301               t=static_cast<T>((cloneMatrix(ip,iq))/h);
00302             } else {
00303               theta = 0.5*h/cloneMatrix(ip,iq);
00304               t = static_cast<T>(1.0/(fabs(theta)+sqrt(1.0+theta*theta)));
00305               if (theta < 0.0) {
00306                 t = -t;
00307               }
00308             }
00309             c = static_cast<T>(1)/sqrt(1+t*t);
00310             s = t*c;
00311             tau = s/(c+1);
00312             h=t*cloneMatrix(ip,iq);
00313             z[ip] -= h;
00314             z[iq] += h;
00315             eigenvalues[ip] -= static_cast<T>(h);
00316             eigenvalues[iq] += static_cast<T>(h);
00317             cloneMatrix(ip,iq)=0.0;
00318             for (j=0;j<ip;j++)  {
00319               rotate(g,h,cloneMatrix,j,ip,j,iq,s,tau);
00320             }
00321             for (j=ip+1;j<iq;j++) {
00322               rotate(g,h,cloneMatrix,ip,j,j,iq,s,tau);
00323             }
00324             for (j=iq+1;j<matrixDim;j++) {
00325               rotate(g,h,cloneMatrix,ip,j,iq,j,s,tau);
00326             }
00327             for (j=0;j<matrixDim;j++) {
00328               rotateT(g,h,eigenvectors,j,ip,j,iq,s,tau);
00329             }
00330           }
00331         }
00332       }
00333       for (ip=0;ip<matrixDim;ip++) {
00334         b[ip] += z[ip];
00335         eigenvalues[ip] = static_cast<T>(b[ip]);
00336         z[ip] = 0.0;
00337       }
00338     }
00339 
00340 //    char x[80];
00341 //    sprintf(x,"Jacobi Method did not converge after %d iterations!",maxIter);
00342 //    this->setStatusString(x);
00343 
00344     return false;
00345   }
00346 
00353   template <class T, int M>
00354   inline void EigenSystem<T,M>::householderReduce(Vec<T,M>& d, Vec<T,M>& e,
00355               bool vecs) {
00356     int l,k,j,i;
00357     T scale,hh,h,g,f;
00358 
00359     // add wrapper to handle weird 1-based Numerical Recipes in C indexing
00360     int n = M;
00361     T* dd = d.x - 1;
00362     T* ee = e.x - 1;
00363     T** aa = new T*[M]; aa--;
00364     for (i=1;i<=M;i++) aa[i] = &(a.x[(i-1)*M-1]);
00365 
00366     for (i=n;i>=2;i--) {
00367       l=i-1;
00368       h=scale=0.0;
00369       if (l > 1) {
00370   for (k=1;k<=l;k++)
00371     scale += fabs(aa[i][k]);
00372   if (scale == 0.0)
00373     ee[i]=aa[i][l];
00374   else {
00375     for (k=1;k<=l;k++) {
00376       aa[i][k] /= scale;
00377       h += aa[i][k]*aa[i][k];
00378     }
00379     f=aa[i][l];
00380     g=(f >= 0.0 ? -sqrt(h) : sqrt(h));
00381     ee[i]=scale*g;
00382     h -= f*g;
00383     aa[i][l]=f-g;
00384     f=0.0;
00385     for (j=1;j<=l;j++) {
00386       /* Next statement can be omitted if eigenvectors not wanted */
00387       if (vecs) aa[j][i]=aa[i][j]/h;
00388       g=0.0;
00389       for (k=1;k<=j;k++)
00390         g += aa[j][k]*aa[i][k];
00391       for (k=j+1;k<=l;k++)
00392         g += aa[k][j]*aa[i][k];
00393       ee[j]=g/h;
00394       f += ee[j]*aa[i][j];
00395     }
00396     hh=f/(h+h);
00397     for (j=1;j<=l;j++) {
00398       f=aa[i][j];
00399       ee[j]=g=ee[j]-hh*f;
00400       for (k=1;k<=j;k++)
00401         aa[j][k] -= (f*ee[k]+g*aa[i][k]);
00402     }
00403   }
00404       } else
00405   ee[i]=aa[i][l];
00406       dd[i]=h;
00407     }
00408     /* Next statement can be omitted if eigenvectors not wanted */
00409     if (vecs) dd[1]=0.0;
00410     ee[1]=0.0;
00411     /* Contents of this loop can be omitted if eigenvectors not
00412        wanted except for statement dd[i]=aa[i][i]; */
00413     for (i=1;i<=n;i++) {
00414       if (vecs) {
00415   l=i-1;
00416   if (dd[i]) {
00417     for (j=1;j<=l;j++) {
00418       g=0.0;
00419       for (k=1;k<=l;k++)
00420         g += aa[i][k]*aa[k][j];
00421       for (k=1;k<=l;k++)
00422         aa[k][j] -= g*aa[k][i];
00423     }
00424   }
00425       }
00426       dd[i]=aa[i][i];
00427       if (vecs) {
00428   aa[i][i]=1.0;
00429   for (j=1;j<=l;j++) aa[j][i]=aa[i][j]=0.0;
00430       }
00431     }
00432 
00433     // free memory from stupid wrapper
00434     aa++; delete [] aa;
00435   }
00436 
00443   template <class T, int M>
00444   inline void EigenSystem<T,M>::householderSolve(Vec<T,M>& d, Vec<T,M>& e, bool vecs) {
00445     int m,l,iter,i,k;
00446     T s,r,p,g,f,dd,c,b;
00447 
00448     // add wrapper to handle weird 1-based Numerical Recipes in C indexing
00449     int n = M;
00450     T* ddd = d.x - 1;
00451     T* ee = e.x - 1;
00452     T** aa = new T*[M]; aa--;
00453     for (i=1;i<=M;i++) aa[i] = &(a.x[(i-1)*M-1]);
00454 
00455     for (i=2;i<=n;i++) ee[i-1]=ee[i];
00456     ee[n]=0.0;
00457     for (l=1;l<=n;l++) {
00458       iter=0;
00459       do {
00460   for (m=l;m<=n-1;m++) {
00461     dd=fabs(ddd[m])+fabs(ddd[m+1]);
00462     if ((T)(fabs(ee[m])+dd) == dd) break;
00463   }
00464   if (m != l) {
00465     if (iter++ == 30) {
00466       std::stringstream ss;
00467       ss << "EigenSystem<" << M << ">::householderSolve: error, "
00468          << "too many interaions." << std::endl << std::flush;
00469       throw Exception(ss.str());
00470     }
00471     g=(ddd[l+1]-ddd[l])/(2.0*ee[l]);
00472     r=pythag(g,T(1));
00473     g=ddd[m]-ddd[l]+ee[l]/(g+PUMA_SIGN(r,g));
00474     s=c=1.0;
00475     p=0.0;
00476     for (i=m-1;i>=l;i--) {
00477       f=s*ee[i];
00478       b=c*ee[i];
00479       ee[i+1]=(r=pythag(f,g));
00480       if (r == 0.0) {
00481         ddd[i+1] -= p;
00482         ee[m]=0.0;
00483         break;
00484       }
00485       s=f/r;
00486       c=g/r;
00487       g=ddd[i+1]-p;
00488       r=(ddd[i]-g)*s+2.0*c*b;
00489       ddd[i+1]=g+(p=s*r);
00490       g=c*r-b;
00491       /* Next loop can be omitted if eigenvectors not wanted*/
00492       if (vecs) for (k=1;k<=n;k++) {
00493         f=aa[k][i+1];
00494         aa[k][i+1]=s*aa[k][i]+c*f;
00495         aa[k][i]=c*aa[k][i]-s*f;
00496       }
00497     }
00498     if (r == 0.0 && i >= l) continue;
00499     ddd[l] -= p;
00500     ee[l]=g;
00501     ee[m]=0.0;
00502   }
00503       } while (m != l);
00504     }
00505 
00506     // free memory from stupid wrapper
00507     aa++; delete [] aa;
00508   }
00509 
00510 
00517   template<class T, int M>
00518   inline void EigenSystem<T, M>::eigenSort(Vec<T, M>& d, SMat<T, M>& v)
00519   {
00520     int k, j, i;
00521     T p;
00522     for(i = 0; i<M-1; i++) {
00523       p = rtc_abs(d.x[k = i]);
00524       for(j = i+1; j<M; j++)
00525         if (rtc_abs(d.x[j])>=p)
00526           p = d.x[k = j];
00527       if (k!=i) {
00528         d.x[k] = d.x[i];
00529         d.x[i] = p;
00530         for(j = 0; j<M; j++) {
00531           p = v.x[j*M+i];
00532           v.x[j*M+i] = v.x[j*M+k];
00533           v.x[j*M+k] = p;
00534         }
00535       }
00536     }
00537   }
00538 
00542   template <class T, int M>
00543   inline void EigenSystem<T,M>::balance() {
00544     int last,j,i;
00545     T s,r,g,f,c,sqrdx;
00546     sqrdx=PUMA_RADIX*PUMA_RADIX;
00547     last=0;
00548     while (last == 0) {
00549       last=1;
00550       for (i=0;i<M;i++) { // Calculate row and column norms.
00551   r=c=0.0;
00552   for (j=0;j<M;j++) if (j != i) {
00553     c += fabs(a.x[j*M+i]);
00554     r += fabs(a.x[i*M+j]);
00555   }
00556   if (c && r) { //If both are nonzero,
00557     g=r/PUMA_RADIX;
00558     f=1.0;
00559     s=c+r;
00560     while (c<g) {
00561       f *= PUMA_RADIX;
00562       c *= sqrdx;
00563     }
00564     g=r*PUMA_RADIX;
00565     while (c>g) {
00566       f /= PUMA_RADIX;
00567       c /= sqrdx;
00568     }
00569     if ((c+r)/f < 0.95*s) {
00570       last=0;
00571       g=1.0/f;
00572       for (j=0;j<M;j++) a.x[i*M+j] *= g;
00573       for (j=0;j<M;j++) a.x[j*M+i] *= f;
00574     }
00575   }
00576       }
00577     }
00578   }
00579 
00584   template <class T, int M>
00585   inline void EigenSystem<T,M>::hessenbergReduce() {
00586     int m,j,i;
00587     T y,x;
00588 
00589     for (m=1;m<M-1;m++) { // m is called r + 1 in the text.
00590       x=0.0;
00591       i=m;
00592       for (j=m;j<M;j++) { // Find the pivot.
00593   if (fabs(a.x[j*M+m-1]) > fabs(x)) {
00594     x=a.x[j*M+m-1];
00595     i=j;
00596   }
00597       }
00598       if (i != m) { // Interchange rows and columns.
00599   for (j=m-1;j<M;j++) puma_swap(a.x[i*M+j],a.x[m*M+j]);
00600   for (j=0;j<M;j++) puma_swap(a.x[j*M+i],a.x[j*M+m]);
00601       }
00602       if (x) { // Carry out the elimination.
00603   for (i=m+1;i<M;i++) {
00604     if ((y=a.x[i*M+m-1]) != 0.0) {
00605       y /= x;
00606       a.x[i*M+m-1]=y;
00607       for (j=m;j<M;j++) a.x[i*M+j] -= y*a.x[m*M+j];
00608       for (j=0;j<M;j++) a.x[j*M+m] += y*a.x[j*M+i];
00609     }
00610   }
00611       }
00612     }
00613   }
00614 
00619   template <class T, int M>
00620   inline void EigenSystem<T,M>::qr(Vec<T,M>& wreal, Vec<T,M>& wimag) {
00621     int nn,m,l,k,j,its,i,mmin;
00622     T z,y,x,w,v,u,t,s,r,q,p,anorm;
00623 
00624     T* wr = wreal.x - 1;
00625     T* wi = wimag.x - 1;
00626     T** aa = new T*[M]; aa--;
00627     for (i=1;i<=M;i++) aa[i] = &(a.x[(i-1)*M-1]);
00628 
00629     anorm=0.0;
00630     for (i=1;i<=M;i++)
00631       for (j=rtc_max(i-1,1);j<=M;j++)
00632   anorm += fabs(aa[i][j]);
00633     nn=M;
00634     t=0.0;
00635     while (nn >= 1) {
00636       its=0;
00637       do {
00638   for (l=nn;l>=2;l--) {
00639     s=fabs(aa[l-1][l-1])+fabs(aa[l][l]);
00640     if (s == 0.0) s=anorm;
00641     if ((T)(fabs(aa[l][l-1]) + s) == s) {
00642       aa[l][l-1]=0.0;
00643       break;
00644     }
00645   }
00646   x=aa[nn][nn];
00647   if (l == nn) {
00648     wr[nn]=x+t;
00649     wi[nn--]=0.0;
00650   } else {
00651     y=aa[nn-1][nn-1];
00652     w=aa[nn][nn-1]*aa[nn-1][nn];
00653     if (l == (nn-1)) {
00654       p=0.5*(y-x);
00655       q=p*p+w;
00656       z=sqrt(fabs(q));
00657       x += t;
00658       if (q >= 0.0) { // ...a real pair.
00659         z=p+PUMA_SIGN(z,p);
00660         wr[nn-1]=wr[nn]=x+z;
00661         if (z) wr[nn]=x-w/z;
00662         wi[nn-1]=wi[nn]=0.0;
00663       } else { //...a complex pair.
00664         wr[nn-1]=wr[nn]=x+p;
00665         wi[nn-1]= -(wi[nn]=z);
00666       }
00667       nn -= 2;
00668     } else { // No roots found. Continue iteration.
00669       if (its == 30) {
00670         std::stringstream ss;
00671         ss << "EigenSystem<" << M << ">::qr: error, " << "too many iterations in hqr\n";
00672         throw Exception(ss.str());
00673       }
00674       if (its == 10 || its == 20) { // Form exceptional shift.
00675         t += x;
00676         for (i=1;i<=nn;i++) aa[i][i] -= x;
00677         s=fabs(aa[nn][nn-1])+fabs(aa[nn-1][nn-2]);
00678         y=x=0.75*s;
00679         w = -0.4375*s*s;
00680       }
00681       ++its;
00682       for (m=(nn-2);m>=l;m--) {
00683         z=aa[m][m];
00684         r=x-z;
00685         s=y-z;
00686         p=(r*s-w)/aa[m+1][m]+aa[m][m+1]; // Equation (11.6.23).
00687         q=aa[m+1][m+1]-z-r-s;
00688         r=aa[m+2][m+1];
00689         s=fabs(p)+fabs(q)+fabs(r);
00690         p /= s; q /= s; r /= s;
00691         if (m == l) break;
00692 
00693         u=fabs(aa[m][m-1])*(fabs(q)+fabs(r));
00694         v=fabs(p)*(fabs(aa[m-1][m-1])+fabs(z)+fabs(aa[m+1][m+1]));
00695         if ((T)(u+v) == v) break;
00696       }
00697       for (i=m+2;i<=nn;i++) {
00698         aa[i][i-2]=0.0;
00699         if (i != (m+2)) aa[i][i-3]=0.0;
00700       }
00701       for (k=m;k<=nn-1;k++) {
00702         if (k != m) {
00703     p=aa[k][k-1];
00704     q=aa[k+1][k-1];
00705     r=0.0;
00706     if (k != (nn-1)) r=aa[k+2][k-1];
00707     if ((x=fabs(p)+fabs(q)+fabs(r)) != 0.0) {
00708       p /= x;
00709       q /= x;
00710       r /= x;
00711     }
00712         }
00713         if ((s=PUMA_SIGN(sqrt(p*p+q*q+r*r),p)) != 0.0) {
00714     if (k == m) {
00715       if (l != m)
00716         aa[k][k-1] = -aa[k][k-1];
00717     } else
00718       aa[k][k-1] = -s*x;
00719     p += s;
00720     x=p/s;
00721     y=q/s;
00722     z=r/s;
00723     q /= p;
00724     r /= p;
00725     for (j=k;j<=nn;j++) {
00726       p=aa[k][j]+q*aa[k+1][j];
00727       if (k != (nn-1)) {
00728         p += r*aa[k+2][j];
00729         aa[k+2][j] -= p*z;
00730       }
00731       aa[k+1][j] -= p*y;
00732       aa[k][j] -= p*x;
00733     }
00734     mmin = nn<k+3 ? nn : k+3;
00735     for (i=l;i<=mmin;i++) {
00736       p=x*aa[i][k]+y*aa[i][k+1];
00737       if (k != (nn-1)) {
00738         p += z*aa[i][k+2];
00739         aa[i][k+2] -= p*r;
00740       }
00741       aa[i][k+1] -= p*q;
00742       aa[i][k] -= p;
00743     }
00744         }
00745       }
00746     }
00747         }
00748       } while (l < nn-1);
00749     }
00750     aa++; delete [] aa;
00751   }
00752 
00753 }; // end namspace puma
00754 
00755 #endif