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