Scopira 20080306

matrixmath.h

00001 
00002 /*
00003  *  Copyright (c) 2002-2003    National Research Council
00004  *
00005  *  All rights reserved.
00006  *
00007  *  This material is confidential and proprietary information of
00008  *  National Research Council Canada ("Confidential Information").
00009  *  This Confidential Information may only be used and reproduced
00010  *  in accordance with the terms of the license agreement.
00011  *
00012  */
00013 
00014 #ifndef __INCLUDED_SCOPIRA_BASEKIT_MATRIXMATH_H__
00015 #define __INCLUDED_SCOPIRA_BASEKIT_MATRIXMATH_H__
00016 
00017 #include <scopira/tool/output.h>
00018 #include <scopira/basekit/math.h>
00019 #include <scopira/basekit/narray.h>
00020 
00021 // THIS FILE HAS BEEN FULLY DOCUMENTED
00022 
00023 namespace scopira
00024 {
00025   namespace basekit
00026   {
00027     /*************************
00028       new data model stuff
00029     *************************/
00030 
00038     template <class T, class SCA>
00039       void mul_matrix(T &dest, SCA d);
00040 
00051     template <class T, class T2, class T3>
00052       void mul_matrix(T &dest, const T2 &m1, const T3 &m2, bool t1 = false, bool t2 = false);
00053 
00062     template <class T>
00063       bool invert_matrix(T &dest, const T& src);
00064       
00072     template <class D, class S>
00073       void transpose_matrix(D &dest, const S& src);
00074 
00081     template <class SRC, class DEST>
00082       void resample_nn(DEST &dest, const SRC &src);
00083 
00090     template <class SRC, class DEST>
00091       void resample_linear(DEST &dest, const SRC &src);
00092 
00102     template <class T> 
00103       bool lu_decomposition(narray<T,2>& dest, const narray<T,2>& src,narray<int>& idx, bool& odd);
00104     
00115     template <class T>
00116       void lu_backsubstitution(narray<T>& dest, const narray<T,2>& inv_a, const narray<int>& index);
00117   
00127     template <class T>
00128       bool svd(narray<T,2>& Mu,narray<T>& Vw,narray<T,2>& Mv);
00129     
00141     template <class T>
00142       void svd_backsubstitution(narray<T,2> &Mu, narray<T> &Vw, narray<T,2> &Mv, narray<T> &b, narray<T> &x);
00143     
00148     template <class DT> 
00149       DT pythagorean(DT a, DT b);
00150       
00158     template <class T>
00159       void elim_hessian(narray<T,2> &matrix);
00160       
00170     template <class T>
00171       void eigen_hessenberg(narray<T,2> &matrix, narray<T> &eigen_real, narray<T> &eigen_imaginary);
00172       
00183     template <class T>
00184       void gauss_jordan(narray<T,2> &matrix, narray<T,2> &rhs);
00185       
00194     template <class T>
00195       void sort_covariance(narray<T,2> &matrix, narray<int> &fitted, int mfit);
00196   }
00197 }
00198 
00199 template <class T, class SCA>
00200   void scopira::basekit::mul_matrix(T &dest, SCA d)
00201 {
00202   typename T::iterator tar, endtar;
00203 
00204   endtar = dest.end();
00205   for (tar = dest.begin(); tar != endtar; ++tar)
00206     *tar *= d;
00207 }
00208 
00209 template <class T, class T2, class T3>
00210   void scopira::basekit::mul_matrix(T &dest, const T2 &m1, const T3 &m2, bool t1, bool t2)
00211 {
00212   typedef typename T::data_type data_type;
00213   size_t w, h, x, y, i, len;
00214   size_t rx1, ry1, *x1, *y1;
00215   size_t rx2, ry2, *x2, *y2;
00216   data_type sum;
00217 
00218   // calc my dimensions and settings by the transpose flags
00219   if (t1) {
00220     h = m1.width();
00221     len = m1.height();
00222     x1 = &ry1;
00223     y1 = &rx1;
00224   } else {
00225     h = m1.height();
00226     len = m1.width();
00227     x1 = &rx1;
00228     y1 = &ry1;
00229   }
00230   if (t2) {
00231     w = m2.height();
00232     x2 = &ry2;
00233     y2 = &rx2;
00234   } else {
00235     w = m2.width();
00236     x2 = &rx2;
00237     y2 = &ry2;
00238   }
00239 
00240   assert( (t1 ? m1.height() : m1.width()) == (t2 ? m2.width() : m2.height()) );
00241 
00242   // resize myself
00243   dest.resize(w, h);
00244 
00245   // do the multiply
00246   for (y=0; y<h; y++)
00247     for (x=0; x<w; x++) {
00248       // start the accumulator
00249       sum = 0;
00250       *y1 = y;
00251       *x2 = x;
00252       for (i=0; i<len; i++) {
00253         *x1 = i;
00254         *y2 = i;
00255         sum += m1(rx1, ry1) * m2(rx2, ry2);
00256       }
00257       dest(x,y) = sum;
00258     }
00259 }
00260 
00261 template <class T>
00262   bool scopira::basekit::invert_matrix(T &dest, const T& src)
00263 {
00264   typedef typename T::data_type data_type;
00265   size_t i, j, n;
00266   bool odd;
00267   narray<data_type,2> m_orig;
00268   narray<int> v_idx;
00269   narray<data_type> v_col;
00270 
00271   n = src.width();
00272   
00273   m_orig.resize(n, src.height());
00274   dest.resize(n, src.height());
00275   v_idx.resize(n);
00276   v_idx.clear();
00277   v_col.resize(n);
00278   v_col.clear();
00279 
00280   //decompose the matrix just once
00281   if (!scopira::basekit::lu_decomposition<data_type>(m_orig, src, v_idx, odd))       
00282     return false; // a singularity in the time continuum has been detected captain, abort!
00283 
00284   //find inverse by columns
00285   for (j=0; j<n; j++) {                     
00286     v_col.clear();
00287     v_col.set(j, 1.0);
00288     scopira::basekit::lu_backsubstitution<data_type>(v_col, m_orig, v_idx);
00289     for (i=0; i<n; i++)
00290       dest.set(j, i, v_col.get(i));
00291   }
00292 
00293   return true;
00294 }
00295 
00296 template <class D, class S>
00297   void scopira::basekit::transpose_matrix(D &dest, const S& src)
00298 {
00299   size_t x, y, w, h;
00300 
00301   w = src.width();
00302   h = src.height();
00303 
00304   dest.resize(h, w);
00305 
00306   for (y=0; y<h; ++y)
00307     for (x=0; x<w; ++x)
00308       dest(y,x) = src(x,y);
00309 }
00310 
00311 template <class SRC, class DEST>
00312   void scopira::basekit::resample_nn(DEST &dest, const SRC &src)
00313 {
00314   typedef typename DEST::data_type data_type;
00315 
00316   if (dest.empty() || src.empty())
00317     return;
00318 
00319   for (size_t y=0; y<dest.height(); ++y)
00320     for (size_t x=0; x<dest.width(); ++x)
00321       dest(x,y) = src(src.width() * x / dest.width(), src.height() * y / dest.height());
00322 }
00323 
00324 template <class SRC, class DEST>
00325   void scopira::basekit::resample_linear(DEST &dest, const SRC &src)
00326 {
00327   typedef typename DEST::data_type data_type;
00328   double xsrc_rate, xleft, xremainer, xfer;   // all in "src" units
00329   double xdest_rate;
00330   double ysrc_rate, yleft, yremainer, yfer;   // all in "src" units
00331   double ydest_rate;
00332 
00333   if (dest.empty() || src.empty())
00334     return;
00335 
00336   dest.set_all(0);
00337 
00338   xsrc_rate = static_cast<double>(src.width()) / dest.width();
00339   ysrc_rate = static_cast<double>(src.height()) / dest.height();
00340   // used a mutliplier to convert src-units to dest
00341   xdest_rate = static_cast<double>(dest.width()) / src.width();
00342   ydest_rate = static_cast<double>(dest.height()) / src.height();
00343 
00344   yremainer = 0;
00345   size_t ysrc = 0;
00346   for (size_t ydest=0; ydest<dest.height(); ++ydest) {
00347     yleft = ysrc_rate;
00348 
00349     while (!scopira::basekit::is_zero(yleft)) {
00350       yfer = yleft;
00351       if (yfer + yremainer > 1)
00352         yfer = 1 - yremainer;
00353 
00354       xremainer = 0;
00355       size_t xsrc = 0;
00356       for (size_t xdest=0; xdest<dest.width(); ++xdest) {
00357         xleft = xsrc_rate;
00358 
00359         // while we still have work to do, iterate over the src blocks
00360         while (!scopira::basekit::is_zero(xleft)) {
00361           xfer = xleft;
00362           if (xfer + xremainer > 1)
00363             xfer = 1 - xremainer;
00364           dest(xdest,ydest) += static_cast<data_type>(src(xsrc,ysrc) * xfer * xdest_rate * yfer * ydest_rate);
00365           xleft -= xfer;
00366           xremainer += xfer;
00367           if (xremainer >= 1) {
00368             assert(scopira::basekit::is_equal(xremainer,1.0));   // >1 shouldnt happen
00369             xremainer = 0;
00370             ++xsrc;
00371           }
00372         }//while xleft
00373       }//for xdest
00374 
00375       yleft -= yfer;
00376       yremainer += yfer;
00377       if (yremainer >= 1) {
00378         assert(scopira::basekit::is_equal(yremainer,1.0));
00379         yremainer = 0;
00380         ++ysrc;
00381       }
00382     }//while yleft
00383   }//for ydest
00384 }
00385 
00386 // Calculates LU decomposition of a rowise permutation.
00387 template <class T>
00388   bool scopira::basekit::lu_decomposition(narray<T,2>& dest, const narray<T,2>& src, narray<int>& v_idx, bool& odd)
00389 {
00390   int i, max, j, k, n;
00391   T largest, dum, sum, tmp;
00392   //stores the implicit scaling of each row
00393   narray<T> scaling;            
00394 
00395   n = src.width();
00396   dest.copy(src);
00397   scaling.resize(n);
00398   scaling.clear();
00399   //no rows interchanged yet
00400   odd = false;           
00401   // to avoid compile warning    
00402   tmp = 0; 
00403   
00404   //loop over the rows to get the implicit scaling information
00405   for (i=0; i<n; i++) {
00406     largest = 0;
00407     for (j=0; j<n; j++) {
00408       tmp = ::fabs(dest.get(j,i));
00409       if (tmp > largest)
00410         largest = tmp;
00411     }
00412     // no non-zero largest element
00413     if (largest == 0)
00414       return false;
00415     //save the scaling
00416     scaling.set(i, 1.0/largest);          
00417   }
00418   
00419   //loop over columns 
00420   for (j=0; j<n; j++) {       
00421     for (i=0; i<j; i++) {
00422       sum = dest.get(j,i);
00423       for (k=0; k<i; k++)
00424         sum -= dest.get(k, i) * dest.get(j, k);
00425       dest.set(j, i, sum);
00426     }
00427     //init the search for the largest pivot element
00428     largest = 0;                
00429     max = 0;                
00430     for (i=j; i<n; i++) {
00431       sum = dest.get(j, i);
00432       for (k=0; k<j; k++)
00433         sum -= dest.get(k, i) * dest.get(j, k);
00434       dest.set(j, i, sum);
00435       //is the figure of merit for the pivot better than the best so far
00436       dum=scaling.get(i) * ::fabs(sum);
00437       if (dum >= largest) {
00438         largest = dum;
00439         max = i;
00440       }
00441     }
00442     //do we need to interchange rows
00443     if (j != max) {          
00444       //yes, do so...
00445       for (k=0; k<n; k++) {   
00446         dum = dest.get(k, max);
00447         dest.set(k, max, dest.get(k, j));
00448         dest.set(k, j, dum);
00449       }
00450       //...and change the parity of odd
00451       odd = !odd;
00452       //also interchange the scale factor
00453       scaling.set(max, scaling.get(j));      
00454     }
00455     v_idx.set(j, max);
00456     if (dest.get(j, j) == 0)
00457       dest.set(j, j, tinynum_c);
00458     //if the pivot element is zero, the matrix is singular (at least to the precision of
00459     //the algorithm. For some applications on singular matrices, it is desirable to
00460     //substitue tinynum_c for zero
00461     if (j != (n-1)) {            
00462        //now, finally, divide by the pivot element
00463       dum = 1.0 / dest.get(j, j);
00464       for (i=j+1; i<n; i++)
00465         dest.set(j, i, dest.get(j,i)*dum);
00466     }
00467   } //go back for the next column in the reduction  
00468   return true;
00469 }
00470 
00471 
00472 // Solves the set of n linear equations a�x = b.
00473 template <class T>
00474   void scopira::basekit::lu_backsubstitution(narray<T>& dest, const narray<T,2>& inv_a, const narray<int>& index)
00475 {
00476   int i, j,k, m, n;
00477   T sum;
00478   
00479   n = inv_a.width();
00480   m = -1;
00481   
00482   //forward substitution,
00483   for (i=0; i<n; i++) {
00484     k = index.get(i);
00485     sum = dest.get(k);
00486     dest.set(k, dest.get(i));
00487     if (m != -1)
00488       for (j=m; j<= i-1; j++)
00489         sum -= inv_a.get(j, i) * dest.get(j);
00490     else
00491       if (sum != 0.0)
00492         m=i;
00493     dest.set(i, sum);
00494   }
00495   
00496   dest.copy(dest);
00497   //backsubstitution,
00498   for (i=n-1; i>=0; i--) {    
00499     sum = dest.get(i);
00500     for (j=i+1; j<n; j++)
00501       sum-=inv_a.get(j,i) * dest.get(j);
00502     //store a component of the solution vector dest.
00503     dest.set(i, sum/inv_a.get(i,i));
00504   }
00505 }
00506 
00507 
00508 namespace scopira
00509 {
00510   namespace basekit
00511   {
00513     template <class T>
00514       inline T svd_sign(T v1, T v2) { if (v2>=0.0) return fabs(v1); else return -fabs(v1); }
00515   }
00516 }
00517 
00518 //construct the singular value decomposition of the matrix u
00519 template <class T>    
00520   bool scopira::basekit::svd(narray<T,2>& u,narray<T>& w,narray<T,2>& v)
00521 {
00522 
00523   int i,j,k,left,height,width,nm,q,p;
00524   T a,b,c,num_r,h,s,scale,x,y,z;
00525   bool flag;
00526   narray<T> vec_t;
00527   
00528   width = u.width();
00529   height=u.height();
00530 
00531   v.resize(width,width);
00532   v.clear();
00533   w.resize(width);
00534   w.clear();
00535   
00536   b=scale=num_r=0.0;  
00537   left= nm = 0;
00538   
00539   vec_t.resize(width);
00540   vec_t.clear();
00541   //householder reduction to bidiagonal form 
00542   for(i=0;i<width;i++){
00543     //left-hand reduction 
00544     left = i+1;
00545     vec_t[i]=(scale*b);
00546     b = s = scale = 0.0;
00547     
00548     if(i < height) {
00549       for(k=i;k<height;k++)
00550         scale += fabs(u(i,k));
00551       if(scale != 0.0) {
00552         for(k=i;k<height;k++) {
00553           u(i,k)=u(i,k)/scale;
00554           s += u(i,k) * u(i,k);
00555         }
00556       
00557         a = u(i,i);
00558         b = -svd_sign(sqrt(s),a);
00559         h = a * b - s;
00560         u(i,i)=a - b;
00561         
00562         for(j=left;j<width;j++) {
00563           s=0.0;
00564           for(k=i;k<height;k++)
00565             s += u(i,k) * u(j,k);
00566           a = s / h;
00567           for(k=i;k<height;k++)
00568             u(j,k)=u(j,k)+ a * u(i,k);
00569         }
00570         for(k=i;k<height;k++)
00571           u(i,k)=u(i,k)*scale;
00572       }
00573     }
00574   
00575     w[i]=scale *b;
00576     b=s=scale=0.0;
00577   
00578     //right-hand reduction 
00579     if(i<height && i+1 != width) {
00580       
00581       for (k=left;k<width;k++)
00582         scale += fabs(u(k,i));
00583       
00584       if (scale != 0.0){
00585         for (k=left;k<width;k++){
00586           u(k,i) = u(k,i)/ scale;
00587           s += u(k,i) * u(k,i);
00588         }
00589    
00590         a=u(left,i);
00591         b = -svd_sign(sqrt(s),a);
00592         h=a * b - s;
00593         u(left,i)= a-b;
00594         for (k=left;k<width;k++)
00595           vec_t[k] = u(k,i)/h;
00596         for (j=left;j<height;j++){
00597           s=0.0;
00598           for (k=left;k<width;k++)         
00599             s += u(k,j)* u(k,i);
00600           for (k=left;k<width;k++)   
00601             u(k,j) = u(k,j) + s * vec_t[k];
00602         }
00603         for (k=left;k<width;k++)        
00604           u(k,i) = u(k,i) * scale;
00605       } 
00606     }
00607     num_r=std::max(num_r,(fabs(w[i])+fabs(vec_t[i])));   
00608   }
00609   
00610   //accumulation of right-hand transformations.
00611   for (i=width-1;i>=0;i--){
00612     if(i < width-1){
00613       if (b !=0.0){
00614         //double division to avoid possible underflow.
00615         for (j=left;j<width;j++)
00616           v(i,j)=(u(j,i) /u(left,i))/b;
00617           
00618         for (j=left;j<width;j++){
00619           s=0.0;
00620           for (k=left;k<width;k++)
00621             s+=u(k,i)* v(j,k);
00622           for (k=left;k<width;k++)
00623             v(j,k) = v(j,k)+ s * v(i,k);
00624         }
00625       }
00626       for (j=left;j<width;j++){
00627         v(j,i) = 0.0;
00628         v(i,j) = 0.0; 
00629       }
00630     }
00631     v(i,i) = 1.0;
00632     b=vec_t[i];
00633     left=i;
00634   }
00635   
00636   //accumulation of left-hand transformations.
00637   for (i=std::min(height,width)-1;i>=0;i--)
00638   {
00639     left=i+1;
00640     b=w[i];
00641     for (j=left;j<width;j++)
00642       u(j,i) = 0.0;
00643     if (b !=0.0){
00644       b = 1.0 / b;
00645       for (j=left;j<width;j++){
00646         s=0.0;
00647         for (k=left;k<height;k++)
00648           s+= u(i,k)* u(j,k);
00649         a=(s/u(i,i) )*b;
00650         for (k=i;k<height;k++)
00651           u(j,k) = u(j,k) + a * u(i,k);
00652       }
00653       for (j=i;j<height;j++)
00654         u(i,j) = u(i,j) * b;
00655     }
00656     else
00657       for (j=i;j<height;j++)
00658         u(i,j) = 0.0;
00659 
00660     u(i,i) = u(i,i) + 1;  
00661   }
00662 
00663   //diagonalization of the bidiagonal form: Loop over
00664   //singular values, and over allowed iterations.
00665   for (k=width-1;k>=0;k--){
00666     for (p=0;p<30;p++){
00667       flag=true;
00668       for (left=k;left>=0;left--){ 
00669         //test for splitting.
00670         nm = left-1;    //note that vec_t[1] is always zero.
00671         if (static_cast<T>(fabs(vec_t[left])+num_r) == static_cast<T>(num_r)){
00672           flag=false;
00673           break;
00674         }
00675         if (static_cast<T>(fabs(w[nm])+num_r) == static_cast<T>(num_r))
00676           break;
00677       }
00678       if (flag){
00679         c=0.0; //cancellation of vec_t[l], ifl>1.
00680         s=1.0;
00681         for (i=left;i<=k;i++) {
00682 
00683           a=s * vec_t[i];
00684           vec_t[i] = c * vec_t[i];
00685 
00686           if (static_cast<T>(fabs(a)+num_r) == static_cast<T>(num_r))
00687             break;
00688             
00689           b=w[i];
00690           h= pythagorean<T>(a,b);
00691           w[i] = h;
00692           h=1.0/h;
00693           c=b*h;
00694           s = -a*h;
00695 
00696           for (j=0;j<height;j++){
00697             y=u(nm,j);
00698             z=u(i,j);
00699             u(nm,j) = y*c+z*s;
00700             u(i,j) = z*c-y*s;
00701           }
00702         }
00703       }
00704 
00705       z=w[k];
00706       
00707       if (left == k){
00708         //convergence.
00709         if (z < 0.0){
00710           //singular value is made nonnegative.
00711           w[k] = -z;
00712           for (j=0;j<width;j++)
00713             u(k,j) =-(u(k,j));
00714         }
00715         break;
00716       }
00717       
00718       if (p == 29)
00719         return false;
00720 
00721       x=w[left]; //shift from bottom 2-by-2minor.
00722       nm=k-1;
00723       y=w[nm];
00724       b=vec_t[nm];
00725       h=vec_t[k];
00726       a=((y-z)*(y+z)+(b-h)*(b+h))/(2.0*h*y);
00727       b= pythagorean<T>(a,1.0);
00728       a = ((x - z) * (x + z) + h * ((y / (a + svd_sign(b,a))) - h)) / x;
00729       c=s=1.0;
00730       //next QR transformation:
00731       for (j=left;j<=nm;j++){
00732         i=j+1;
00733         b=vec_t[i];
00734         y=w[i];
00735         h=s*b;
00736         b=c*b;
00737         z= pythagorean<T>(a,h);        
00738         vec_t[j] = z;
00739         c=a/z;
00740         s=h/z;
00741         a=x*c+b*s;
00742         b = b*c-x*s;
00743         h=y*s;
00744         y *= c;
00745         for (q=0;q<width;q++){
00746           x=v(j,q);
00747           z=v(i,q);
00748           v(j,q) = x*c+z*s;
00749           v(i,q) = z*c-x*s;
00750         }
00751         z=pythagorean<T>(a,h);
00752         w[j] = z; //rotation can be arbitrary if z = 0.
00753         if (z != 0.0){
00754           z=1.0/z;
00755           c=a*z;
00756           s=h*z;
00757         }
00758         a=c*b+s*y;
00759         x=c*y-s*b;
00760         for (q=0;q<height;q++){
00761           y=u(j,q);
00762           z=u(i,q);
00763           u(j,q) = y*c+z*s;
00764           u(i,q) = z*c-y*s;
00765         }
00766       }
00767      
00768       vec_t[left] = 0.0;
00769       vec_t[k] = a;
00770       w[k] = x;
00771     } 
00772   }
00773    return true;
00774 }
00775 
00776 // solves Dx = b for x using backsubstitution and the results of SVD
00777 template <class T>
00778   void scopira::basekit::svd_backsubstitution(narray<T,2> &Mu, narray<T> &Vw, narray<T,2> &Mv, narray<T> &b, narray<T> &x)
00779 {
00780   narray<T> temp;
00781   T s;
00782   int w, h, i, j;
00783   
00784   w = Mu.width();
00785   h = Mu.height();
00786   temp.resize(w);
00787   
00788   for (i=0; i<w; i++) {
00789     s = 0;
00790     if (Vw[i] != 0) {
00791       for (j=0; j<h; j++) {
00792         s += Mu(i,j) * b[j];
00793       }
00794       s = s/Vw[i];
00795     }
00796     temp[i] = s;
00797   }
00798   
00799   for (i=0; i<w; i++) {
00800     s = 0;
00801     for (j=0; j<w; j++) {
00802       s+= Mv(j,i) * temp[j];
00803     }
00804     x[i] = s;
00805   }
00806 }
00807 
00808 //Computes Pythagorean Theorem: (a^2 + b^2)^0.5 without destructive underflow or overflow
00809 template <class DT> 
00810   DT scopira::basekit::pythagorean(DT a, DT b)
00811 {
00812   DT abs_a,abs_b;
00813 
00814   abs_a = fabs(a);
00815   abs_b = fabs(b);
00816 
00817   if(abs_a < abs_b)
00818     return(abs_b == 0.0 ? 0.0 : abs_b * sqrt(pow((abs_a / abs_b),2) +1.0));
00819   else
00820     return(abs_a * sqrt(pow((abs_b/abs_a),2) +1.0));
00821 }
00822 
00823 // reduces a general matrix to hessenberg form
00824 template <class T>
00825   void scopira::basekit::elim_hessian(narray<T,2> &matrix)
00826 {
00827   int i, j, m, n;
00828   T x, y, temp;
00829   
00830   n = matrix.height();
00831   for (m=1; m<n-1; m++) {
00832     x = 0.0;
00833     i = m;
00834     for (j=m; j<n; j++) {
00835       if (fabs(matrix(m-1,j)) > fabs(x)) {
00836         x = matrix(m-1,j);
00837         i = j;
00838       }
00839     }
00840     if (i != m) {
00841       for (j = m-1; j<n; j++) {
00842         temp = matrix(j,i);
00843         matrix(j,i) = matrix(j,m);
00844         matrix(j,m) = temp;
00845       }
00846       for (j=0; j<n; j++) {
00847         temp = matrix(i,j);
00848         matrix(i,j) = matrix(m,j);
00849         matrix(m,j) = temp;
00850       }
00851     }
00852     if (x != 0.0) {
00853       for (i=m+1; i<n; i++) {
00854         y = matrix(m-1,i);
00855         if (y != 0.0) {
00856           y /= x;
00857           matrix(m-1,i) = y;
00858           for (j=m; j<n; j++)
00859             matrix(j,i) -= y*matrix(j,m);
00860           for (j=0; j<n; j++)
00861             matrix(m,j) += y*matrix(i,j);
00862         }
00863       }
00864     }
00865   }
00866 }
00867 
00868 // calculates the eigenvalues of a Hessenberg matrix
00869 template <class T>
00870   void scopira::basekit::eigen_hessenberg(narray<T,2> &matrix, narray<T> &eigen_real, narray<T> &eigen_imaginary)
00871 {
00872   int n,nn,m,l,k,mmin,i, j, iterations;
00873   T matrix_norm, z,y,x,w,v,u,t,s,r,q,p;;
00874   
00875   n = matrix.height();
00876   matrix_norm = fabs(matrix(0,0));
00877   for (i=1; i<n; i++) {
00878     for (j=i-1; j<n; j++) {
00879       matrix_norm += fabs(matrix(j,i));
00880     }
00881   }
00882   nn = n-1;
00883   t = 0.0;
00884   while (nn >= 0) {
00885     iterations = 0;
00886     do {
00887       for (l=nn;l>=1; l--) {
00888         s = fabs(matrix(l-1,l-1)) + fabs(matrix(l,l));
00889         if (s == 0.0)
00890           s = matrix_norm;
00891         if ((fabs(matrix(l-1,l)) + s) == s)
00892           break;
00893       }
00894       x = matrix(nn,nn);
00895       if (l == nn) {
00896         eigen_real[nn] = x + t;
00897         eigen_imaginary[nn--] = 0.0;
00898       } else {
00899         y = matrix(nn-1,nn-1);
00900         w = matrix(nn-1,nn)*matrix(nn,nn-1);
00901         if (l == (nn-1)) {
00902           p = 0.5*(y-x);
00903           q = p*p + w;
00904           z = sqrt(fabs(q));
00905           x += t;
00906           if (q >= 0.0) {
00907             z = p + SIGN(z,p);
00908             eigen_real[nn-1] = eigen_real[nn] = x + z;
00909             if (z != 0.0)
00910               eigen_real[nn] = x - w/z;
00911             eigen_imaginary[nn-1] = eigen_imaginary[nn] = 0.0;
00912           } else {
00913             eigen_real[nn-1] = eigen_real[nn] = x + p;
00914             eigen_imaginary[nn-1] = -(eigen_imaginary[nn] = z);
00915           }
00916           nn -= 2;
00917         } else {
00918           if (iterations == 30) {
00919             OUTPUT << "Too many iterations while computing Hessenberg eigenvalues\n";
00920             return;
00921           }
00922           if (iterations == 10 || iterations == 20) {
00923             t += x;
00924             for (i=0; i <= nn; i++)
00925               matrix(i,i) -= x;
00926             s = fabs(matrix(nn-1,nn)) + fabs(matrix(nn-2,nn-1));
00927             y = x = 0.75*s;
00928             w = -0.4375*s*s;
00929           }
00930           ++iterations;
00931           for (m=nn-2; m>=l; m--) {
00932             z = matrix(m,m);
00933             r = x - z;
00934             s = y - z;
00935             p = (r*s-w)/matrix(m,m+1) + matrix(m+1,m);
00936             q = matrix(m+1,m+1) - z - r - s;
00937             r = matrix(m+1,m+2);
00938             s = fabs(p) + fabs(q) + fabs(r);
00939             p /= s;
00940             q /= s;
00941             r /= s;
00942             if (m == l)
00943               break;
00944             u = fabs(matrix(m-1,m)) *  (fabs(q) + fabs(r));
00945             v = fabs(p) * (fabs(matrix(m-1,m-1)) + fabs(z) + fabs(matrix(m+1,m+1)));
00946             if ((u + v) == v)
00947               break;
00948           }
00949           for (i=m+2; i<=nn; i++) {
00950             matrix(i-2,i) = 0.0;
00951             if (i != (m+2))
00952               matrix(i-3,i) = 0.0;
00953           }
00954           for (k=m; k<=nn-1; k++) {
00955             if (k != m) {
00956               p = matrix(k-1,k);
00957               q = matrix(k-1,k+1);
00958               r = 0.0;
00959               if (k != nn-1)
00960                 r = matrix(k-1,k+2);
00961               if ((x = fabs(p) + fabs(q) + fabs(r)) != 0.0) {
00962                 p /= x;
00963                 q /=x;
00964                 r /= x;
00965               }
00966             }
00967             if ((s = SIGN(sqrt(p*p + q*q + r*r),p)) != 0.0) {
00968               if (k == m) {
00969                 if (l != m) 
00970                   matrix(k-1,k) = -matrix(k-1,k);
00971               } else {
00972                 matrix(k-1,k) = -s*x;
00973               }
00974               p += s;
00975               x = p/s;
00976               y = q/s;
00977               z = r/s;
00978               q /= p;
00979               r /= p;
00980               for (j=k; j<=nn; j++) {
00981                 p = matrix(j,k) + q*matrix(j,k+1);
00982                 if (k != nn-1) {
00983                   p += r*matrix(j,k+2);
00984                   matrix(j,k+2) -= p*z;
00985                 }
00986                 matrix(j,k+1) -= p*y;
00987                 matrix(j,k) -= p*x;
00988               }
00989               mmin = nn < k+3 ? nn : k+3;
00990               for (i=l; i<=mmin; i++) {
00991                 p = x*matrix(k,i) + y*matrix(k+1,i);
00992                 if (k != nn-1) {
00993                   p += z*matrix(k+2,i);
00994                   matrix(k+2,i) -= p*r;
00995                 }
00996                 matrix(k+1,i) -= p*q;
00997                 matrix(k,i) -= p;
00998               }
00999             }
01000           }
01001         }
01002       }
01003     } while (l < nn-1);
01004   }
01005 }
01006 
01007 // Gauss-Jordan matrix inversion and linear equation solution
01008 template <class T>
01009   void scopira::basekit::gauss_jordan(narray<T,2> &matrix, narray<T,2> &rhs)
01010 {
01011   narray<int> index_x, index_y, index_pivot;
01012   int i, x, y, j, k, l, ll, n, m;
01013   T max, pivot_inv, temp;
01014   
01015   n = matrix.height();
01016   m = rhs.width();
01017   index_x.resize(n);
01018   index_y.resize(n);
01019   index_pivot.resize(n);
01020   for (j=0; j<n; j++) 
01021     index_pivot[j] = 0;
01022   for (i=0; i<n; i++) {
01023     max = 0.0;
01024     for (j=0; j<n; j++) {
01025       if (index_pivot[j] != 1) {
01026         for (k=0; k<n; k++) {
01027           if (index_pivot[k] == 0) {
01028             if (fabs(matrix(k,j)) >= max) {
01029               max = fabs(matrix(k,j));
01030               x = k;
01031               y = j;
01032             }
01033           } else if (index_pivot[k] > 1) {
01034             OUTPUT << "Singular matrix-1 in Gauss-Jordan elimination\n";
01035             return;
01036           }
01037         }
01038       }
01039     }
01040     index_pivot[x]++;
01041     if (x != y) {
01042       for (l=0; l<n; l++) {
01043         temp = matrix(l,y);
01044         matrix(l,y) = matrix(l,x);
01045         matrix(l,x) = temp;
01046       }
01047       for (l=0; l<m; l++) {
01048         temp = rhs(l,y);
01049         rhs(l,y) = rhs(l,x);
01050         rhs(l,x) = temp;
01051       }
01052     }
01053     
01054     index_y[i] = y;
01055     index_x[i] = x;
01056     if (matrix(x,x) == 0.0) {
01057       OUTPUT << "Singular matrix-1 in Gauss-Jordan elimination\n";
01058       return;
01059     }
01060     pivot_inv = 1.0/matrix(x,x);
01061     matrix(x,x) = 1.0;
01062     for (l=0; l<n; l++)
01063       matrix(l,x) *= pivot_inv;
01064     for (l=0; l<m; l++)
01065       rhs(l,x) *= pivot_inv;
01066     for (ll=0; ll<n; ll++) {
01067       if (ll != x) {
01068         temp = matrix(x,ll);
01069         matrix(x,ll) = 0.0;
01070         for (l=0; l<n; l++)
01071           matrix(l,ll) -= matrix(l,x)*temp;
01072         for (l=0; l<m; l++) 
01073           rhs(l,ll) -= rhs(l,x)*temp;
01074       }
01075     }
01076   }
01077   for (l=n-1; l>=0; l--) {
01078     if (index_y[l] != index_x[l]) {
01079       for (k=0; k<n; k++) {
01080         temp = matrix(index_y[l],k);
01081         matrix(index_y[l],k) = matrix(index_x[l],k);
01082         matrix(index_x[l],k) = temp;
01083       }
01084     }
01085   } 
01086 }
01087 
01088 // rearranges covariance matrix
01089 template <class T>
01090   void scopira::basekit::sort_covariance(narray<T,2> &matrix, narray<int> &fitted, int mfit)
01091 {
01092   int i,j,k,n;
01093   T temp;
01094   
01095   n = matrix.height();
01096   for (i=mfit; i<n; i++) {
01097     for (j=0; j<=i; j++) {
01098       matrix(j,i) = matrix(i,j) = 0.0;
01099     }
01100   }
01101   k = mfit-1;
01102   for (j=n-1; j>=0; j--) {
01103     if (fitted[j]) {
01104       for (i=0; i<n; i++) {
01105         temp = matrix(k,i);
01106         matrix(k,i) = matrix(j,i);
01107         matrix(j,i) = temp;
01108       }
01109       for (i=0; i<n; i++) {
01110         temp = matrix(i,k);
01111         matrix(i,k) = matrix(i,j);
01112         matrix(i,j) = temp;
01113       }
01114       k--;
01115     }
01116   }
01117 }
01118 
01119 #endif