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