ADMB Documentation  11.1.2192
 All Classes Files Functions Variables Typedefs Friends Defines
dmat42.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: dmat42.cpp 2185 2014-07-18 00:40:55Z 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     for (i=n-1;i>=0;i--)
00264     {
00265       if ( i < n-2 )
00266       {
00267         if (g != 0.0)
00268         {
00269           h = u[i][i+1] * g;
00270           for (j=l;j<n;j++)
00271           {
00272             v[j][i] = u[i][j]/h;
00273           }
00274           for (j=l;j<n;j++)
00275           {
00276             s = 0.0;
00277             for (k=l;k<n;k++)
00278             {
00279               s += (u[i][k] * v[k][j]);
00280             }
00281             for (k=l;k<n;k++)
00282             {
00283               v[k][j] += (s * v[k][i]);
00284             }
00285           } /* end j */
00286         } /* end g */
00287         for (j=l;j<n;j++)
00288         {
00289           v[i][j] = v[j][i] = 0.0;
00290         }
00291       }
00292       v[i][i] = 1.0;
00293       g = e[i];
00294       l = i;
00295     } /* end i */
00296   } /* end withv, parens added for clarity */
00297 
00298 /* accumulation of left-hand transformations */
00299   if (withu) {
00300     for (i=min(m,n)-1;i>=0;i--) {
00301       l = i + 1;
00302       g = q[i];
00303       for (j=l;j<n;j++)  /* upper limit was 'n' */
00304         u[i][j] = 0.0;
00305       if (g != 0.0) {
00306         h = u[i][i] * g;
00307         for (j=l;j<n;j++) { /* upper limit was 'n' */
00308           s = 0.0;
00309           for (k=l;k<m;k++)
00310             s += (u[k][i] * u[k][j]);
00311           f = s / h;
00312           for (k=i;k<m;k++)
00313             u[k][j] += (f * u[k][i]);
00314         } /* end j */
00315         for (j=i;j<m;j++)
00316           u[j][i] /= g;
00317       } /* end g */
00318       else {
00319         for (j=i;j<m;j++)
00320           u[j][i] = 0.0;
00321       }
00322       u[i][i] += 1.0;
00323     } /* end i*/
00324   } /* end withu, parens added for clarity */
00325 
00326 /* diagonalization of the bidiagonal form */
00327   eps *= x;
00328   for (k=n-1;k>=0;k--) {
00329     iter = 0;
00330 test_f_splitting:
00331     for (l=k;l>=0;l--) {
00332       if (fabs(e[l]) <= eps) goto test_f_convergence;
00333       if (fabs(q[l-1]) <= eps) goto cancellation;
00334     } /* end l */
00335 
00336 /* cancellation of e[l] if l > 0 */
00337 cancellation:
00338     c = 0.0;
00339     s = 1.0;
00340     l1 = l - 1;
00341     for (i=l;i<=k;i++) {
00342       f = s * e[i];
00343       e[i] *= c;
00344       if (fabs(f) <= eps) goto test_f_convergence;
00345       g = q[i];
00346       h = q[i] = sqrt(f*f + g*g);
00347       c = g / h;
00348       s = -f / h;
00349       if (withu) {
00350         for (j=0;j<m;j++) {
00351           y = u[j][l1];
00352           z = u[j][i];
00353           u[j][l1] = y * c + z * s;
00354           u[j][i] = -y * s + z * c;
00355         } /* end j */
00356       } /* end withu, parens added for clarity */
00357     } /* end i */
00358 test_f_convergence:
00359     z = q[k];
00360     if (l == k) goto convergence;
00361 
00362 /* shift from bottom 2x2 minor */
00363     iter++;
00364     if (iter > 30) {
00365       retval = k;
00366       break;
00367     }
00368     x = q[l];
00369     y = q[k-1];
00370     g = e[k-1];
00371     h = e[k];
00372     f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2*h*y);
00373     g = sqrt(f*f + 1.0);
00374     f = ((x-z)*(x+z) + h*(y/((f<0)?(f-g):(f+g))-h))/x;
00375 /* next QR transformation */
00376     c = s = 1.0;
00377     for (i=l+1;i<=k;i++) {
00378       g = e[i];
00379       y = q[i];
00380       h = s * g;
00381       g *= c;
00382       e[i-1] = z = sqrt(f*f+h*h);
00383       c = f / z;
00384       s = h / z;
00385       f = x * c + g * s;
00386       g = -x * s + g * c;
00387       h = y * s;
00388       y *= c;
00389       if (withv) {
00390         for (j=0;j<n;j++) {
00391           x = v[j][i-1];
00392           z = v[j][i];
00393           v[j][i-1] = x * c + z * s;
00394           v[j][i] = -x * s + z * c;
00395         } /* end j */
00396       } /* end withv, parens added for clarity */
00397       q[i-1] = z = sqrt(f*f + h*h);
00398       c = f/z;
00399       s = h/z;
00400       f = c * g + s * y;
00401       x = -s * g + c * y;
00402       if (withu) {
00403         for (j=0;j<m;j++) {
00404           y = u[j][i-1];
00405           z = u[j][i];
00406           u[j][i-1] = y * c + z * s;
00407           u[j][i] = -y * s + z * c;
00408         } /* end j */
00409       } /* end withu, parens added for clarity */
00410     } /* end i */
00411     e[l] = 0.0;
00412     e[k] = f;
00413     q[k] = x;
00414     goto test_f_splitting;
00415 convergence:
00416     if (z < 0.0) {
00417 /* q[k] is made non-negative */
00418       q[k] = - z;
00419       if (withv) {
00420         for (j=0;j<n;j++)
00421           v[j][k] = -v[j][k];
00422       } /* end withv, parens added for clarity */
00423     } /* end z */
00424   } /* end k */
00425 
00426   free(e);
00427 
00428   return retval;
00429 }
00430 
00442 int svd_nlm(int m, int n, int withu, int withv, double eps, double tol,
00443             const dmatrix& aa, const dvector& _q,
00444             const dmatrix& _u, const dmatrix& _v)
00445 {
00446   ADUNCONST(dmatrix,u)
00447   ADUNCONST(dmatrix,v)
00448   ADUNCONST(dvector,q)
00449 
00450   int i,j,k,l,l1,iter,retval;
00451   double c,f,g,h,s,x,y,z;
00452   double *e;
00453 
00454   e = (double *)calloc(n,sizeof(double));
00455   retval = 0;
00456 
00457   u=aa;
00458 /* Householder's reduction to bidiagonal form. */
00459   g = x = 0.0;
00460   for (i=0;i<n;i++)
00461   {
00462     e[i] = g;
00463     s = 0.0;
00464     l = i+1;
00465     for (j=i;j<m;j++)
00466     {
00467       s += (u[j][i]*u[j][i]);
00468     }
00469     if (s < tol)
00470       g = 0.0;
00471     else
00472     {
00473       f = u[i][i];
00474       g = (f < 0) ? sqrt(s) : -sqrt(s);
00475       h = f * g - s;
00476       u[i][i] = f - g;
00477       for (j=l;j<n;j++)
00478       {
00479         s = 0.0;
00480         for (k=i;k<m;k++)
00481         {
00482           s += (u[k][i] * u[k][j]);
00483         }
00484         f = s / h;
00485         for (k=i;k<m;k++)
00486         {
00487           u[k][j] += (f * u[k][i]);
00488         }
00489       } /* end j */
00490     } /* end s */
00491     q[i] = g;
00492     s = 0.0;
00493     for (j=l;j<n;j++)
00494     {
00495       s += (u[i][j] * u[i][j]);
00496     }
00497     if (s < tol)
00498       g = 0.0;
00499     else
00500     {
00501       f = u[i][i+1];
00502       g = (f < 0) ? sqrt(s) : -sqrt(s);
00503       h = f * g - s;
00504       u[i][i+1] = f - g;
00505       for (j=l;j<n;j++)
00506       {
00507         e[j] = u[i][j]/h;
00508       }
00509       for (j=l;j<m;j++)
00510       {
00511         s = 0.0;
00512         for (k=l;k<n;k++)
00513         {
00514           s += (u[j][k] * u[i][k]);
00515         }
00516         for (k=l;k<n;k++)
00517         {
00518           u[j][k] += (s * e[k]);
00519         }
00520       } /* end j */
00521     } /* end s */
00522     y = fabs(q[i]) + fabs(e[i]);
00523     if (y > x)
00524     {
00525       x = y;
00526     }
00527   } /* end i */
00528 
00529 /* accumulation of right-hand transformations */
00530   if (withv)
00531   {
00532     for (i=n-1;i>=0;i--)
00533     {
00534       if (g != 0.0)
00535       {
00536         h = u[i][i+1] * g;
00537         for (j=l;j<n;j++)
00538         {
00539           v[j][i] = u[i][j]/h;
00540         }
00541         for (j=l;j<n;j++)
00542         {
00543           s = 0.0;
00544           for (k=l;k<n;k++)
00545           {
00546             s += (u[i][k] * v[k][j]);
00547           }
00548           for (k=l;k<n;k++)
00549           {
00550             v[k][j] += (s * v[k][i]);
00551           }
00552         } /* end j */
00553       } /* end g */
00554       for (j=l;j<n;j++)
00555       {
00556         v[i][j] = v[j][i] = 0.0;
00557       }
00558       v[i][i] = 1.0;
00559       g = e[i];
00560       l = i;
00561     } /* end i */
00562   } /* end withv, parens added for clarity */
00563 
00564 /* accumulation of left-hand transformations */
00565   if (withu) {
00566     for (i=n-1;i>=0;i--) {
00567       l = i + 1;
00568       g = q[i];
00569       for (j=l;j<n;j++)  /* upper limit was 'n' */
00570         u[i][j] = 0.0;
00571       if (g != 0.0) {
00572         h = u[i][i] * g;
00573         for (j=l;j<n;j++) { /* upper limit was 'n' */
00574           s = 0.0;
00575           for (k=l;k<m;k++)
00576             s += (u[k][i] * u[k][j]);
00577           f = s / h;
00578           for (k=i;k<m;k++)
00579             u[k][j] += (f * u[k][i]);
00580         } /* end j */
00581         for (j=i;j<m;j++)
00582           u[j][i] /= g;
00583       } /* end g */
00584       else {
00585         for (j=i;j<m;j++)
00586           u[j][i] = 0.0;
00587       }
00588       u[i][i] += 1.0;
00589     } /* end i*/
00590   } /* end withu, parens added for clarity */
00591 
00592 /* diagonalization of the bidiagonal form */
00593   eps *= x;
00594   for (k=n-1;k>=0;k--) {
00595     iter = 0;
00596 test_f_splitting:
00597     for (l=k;l>=0;l--) {
00598       if (fabs(e[l]) <= eps) goto test_f_convergence;
00599       if (fabs(q[l-1]) <= eps) goto cancellation;
00600     } /* end l */
00601 
00602 /* cancellation of e[l] if l > 0 */
00603 cancellation:
00604     c = 0.0;
00605     s = 1.0;
00606     l1 = l - 1;
00607     for (i=l;i<=k;i++) {
00608       f = s * e[i];
00609       e[i] *= c;
00610       if (fabs(f) <= eps) goto test_f_convergence;
00611       g = q[i];
00612       h = q[i] = sqrt(f*f + g*g);
00613       c = g / h;
00614       s = -f / h;
00615       if (withu) {
00616         for (j=0;j<m;j++) {
00617           y = u[j][l1];
00618           z = u[j][i];
00619           u[j][l1] = y * c + z * s;
00620           u[j][i] = -y * s + z * c;
00621         } /* end j */
00622       } /* end withu, parens added for clarity */
00623     } /* end i */
00624 test_f_convergence:
00625     z = q[k];
00626     if (l == k) goto convergence;
00627 
00628 /* shift from bottom 2x2 minor */
00629     iter++;
00630     if (iter > 30) {
00631       retval = k;
00632       break;
00633     }
00634     x = q[l];
00635     y = q[k-1];
00636     g = e[k-1];
00637     h = e[k];
00638     f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2*h*y);
00639     g = sqrt(f*f + 1.0);
00640     f = ((x-z)*(x+z) + h*(y/((f<0)?(f-g):(f+g))-h))/x;
00641 /* next QR transformation */
00642     c = s = 1.0;
00643     for (i=l+1;i<=k;i++) {
00644       g = e[i];
00645       y = q[i];
00646       h = s * g;
00647       g *= c;
00648       e[i-1] = z = sqrt(f*f+h*h);
00649       c = f / z;
00650       s = h / z;
00651       f = x * c + g * s;
00652       g = -x * s + g * c;
00653       h = y * s;
00654       y *= c;
00655       if (withv) {
00656         for (j=0;j<n;j++) {
00657           x = v[j][i-1];
00658           z = v[j][i];
00659           v[j][i-1] = x * c + z * s;
00660           v[j][i] = -x * s + z * c;
00661         } /* end j */
00662       } /* end withv, parens added for clarity */
00663       q[i-1] = z = sqrt(f*f + h*h);
00664       c = f/z;
00665       s = h/z;
00666       f = c * g + s * y;
00667       x = -s * g + c * y;
00668       if (withu) {
00669         for (j=0;j<m;j++) {
00670           y = u[j][i-1];
00671           z = u[j][i];
00672           u[j][i-1] = y * c + z * s;
00673           u[j][i] = -y * s + z * c;
00674         } /* end j */
00675       } /* end withu, parens added for clarity */
00676     } /* end i */
00677     e[l] = 0.0;
00678     e[k] = f;
00679     q[k] = x;
00680     goto test_f_splitting;
00681 convergence:
00682     if (z < 0.0) {
00683 /* q[k] is made non-negative */
00684       q[k] = - z;
00685       if (withv) {
00686         for (j=0;j<n;j++)
00687           v[j][k] = -v[j][k];
00688       } /* end withv, parens added for clarity */
00689     } /* end z */
00690   } /* end k */
00691 
00692   free(e);
00693 
00694   return retval;
00695 }