ADMB Documentation  11.1.2192
 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,imax,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     big=0.0;
00111     for (i=j;i<=ub;i++)
00112     {
00113       sum=bb.elem(i,j);
00114       for (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 (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 (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 (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 (i=lb;i<=ub;i++)
00180   {
00181     indxinv(indx.elem(i))=i;
00182   }
00183 
00184   for (i=lb;i<=ub;i++)
00185   {
00186     y.elem(indxinv(i))=z.elem_value(i);
00187   }
00188 
00189   for (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 (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   int i;
00280   for (i=lb;i<=ub;i++)
00281   {
00282     indxinv(indx.elem(i))=i;
00283   }
00284 
00285   double dfsum=0.;
00286   #ifndef SAFE_INITIALIZE
00287     dfb.initialize();
00288     dfy.initialize();
00289     dfz.initialize();
00290     dfpart_prod.initialize();
00291   #endif
00292 
00293   for (i=lb;i<=ub;i++)
00294   {
00295     // x.elem(i)=sum/b.elem(i,i);
00296     dfsum+=dfx.elem(i)/b.elem(i,i);
00297     dfb.elem(i,i)-=dfx.elem(i)*x.elem(i)/b.elem(i,i);
00298     dfx.elem(i)=0.;
00299     for (int j=ub;j>=i+1;j--)
00300     {
00301       // sum -=b.elem(i,j)*x.elem(j);
00302       dfb.elem(i,j)-=dfsum*x.elem(j);
00303       dfx.elem(j)-=dfsum*b.elem(i,j);
00304     }
00305     // sum=y.elem(i);
00306     dfy.elem(i)+=dfsum;
00307     dfsum=0.;
00308   }
00309 
00310   for (i=ub;i>=lb;i--)
00311   {
00312     // y.elem(i)=sum;
00313     dfsum+=dfy.elem(i);
00314     dfy.elem(i)=0.;
00315     for (int j=i-1;j>=lb;j--)
00316     {
00317       // sum-=b.elem(i,j)*y.elem(j);
00318       dfb.elem(i,j)-=dfsum*y.elem(j);
00319       dfy.elem(j)-=dfsum*b.elem(i,j);
00320     }
00321     //sum=y.elem(i);
00322     dfy.elem(i)=dfsum;
00323     dfsum=0.;
00324   }
00325 
00326   for (i=ub;i>=lb;i--)
00327   {
00328     //y.elem(indxinv(i))=z.elem_value(i);
00329     dfz.elem(i)=dfy.elem(indxinv(i));
00330   }
00331 
00332   dfz.save_dvector_derivatives(zpos);
00333 
00334   //ln_unsigned_det=part_prod(ub);
00335   dfpart_prod(ub)+=df_ln_det;
00336   df_ln_det=0.0;
00337 
00338   int j;
00339   for (j=ub;j>=lb+1;j--)
00340   {
00341     //part_prod(j)=part_prod(j-1)+log(fabs(bb(j,j));
00342     dfpart_prod(j-1)+=dfpart_prod(j);
00343     dfb(j,j)+=dfpart_prod(j)/b(j,j);
00344     dfpart_prod(j)=0.0;
00345   }
00346 
00347   //part_prod(lb)=log(fabs(bb(lb,lb));
00348   dfb(lb,lb)+=dfpart_prod(lb)/b(lb,lb);
00349   dfpart_prod(lb)=0.0;
00350 
00351   for (j=ub;j>=lb;j--)
00352   {
00353     for (int i=ub;i>=lb;i--)
00354     {
00355       if (i<=j)
00356       {
00357         // b.elem(i,j)=sum;
00358         dfsum+=dfb.elem(i,j);
00359         dfb.elem(i,j)=0.;
00360       }
00361       else
00362       {
00363         // b.elem(i,j)=sum/b.elem(j,j);
00364         dfsum+=dfb.elem(i,j)/b.elem(j,j);
00365         dfb.elem(j,j)-=dfb.elem(i,j)*b.elem(i,j)/b.elem(j,j);
00366         dfb.elem(i,j)=0.;
00367       }
00368 
00369       for (int k=min(i-1,j-1);k>=lb;k--)
00370       {
00371         // sum-=b.elem(i,k)*b.elem(k,j);
00372         dfb.elem(i,k)-=dfsum*b.elem(k,j);
00373         dfb.elem(k,j)-=dfsum*b.elem(i,k);
00374       }
00375       // sum=value(a(indx.elem(i),j);
00376       save_dmatrix_derivatives(a_pos,dfsum,indx.elem(i),j); // like this
00377       dfsum=0.;
00378     }
00379   }
00380 }
00381 #undef TINY