ADMB Documentation  11.1x.2711
 All Classes Files Functions Variables Typedefs Friends Defines
fvar_m24.cpp
Go to the documentation of this file.
00001 
00006 #include <fvar.hpp>
00007 
00008 #ifdef __TURBOC__
00009   #pragma hdrstop
00010   #include <iostream.h>
00011 #endif
00012 
00013 #if defined (__WAT32__)
00014   #include <iostream.h>
00015   #include <strstrea.h>
00016 #endif
00017 
00018 #ifdef __ZTC__
00019   #include <iostream.hpp>
00020 #endif
00021 
00022 #define TINY 1.0e-20;
00023 void dmdv_solve(void);
00024 
00025 dvar_vector solve(const dvar_matrix& aa, const dvar_vector& z,
00026   prevariable& ln_unsigned_det, const prevariable& sign);
00033 dvar_vector solve(const dvar_matrix& aa, const dvar_vector& z)
00034 {
00035   dvariable ln_unsigned_det;
00036   dvariable sign;
00037   dvar_vector sol=solve(aa,z,ln_unsigned_det,sign);
00038   return sol;
00039 }
00040 
00052 dvar_vector solve(const dvar_matrix& aa, const dvar_vector& z,
00053   prevariable& ln_unsigned_det, const prevariable& _sign)
00054 {
00055   prevariable& sign=(prevariable&) _sign;
00056 
00057   RETURN_ARRAYS_INCREMENT();
00058   int n=aa.colsize();
00059   int lb=aa.colmin();
00060   int ub=aa.colmax();
00061   if (lb!=aa.rowmin()||ub!=aa.colmax())
00062   {
00063     cerr << "Error matrix not square in solve()"<<endl;
00064     ad_exit(1);
00065   }
00066   ivector indx(lb,ub);
00067   int One=1;
00068   indx.fill_seqadd(lb,One);
00069   double d;
00070   double big,dum,sum,temp;
00071   dvar_matrix_position dmp(aa,0);
00072   dmatrix bb=value(aa);
00073   kkludge_object kkk;
00074   dvar_vector vc(lb,ub,kkk);
00075   dvector vv(lb,ub);
00076 
00077   d=1.0;
00078   for (int i=lb;i<=ub;i++)
00079   {
00080     big=0.0;
00081     for (int j=lb;j<=ub;j++)
00082     {
00083       temp=fabs(bb.elem(i,j));
00084       if (temp > big)
00085       {
00086         big=temp;
00087       }
00088     }
00089     if (big == 0.0)
00090     {
00091       cerr << "Error in matrix inverse -- matrix singular in "
00092       "solve(dvar_dmatrix)\n";
00093     }
00094     vv[i]=1.0/big;
00095   }
00096 
00097   for (int j=lb;j<=ub;j++)
00098   {
00099     for (int i=lb;i<j;i++)
00100     {
00101       sum=bb.elem(i,j);
00102       for (int k=lb;k<i;k++)
00103       {
00104         sum -= bb.elem(i,k)*bb.elem(k,j);
00105       }
00106       //a[i][j]=sum;
00107       bb.elem(i,j)=sum;
00108     }
00109     int imax = j;
00110     big=0.0;
00111     for (int i=j;i<=ub;i++)
00112     {
00113       sum=bb.elem(i,j);
00114       for (int k=lb;k<j;k++)
00115       {
00116         sum -= bb.elem(i,k)*bb.elem(k,j);
00117       }
00118       bb.elem(i,j)=sum;
00119       dum=vv[i]*fabs(sum);
00120       if ( dum >= big)
00121       {
00122         big=dum;
00123         imax=i;
00124       }
00125     }
00126     if (j != imax)
00127     {
00128       for (int k=lb;k<=ub;k++)
00129       {
00130         dum=bb.elem(imax,k);
00131         bb.elem(imax,k)=bb.elem(j,k);
00132         bb.elem(j,k)=dum;
00133       }
00134       d = -1.*d;
00135       vv[imax]=vv[j];
00136 
00137       //if (j<ub)
00138       {
00139         int itemp=indx.elem(imax);
00140         indx.elem(imax)=indx.elem(j);
00141         indx.elem(j)=itemp;
00142       }
00143       //cout << "indx= " <<indx<<endl;
00144     }
00145 
00146     if (bb.elem(j,j) == 0.0)
00147     {
00148       bb.elem(j,j)=TINY;
00149     }
00150 
00151     if (j != n)
00152     {
00153       dum=1.0/bb.elem(j,j);
00154       for (int i=j+1;i<=ub;i++)
00155       {
00156         bb.elem(i,j) = bb.elem(i,j) * dum;
00157       }
00158     }
00159   }
00160 
00161   // get the determinant
00162   sign=d;
00163   dvector part_prod(lb,ub);
00164   part_prod(lb)=log(fabs(bb(lb,lb)));
00165   if (bb(lb,lb)<0) sign=-sign;
00166   for (int j=lb+1;j<=ub;j++)
00167   {
00168     if (bb(j,j)<0) sign=-sign;
00169     part_prod(j)=part_prod(j-1)+log(fabs(bb(j,j)));
00170   }
00171   ln_unsigned_det=part_prod(ub);
00172 
00173   dvector x(lb,ub);
00174   dvector y(lb,ub);
00175   //int lb=rowmin;
00176   //int ub=rowmax;
00177   dmatrix& b=bb;
00178   ivector indxinv(lb,ub);
00179   for (int i=lb;i<=ub;i++)
00180   {
00181     indxinv(indx.elem(i))=i;
00182   }
00183 
00184   for (int i=lb;i<=ub;i++)
00185   {
00186     y.elem(indxinv(i))=z.elem_value(i);
00187   }
00188 
00189   for (int i=lb;i<=ub;i++)
00190   {
00191     sum=y.elem(i);
00192     for (int j=lb;j<=i-1;j++)
00193     {
00194       sum-=b.elem(i,j)*y.elem(j);
00195     }
00196     y.elem(i)=sum;
00197   }
00198   for (int i=ub;i>=lb;i--)
00199   {
00200     sum=y.elem(i);
00201     for (int j=i+1;j<=ub;j++)
00202     {
00203       sum-=b.elem(i,j)*x.elem(j);
00204     }
00205     x.elem(i)=sum/b.elem(i,i);
00206   }
00207 
00208   vc=nograd_assign(x);
00209   save_identifier_string("PLACE8");
00210   ln_unsigned_det.save_prevariable_position();
00211   save_identifier_string("PLACE7");
00212   part_prod.save_dvector_value();
00213   part_prod.save_dvector_position();
00214   save_identifier_string("PLACE6");
00215   y.save_dvector_value();
00216   x.save_dvector_value();
00217   save_identifier_string("PLACE5");
00218   x.save_dvector_position();
00219   save_identifier_string("PLACE4");
00220   y.save_dvector_position();
00221   indx.save_ivector_value();
00222   save_identifier_string("PLACE3a");
00223   indx.save_ivector_position();
00224   save_identifier_string("PLACE3");
00225   aa.save_dvar_matrix_position();
00226   save_identifier_string("PLACE2b");
00227   vc.save_dvar_vector_position();
00228   save_identifier_string("PLACE2a");
00229   bb.save_dmatrix_value();
00230   save_identifier_string("PLACE2");
00231   bb.save_dmatrix_position();
00232   save_identifier_string("PLACE1");
00233   z.save_dvar_vector_position();
00234   save_identifier_string("PLACE0");
00235   gradient_structure::GRAD_STACK1->
00236      set_gradient_stack(dmdv_solve);
00237   RETURN_ARRAYS_DECREMENT();
00238   return vc;
00239 }
00240 
00243 void dmdv_solve(void)
00244 {
00245   verify_identifier_string("PLACE0");
00246   dvar_vector_position zpos=restore_dvar_vector_position();
00247   verify_identifier_string("PLACE1");
00248   dmatrix_position bpos=restore_dmatrix_position();
00249   verify_identifier_string("PLACE2");
00250   dmatrix b=restore_dmatrix_value(bpos);
00251   verify_identifier_string("PLACE2a");
00252   dvar_vector_position v_pos=restore_dvar_vector_position();
00253   verify_identifier_string("PLACE2b");
00254   dvar_matrix_position a_pos=restore_dvar_matrix_position();
00255   verify_identifier_string("PLACE3");
00256   ivector_position indx_pos=restore_ivector_position();
00257   verify_identifier_string("PLACE3a");
00258   ivector indx=restore_ivector_value(indx_pos);
00259   dvector_position y_pos=restore_dvector_position();
00260   verify_identifier_string("PLACE4");
00261   dvector_position x_pos=restore_dvector_position();
00262   verify_identifier_string("PLACE5");
00263   dvector x=restore_dvector_value(x_pos);
00264   dvector y=restore_dvector_value(y_pos);
00265   verify_identifier_string("PLACE6");
00266   dvector_position part_prod_pos=restore_dvector_position();
00267   dvector part_prod=restore_dvector_value(part_prod_pos);
00268   verify_identifier_string("PLACE7");
00269   double df_ln_det=restore_prevariable_derivative();
00270   verify_identifier_string("PLACE8");
00271   int lb=b.colmin();
00272   int ub=b.colmax();
00273   dmatrix dfb(lb,ub,lb,ub);
00274   dvector dfz(lb,ub);
00275   dvector dfx=restore_dvar_vector_derivatives(v_pos);
00276   dvector dfy(lb,ub);
00277   dvector dfpart_prod(lb,ub);
00278   ivector indxinv(lb,ub);
00279   for (int i=lb;i<=ub;i++)
00280   {
00281     indxinv(indx.elem(i))=i;
00282   }
00283 
00284   double dfsum=0.;
00285   #ifndef SAFE_INITIALIZE
00286     dfb.initialize();
00287     dfy.initialize();
00288     dfz.initialize();
00289     dfpart_prod.initialize();
00290   #endif
00291 
00292   for (int i=lb;i<=ub;i++)
00293   {
00294     // x.elem(i)=sum/b.elem(i,i);
00295     dfsum+=dfx.elem(i)/b.elem(i,i);
00296     dfb.elem(i,i)-=dfx.elem(i)*x.elem(i)/b.elem(i,i);
00297     dfx.elem(i)=0.;
00298     for (int j=ub;j>=i+1;j--)
00299     {
00300       // sum -=b.elem(i,j)*x.elem(j);
00301       dfb.elem(i,j)-=dfsum*x.elem(j);
00302       dfx.elem(j)-=dfsum*b.elem(i,j);
00303     }
00304     // sum=y.elem(i);
00305     dfy.elem(i)+=dfsum;
00306     dfsum=0.;
00307   }
00308 
00309   for (int i=ub;i>=lb;i--)
00310   {
00311     // y.elem(i)=sum;
00312     dfsum+=dfy.elem(i);
00313     dfy.elem(i)=0.;
00314     for (int j=i-1;j>=lb;j--)
00315     {
00316       // sum-=b.elem(i,j)*y.elem(j);
00317       dfb.elem(i,j)-=dfsum*y.elem(j);
00318       dfy.elem(j)-=dfsum*b.elem(i,j);
00319     }
00320     //sum=y.elem(i);
00321     dfy.elem(i)=dfsum;
00322     dfsum=0.;
00323   }
00324 
00325   for (int i=ub;i>=lb;i--)
00326   {
00327     //y.elem(indxinv(i))=z.elem_value(i);
00328     dfz.elem(i)=dfy.elem(indxinv(i));
00329   }
00330 
00331   dfz.save_dvector_derivatives(zpos);
00332 
00333   //ln_unsigned_det=part_prod(ub);
00334   dfpart_prod(ub)+=df_ln_det;
00335   df_ln_det=0.0;
00336 
00337   for (int j=ub;j>=lb+1;j--)
00338   {
00339     //part_prod(j)=part_prod(j-1)+log(fabs(bb(j,j));
00340     dfpart_prod(j-1)+=dfpart_prod(j);
00341     dfb(j,j)+=dfpart_prod(j)/b(j,j);
00342     dfpart_prod(j)=0.0;
00343   }
00344 
00345   //part_prod(lb)=log(fabs(bb(lb,lb));
00346   dfb(lb,lb)+=dfpart_prod(lb)/b(lb,lb);
00347   dfpart_prod(lb)=0.0;
00348 
00349   for (int j=ub;j>=lb;j--)
00350   {
00351     for (int i=ub;i>=lb;i--)
00352     {
00353       if (i<=j)
00354       {
00355         // b.elem(i,j)=sum;
00356         dfsum+=dfb.elem(i,j);
00357         dfb.elem(i,j)=0.;
00358       }
00359       else
00360       {
00361         // b.elem(i,j)=sum/b.elem(j,j);
00362         dfsum+=dfb.elem(i,j)/b.elem(j,j);
00363         dfb.elem(j,j)-=dfb.elem(i,j)*b.elem(i,j)/b.elem(j,j);
00364         dfb.elem(i,j)=0.;
00365       }
00366 
00367       for (int k=min(i-1,j-1);k>=lb;k--)
00368       {
00369         // sum-=b.elem(i,k)*b.elem(k,j);
00370         dfb.elem(i,k)-=dfsum*b.elem(k,j);
00371         dfb.elem(k,j)-=dfsum*b.elem(i,k);
00372       }
00373       // sum=value(a(indx.elem(i),j);
00374       save_dmatrix_derivatives(a_pos,dfsum,indx.elem(i),j); // like this
00375       dfsum=0.;
00376     }
00377   }
00378 }
00379 #undef TINY