ADMB Documentation  11.1.2495
 All Classes Files Functions Variables Typedefs Friends Defines
dmat42.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: dmat42.cpp 2262 2014-08-28 19:31:27Z johnoel $
00003  *
00004  * Author: David Fournier
00005  * Copyright (c) 2009-2012 ADMB Foundation
00006  */
00015 #include <fvar.hpp>
00016 #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
00017 
00018 int svd(int m, int n, int withu, int withv, double eps, double tol,
00019         const dmatrix& a, const dvector& _q,
00020         const dmatrix& _u, const dmatrix& _v);
00021 int svd_nlm(int m, int n, int withu, int withv, double eps, double tol,
00022             const dmatrix& aa, const dvector& _q,
00023             const dmatrix& _u, const dmatrix& _v);
00024 int svd_mln(int m, int n, int withu, int withv, double eps, double tol,
00025             const dmatrix& aa, const dvector& _q,
00026             const dmatrix& _u, const dmatrix& _v);
00027 
00028 /*
00029 static double pythag(double a, double b)
00030 {
00031   double fa=fabs(a);
00032   double fb=fabs(b);
00033   if (fa>fb)
00034     return fa*sqrt(1.0+square(fb/fa));
00035   else
00036     return fb*sqrt(1.0+square(fa/fb));
00037 }
00038 */
00039 
00040 /*
00041 class sing_val_decomp
00042 {
00043   dmatrix a;
00044   dvector w;
00045   dmatrix v;
00046 public:
00047   sing_val_decomp(const dmatrix& _a, const dvector & _w,const dmatrix& _v);
00048   dmatrix get_u(void){return a;}
00049   dvector get_w(void){return w;}
00050   dmatrix get_v(void){return v;}
00051 };
00052 */
00053 
00058 sing_val_decomp::sing_val_decomp(const dmatrix& _a, const dvector & _w,
00059   const dmatrix& _v) :
00060     a(_a), w(_w), v(_v)
00061 {}
00062 
00067 sing_val_decomp singval_decomp(const dmatrix &_a)
00068 {
00069   if (_a.indexmin() !=1 )
00070   {
00071     cerr << "index error in singval_decomp" << endl;
00072     ad_exit(1);
00073   }
00074   int m = _a.indexmax();
00075   int n = _a(1).indexmax();
00076   dmatrix a(1,m,1,n);
00077   a=_a;
00078   dvector w(1,n);
00079   dmatrix u(1,m,1,n);
00080   dmatrix v(1,n,1,n);
00081 
00082   double eps = 1.e-12;
00083   double tol = eps;
00084   int k = svd(m,n,1,1,eps,tol,a,w,u,v);
00085   if(k!=0)
00086   {
00087     cerr << "Error in singval_decomp in iteration " << k << endl;
00088     ad_exit(1);
00089   }
00090   return sing_val_decomp(u,w,v);
00091 }
00092 
00114 int svd(int m,int n,int withu,int withv,double eps,double tol,
00115         const dmatrix& aa, const dvector& _q,
00116         const dmatrix& _u, const dmatrix& _v)
00117 {
00118   ADUNCONST(dmatrix,u)
00119   ADUNCONST(dmatrix,v)
00120   ADUNCONST(dvector,q)
00121 
00122   int urlb=u.rowmin();
00123   int uclb=u.colmin();
00124   u.rowshift(0);
00125   u.colshift(0);
00126   int vrlb=v.rowmin();
00127   int vclb=v.colmin();
00128   v.rowshift(0);
00129   v.colshift(0);
00130   int qlb=q.indexmin();
00131   q.shift(0);
00132   dmatrix a=aa;
00133   int arlb=a.rowmin();
00134   int aclb=a.colmin();
00135   a.rowshift(0);
00136   a.colshift(0);
00137 
00138   int k;
00139   if(m>=n)
00140     k = svd_nlm(m,n,withu,withv,eps,tol,a,q,u,v);
00141   else
00142     k = svd_mln(m,n,withu,withv,eps,tol,a,q,u,v);
00143 
00144   u.rowshift(urlb);
00145   u.colshift(uclb);
00146   v.rowshift(vrlb);
00147   v.colshift(vclb);
00148   q.shift(qlb);
00149   a.rowshift(arlb);
00150   a.colshift(aclb);
00151 
00152   return k;
00153 }
00154 
00166 int svd_mln(int m, int n, int  withu, int withv, double eps, double tol,
00167             const dmatrix& aa, const dvector& _q,
00168             const dmatrix& _u, const dmatrix& _v)
00169 {
00170   ADUNCONST(dmatrix,u)
00171   ADUNCONST(dmatrix,v)
00172   ADUNCONST(dvector,q)
00173 
00174   int i,j,k,l,l1,iter,retval;
00175   double c,f,g,h,s,x,y,z;
00176   double *e;
00177 
00178   e = (double *)calloc(n,sizeof(double));
00179   retval = 0;
00180 
00181   u=aa;
00182 
00183 /* Householder's reduction to bidiagonal form. */
00184   g = x = 0.0;
00185   for (i=0;i<n;i++)
00186   {
00187     e[i] = g;
00188     s = g = 0.0;
00189     l = i+1;
00190     if( i<m )
00191     {
00192       for (j=i;j<m;j++)
00193       {
00194         s += (u[j][i]*u[j][i]);
00195       }
00196       if (s < tol)
00197         g = 0.0;
00198       else
00199       {
00200         f = u[i][i];
00201         g = (f < 0) ? sqrt(s) : -sqrt(s);
00202         h = f * g - s;
00203         u[i][i] = f - g;
00204         for (j=l;j<n;j++)
00205         {
00206           s = 0.0;
00207           for (k=i;k<m;k++)
00208           {
00209             s += (u[k][i] * u[k][j]);
00210           }
00211           f = s / h;
00212           for (k=i;k<m;k++)
00213           {
00214             u[k][j] += (f * u[k][i]);
00215           }
00216         } /* end j */
00217       } /* end s */
00218     }
00219     q[i] = g;
00220     s = g = 0.0;
00221     if( i<m && i!=n-1 )
00222     {
00223       for (j=l;j<n;j++)
00224       {
00225         s += (u[i][j] * u[i][j]);
00226       }
00227       if (s < tol)
00228         g = 0.0;
00229       else
00230       {
00231         f = u[i][i+1];
00232         g = (f < 0) ? sqrt(s) : -sqrt(s);
00233         h = f * g - s;
00234         u[i][i+1] = f - g;
00235         for (j=l;j<n;j++)
00236         {
00237           e[j] = u[i][j]/h;
00238         }
00239         for (j=l;j<m;j++)
00240         {
00241           s = 0.0;
00242           for (k=l;k<n;k++)
00243           {
00244             s += (u[j][k] * u[i][k]);
00245           }
00246           for (k=l;k<n;k++)
00247           {
00248             u[j][k] += (s * e[k]);
00249           }
00250         } /* end j */
00251       } /* end s */
00252     } /* end if*/
00253     y = fabs(q[i]) + fabs(e[i]);
00254     if (y > x)
00255     {
00256       x = y;
00257     }
00258   } /* end i */
00259 
00260 /* accumulation of right-hand transformations */
00261   if (withv)
00262   {
00263     //assert(l == n);
00264     l = n;
00265     for (i=n-1;i>=0;i--)
00266     {
00267       if ( i < n-2 )
00268       {
00269         if (g != 0.0)
00270         {
00271           h = u[i][i+1] * g;
00272           for (j=l;j<n;j++)
00273           {
00274             v[j][i] = u[i][j]/h;
00275           }
00276           for (j=l;j<n;j++)
00277           {
00278             s = 0.0;
00279             for (k=l;k<n;k++)
00280             {
00281               s += (u[i][k] * v[k][j]);
00282             }
00283             for (k=l;k<n;k++)
00284             {
00285               v[k][j] += (s * v[k][i]);
00286             }
00287           } /* end j */
00288         } /* end g */
00289         for (j=l;j<n;j++)
00290         {
00291           v[i][j] = v[j][i] = 0.0;
00292         }
00293       }
00294       v[i][i] = 1.0;
00295       g = e[i];
00296       l = i;
00297     } /* end i */
00298   } /* end withv, parens added for clarity */
00299 
00300 /* accumulation of left-hand transformations */
00301   if (withu) {
00302     for (i=min(m,n)-1;i>=0;i--) {
00303       l = i + 1;
00304       g = q[i];
00305       for (j=l;j<n;j++)  /* upper limit was 'n' */
00306         u[i][j] = 0.0;
00307       if (g != 0.0) {
00308         h = u[i][i] * g;
00309         for (j=l;j<n;j++) { /* upper limit was 'n' */
00310           s = 0.0;
00311           for (k=l;k<m;k++)
00312             s += (u[k][i] * u[k][j]);
00313           f = s / h;
00314           for (k=i;k<m;k++)
00315             u[k][j] += (f * u[k][i]);
00316         } /* end j */
00317         for (j=i;j<m;j++)
00318           u[j][i] /= g;
00319       } /* end g */
00320       else {
00321         for (j=i;j<m;j++)
00322           u[j][i] = 0.0;
00323       }
00324       u[i][i] += 1.0;
00325     } /* end i*/
00326   } /* end withu, parens added for clarity */
00327 
00328 /* diagonalization of the bidiagonal form */
00329   eps *= x;
00330   for (k=n-1;k>=0;k--) {
00331     iter = 0;
00332 test_f_splitting:
00333     for (l=k;l>=0;l--) {
00334       if (fabs(e[l]) <= eps) goto test_f_convergence;
00335       if (fabs(q[l-1]) <= eps) goto cancellation;
00336     } /* end l */
00337 
00338 /* cancellation of e[l] if l > 0 */
00339 cancellation:
00340     c = 0.0;
00341     s = 1.0;
00342     l1 = l - 1;
00343     for (i=l;i<=k;i++) {
00344       f = s * e[i];
00345       e[i] *= c;
00346       if (fabs(f) <= eps) goto test_f_convergence;
00347       g = q[i];
00348       h = q[i] = sqrt(f*f + g*g);
00349       c = g / h;
00350       s = -f / h;
00351       if (withu) {
00352         for (j=0;j<m;j++) {
00353           y = u[j][l1];
00354           z = u[j][i];
00355           u[j][l1] = y * c + z * s;
00356           u[j][i] = -y * s + z * c;
00357         } /* end j */
00358       } /* end withu, parens added for clarity */
00359     } /* end i */
00360 test_f_convergence:
00361     z = q[k];
00362     if (l == k) goto convergence;
00363 
00364 /* shift from bottom 2x2 minor */
00365     iter++;
00366     if (iter > 30) {
00367       retval = k;
00368       break;
00369     }
00370     x = q[l];
00371     y = q[k-1];
00372     g = e[k-1];
00373     h = e[k];
00374     f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2*h*y);
00375     g = sqrt(f*f + 1.0);
00376     f = ((x-z)*(x+z) + h*(y/((f<0)?(f-g):(f+g))-h))/x;
00377 /* next QR transformation */
00378     c = s = 1.0;
00379     for (i=l+1;i<=k;i++) {
00380       g = e[i];
00381       y = q[i];
00382       h = s * g;
00383       g *= c;
00384       e[i-1] = z = sqrt(f*f+h*h);
00385       c = f / z;
00386       s = h / z;
00387       f = x * c + g * s;
00388       g = -x * s + g * c;
00389       h = y * s;
00390       y *= c;
00391       if (withv) {
00392         for (j=0;j<n;j++) {
00393           x = v[j][i-1];
00394           z = v[j][i];
00395           v[j][i-1] = x * c + z * s;
00396           v[j][i] = -x * s + z * c;
00397         } /* end j */
00398       } /* end withv, parens added for clarity */
00399       q[i-1] = z = sqrt(f*f + h*h);
00400       c = f/z;
00401       s = h/z;
00402       f = c * g + s * y;
00403       x = -s * g + c * y;
00404       if (withu) {
00405         for (j=0;j<m;j++) {
00406           y = u[j][i-1];
00407           z = u[j][i];
00408           u[j][i-1] = y * c + z * s;
00409           u[j][i] = -y * s + z * c;
00410         } /* end j */
00411       } /* end withu, parens added for clarity */
00412     } /* end i */
00413     e[l] = 0.0;
00414     e[k] = f;
00415     q[k] = x;
00416     goto test_f_splitting;
00417 convergence:
00418     if (z < 0.0) {
00419 /* q[k] is made non-negative */
00420       q[k] = - z;
00421       if (withv) {
00422         for (j=0;j<n;j++)
00423           v[j][k] = -v[j][k];
00424       } /* end withv, parens added for clarity */
00425     } /* end z */
00426   } /* end k */
00427 
00428   free(e);
00429 
00430   return retval;
00431 }
00432 
00444 int svd_nlm(int m, int n, int withu, int withv, double eps, double tol,
00445             const dmatrix& aa, const dvector& _q,
00446             const dmatrix& _u, const dmatrix& _v)
00447 {
00448   ADUNCONST(dmatrix,u)
00449   ADUNCONST(dmatrix,v)
00450   ADUNCONST(dvector,q)
00451 
00452   int i,j,k,l,l1,iter,retval;
00453   double c,f,g,h,s,x,y,z;
00454   double *e;
00455 
00456   e = (double *)calloc(n,sizeof(double));
00457   retval = 0;
00458 
00459   u=aa;
00460 /* Householder's reduction to bidiagonal form. */
00461   g = x = 0.0;
00462   for (i=0;i<n;i++)
00463   {
00464     e[i] = g;
00465     s = 0.0;
00466     l = i+1;
00467     for (j=i;j<m;j++)
00468     {
00469       s += (u[j][i]*u[j][i]);
00470     }
00471     if (s < tol)
00472       g = 0.0;
00473     else
00474     {
00475       f = u[i][i];
00476       g = (f < 0) ? sqrt(s) : -sqrt(s);
00477       h = f * g - s;
00478       u[i][i] = f - g;
00479       for (j=l;j<n;j++)
00480       {
00481         s = 0.0;
00482         for (k=i;k<m;k++)
00483         {
00484           s += (u[k][i] * u[k][j]);
00485         }
00486         f = s / h;
00487         for (k=i;k<m;k++)
00488         {
00489           u[k][j] += (f * u[k][i]);
00490         }
00491       } /* end j */
00492     } /* end s */
00493     q[i] = g;
00494     s = 0.0;
00495     for (j=l;j<n;j++)
00496     {
00497       s += (u[i][j] * u[i][j]);
00498     }
00499     if (s < tol)
00500       g = 0.0;
00501     else
00502     {
00503       f = u[i][i+1];
00504       g = (f < 0) ? sqrt(s) : -sqrt(s);
00505       h = f * g - s;
00506       u[i][i+1] = f - g;
00507       for (j=l;j<n;j++)
00508       {
00509         e[j] = u[i][j]/h;
00510       }
00511       for (j=l;j<m;j++)
00512       {
00513         s = 0.0;
00514         for (k=l;k<n;k++)
00515         {
00516           s += (u[j][k] * u[i][k]);
00517         }
00518         for (k=l;k<n;k++)
00519         {
00520           u[j][k] += (s * e[k]);
00521         }
00522       } /* end j */
00523     } /* end s */
00524     y = fabs(q[i]) + fabs(e[i]);
00525     if (y > x)
00526     {
00527       x = y;
00528     }
00529   } /* end i */
00530 
00531 /* accumulation of right-hand transformations */
00532   if (withv)
00533   {
00534     //assert(l == n);
00535     l = n;
00536     for (i=n-1;i>=0;i--)
00537     {
00538       if (g != 0.0)
00539       {
00540         h = u[i][i+1] * g;
00541         for (j=l;j<n;j++)
00542         {
00543           v[j][i] = u[i][j]/h;
00544         }
00545         for (j=l;j<n;j++)
00546         {
00547           s = 0.0;
00548           for (k=l;k<n;k++)
00549           {
00550             s += (u[i][k] * v[k][j]);
00551           }
00552           for (k=l;k<n;k++)
00553           {
00554             v[k][j] += (s * v[k][i]);
00555           }
00556         } /* end j */
00557       } /* end g */
00558       for (j=l;j<n;j++)
00559       {
00560         v[i][j] = v[j][i] = 0.0;
00561       }
00562       v[i][i] = 1.0;
00563       g = e[i];
00564       l = i;
00565     } /* end i */
00566   } /* end withv, parens added for clarity */
00567 
00568 /* accumulation of left-hand transformations */
00569   if (withu) {
00570     for (i=n-1;i>=0;i--) {
00571       l = i + 1;
00572       g = q[i];
00573       for (j=l;j<n;j++)  /* upper limit was 'n' */
00574         u[i][j] = 0.0;
00575       if (g != 0.0) {
00576         h = u[i][i] * g;
00577         for (j=l;j<n;j++) { /* upper limit was 'n' */
00578           s = 0.0;
00579           for (k=l;k<m;k++)
00580             s += (u[k][i] * u[k][j]);
00581           f = s / h;
00582           for (k=i;k<m;k++)
00583             u[k][j] += (f * u[k][i]);
00584         } /* end j */
00585         for (j=i;j<m;j++)
00586           u[j][i] /= g;
00587       } /* end g */
00588       else {
00589         for (j=i;j<m;j++)
00590           u[j][i] = 0.0;
00591       }
00592       u[i][i] += 1.0;
00593     } /* end i*/
00594   } /* end withu, parens added for clarity */
00595 
00596 /* diagonalization of the bidiagonal form */
00597   eps *= x;
00598   for (k=n-1;k>=0;k--) {
00599     iter = 0;
00600 test_f_splitting:
00601     for (l=k;l>=0;l--) {
00602       if (fabs(e[l]) <= eps) goto test_f_convergence;
00603       if (fabs(q[l-1]) <= eps) goto cancellation;
00604     } /* end l */
00605 
00606 /* cancellation of e[l] if l > 0 */
00607 cancellation:
00608     c = 0.0;
00609     s = 1.0;
00610     l1 = l - 1;
00611     for (i=l;i<=k;i++) {
00612       f = s * e[i];
00613       e[i] *= c;
00614       if (fabs(f) <= eps) goto test_f_convergence;
00615       g = q[i];
00616       h = q[i] = sqrt(f*f + g*g);
00617       c = g / h;
00618       s = -f / h;
00619       if (withu) {
00620         for (j=0;j<m;j++) {
00621           y = u[j][l1];
00622           z = u[j][i];
00623           u[j][l1] = y * c + z * s;
00624           u[j][i] = -y * s + z * c;
00625         } /* end j */
00626       } /* end withu, parens added for clarity */
00627     } /* end i */
00628 test_f_convergence:
00629     z = q[k];
00630     if (l == k) goto convergence;
00631 
00632 /* shift from bottom 2x2 minor */
00633     iter++;
00634     if (iter > 30) {
00635       retval = k;
00636       break;
00637     }
00638     x = q[l];
00639     y = q[k-1];
00640     g = e[k-1];
00641     h = e[k];
00642     f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2*h*y);
00643     g = sqrt(f*f + 1.0);
00644     f = ((x-z)*(x+z) + h*(y/((f<0)?(f-g):(f+g))-h))/x;
00645 /* next QR transformation */
00646     c = s = 1.0;
00647     for (i=l+1;i<=k;i++) {
00648       g = e[i];
00649       y = q[i];
00650       h = s * g;
00651       g *= c;
00652       e[i-1] = z = sqrt(f*f+h*h);
00653       c = f / z;
00654       s = h / z;
00655       f = x * c + g * s;
00656       g = -x * s + g * c;
00657       h = y * s;
00658       y *= c;
00659       if (withv) {
00660         for (j=0;j<n;j++) {
00661           x = v[j][i-1];
00662           z = v[j][i];
00663           v[j][i-1] = x * c + z * s;
00664           v[j][i] = -x * s + z * c;
00665         } /* end j */
00666       } /* end withv, parens added for clarity */
00667       q[i-1] = z = sqrt(f*f + h*h);
00668       c = f/z;
00669       s = h/z;
00670       f = c * g + s * y;
00671       x = -s * g + c * y;
00672       if (withu) {
00673         for (j=0;j<m;j++) {
00674           y = u[j][i-1];
00675           z = u[j][i];
00676           u[j][i-1] = y * c + z * s;
00677           u[j][i] = -y * s + z * c;
00678         } /* end j */
00679       } /* end withu, parens added for clarity */
00680     } /* end i */
00681     e[l] = 0.0;
00682     e[k] = f;
00683     q[k] = x;
00684     goto test_f_splitting;
00685 convergence:
00686     if (z < 0.0) {
00687 /* q[k] is made non-negative */
00688       q[k] = - z;
00689       if (withv) {
00690         for (j=0;j<n;j++)
00691           v[j][k] = -v[j][k];
00692       } /* end withv, parens added for clarity */
00693     } /* end z */
00694   } /* end k */
00695 
00696   free(e);
00697 
00698   return retval;
00699 }