00001
00002
00003
00004
00005
00006
00011 #include <fvar.hpp>
00012 #define TINY 1.0e-20;
00013
00018 dmatrix fabs(const dmatrix & X){
00019 int rmin = X.rowmin();
00020 int rmax = X.rowmax();
00021 int cmin = X.colmin();
00022 int cmax = X.colmax();
00023 dmatrix ret(rmin,rmax,cmin,cmax);
00024 for(int i = rmin; i<=rmax; ++i){
00025 for(int j = cmin; j<=cmax; ++j){
00026 ret(i,j) = fabs(X(i,j));
00027 }
00028 }
00029 return ret;
00030 }
00031
00032 dvar_matrix solve(const dvar_matrix& aa, const dvar_matrix& tz,
00033 dvariable ln_unsigned_det, dvariable& sign);
00034
00035 dvar_matrix solve(const dvar_matrix& aa, const dvar_matrix& tz)
00036 {
00037 dvariable ln;
00038 dvariable sgn;
00039 return solve(aa,tz,ln,sgn);
00040 }
00041
00063 dmatrix expm(const dmatrix& A)
00064 {
00065 int rmin = A.rowmin();
00066 int rmax = A.rowmax();
00067 if(rmax != A.colmax())
00068 {
00069 cout<<"Error: Not square matrix in expm."<<endl;
00070 ad_exit(1);
00071 }
00072 if(rmin != A.colmin())
00073 {
00074 cout<<"Error: Not square matrix in expm."<<endl;
00075 ad_exit(1);
00076 }
00077 dmatrix I(rmin,rmax,rmin,rmax);
00078 dmatrix AA(rmin,rmax,rmin,rmax);
00079 dmatrix X(rmin,rmax,rmin,rmax);
00080 dmatrix E(rmin,rmax,rmin,rmax);
00081 dmatrix D(rmin,rmax,rmin,rmax);
00082 dmatrix cX(rmin,rmax,rmin,rmax);
00083 I.initialize();
00084 for(int i = rmin; i<=rmax; ++i){I(i,i) = 1.0;}
00085 double log2NormInf;
00086 log2NormInf = log(max(rowsum(fabs(A))));
00087 log2NormInf/=log(2.0);
00088 int e = (int)log2NormInf + 1;
00089 int s = e+1;
00090 s = (s<0) ? 0 : s;
00091 AA = 1.0/pow(2.0,s)*A;
00092 X = AA;
00093 double c = 0.5;
00094
00095 E = I+c*AA;
00096 D = I-c*AA;
00097 int q = 6, p = 1;
00098 for(int k = 2; k<=q; ++k){
00099 c*=((double)q-k+1.0)/((double)k*(2*q-k+1));
00100 X = AA*X;
00101 cX = c*X;
00102 E+=cX;
00103 if(p==1){D+=cX;}else{D-=cX;}
00104 p = (p==1) ? 0 : 1;
00105 }
00106
00107 E = solve(D,E);
00108 for(int k = 1; k<=s; ++k){
00109 E = E*E;
00110 }
00111 return E;
00112 }
00113
00136 dvar_matrix expm(const dvar_matrix& A)
00137 {
00138 RETURN_ARRAYS_INCREMENT();
00139 int rmin = A.rowmin();
00140 int rmax = A.rowmax();
00141
00142 if(rmax != A.colmax())
00143 {
00144 cout<<"Error: Not square matrix in expm."<<endl;
00145 ad_exit(1);
00146 }
00147 if(rmin != A.colmin())
00148 {
00149 cout<<"Error: Not square matrix in expm."<<endl;
00150 ad_exit(1);
00151 }
00152
00153 dvar_matrix I(rmin,rmax,rmin,rmax);
00154 dvar_matrix AA(rmin,rmax,rmin,rmax);
00155 dvar_matrix X(rmin,rmax,rmin,rmax);
00156 dvar_matrix E(rmin,rmax,rmin,rmax);
00157 dvar_matrix D(rmin,rmax,rmin,rmax);
00158 dvar_matrix cX(rmin,rmax,rmin,rmax);
00159
00160 I.initialize();
00161 for(int i = rmin; i<=rmax; ++i){I(i,i) = 1.0;}
00162
00163 dvariable log2NormInf;
00164 log2NormInf = log(max(rowsum(fabs(value(A)))));
00165 log2NormInf/=log(2.0);
00166 int e = (int)value(log2NormInf) + 1;
00167 int s = e+1;
00168 s = (s<0) ? 0 : s;
00169 AA = 1.0/pow(2.0,s)*A;
00170
00171 X = AA;
00172 dvariable c = 0.5;
00173
00174 E = I+c*AA;
00175 D = I-c*AA;
00176 int q = 6, p = 1, k;
00177 for(k = 2; k<=q; ++k){
00178 c*=((double)q-k+1.0)/((double)k*(2*q-k+1));
00179 X = AA*X;
00180 cX = c*X;
00181 E+=cX;
00182 if(p==1){D+=cX;}else{D-=cX;}
00183 p = (p==1) ? 0 : 1;
00184 }
00185
00186 E = solve(D,E);
00187 for(k = 1; k<=s; ++k){
00188 E = E*E;
00189 }
00190 RETURN_ARRAYS_DECREMENT();
00191 return E;
00192 }
00193
00194 dvar_matrix solve(const dvar_matrix& aa, const dvar_matrix& tz,
00195 dvariable ln_unsigned_det, dvariable& sign)
00196 {
00197 RETURN_ARRAYS_INCREMENT();
00198 int n = aa.colsize();
00199 int lb=aa.colmin();
00200 int ub=aa.colmax();
00201 if (lb!=aa.rowmin()||ub!=aa.colmax())
00202 {
00203 cerr << "Error matrix not square in solve()"<<endl;
00204 ad_exit(1);
00205 }
00206 dvar_matrix bb(lb,ub,lb,ub);
00207 bb=aa;
00208 ivector indx(lb,ub);
00209 int One=1;
00210 indx.fill_seqadd(lb,One);
00211 dvariable d;
00212 dvariable big,dum,sum,temp;
00213 dvar_vector vv(lb,ub);
00214
00215 d=1.0;
00216 for (int i=lb;i<=ub;i++)
00217 {
00218 big=0.0;
00219 for (int j=lb;j<=ub;j++)
00220 {
00221 temp=fabs(bb(i,j));
00222 if (temp > big)
00223 {
00224 big=temp;
00225 }
00226 }
00227 if (big == 0.0)
00228 {
00229 cerr << "Error in matrix inverse -- matrix singular in inv(dvar_matrix)\n";
00230 }
00231 vv[i]=1.0/big;
00232 }
00233
00234 for (int j=lb;j<=ub;j++)
00235 {
00236 for (int i=lb;i<j;i++)
00237 {
00238 sum=bb(i,j);
00239 for (int k=lb;k<i;k++)
00240 {
00241 sum -= bb(i,k)*bb(k,j);
00242 }
00243
00244 bb(i,j)=sum;
00245 }
00246 int imax = j;
00247 big=0.0;
00248 for (int i=j;i<=ub;i++)
00249 {
00250 sum=bb(i,j);
00251 for (int k=lb;k<j;k++)
00252 {
00253 sum -= bb(i,k)*bb(k,j);
00254 }
00255 bb(i,j)=sum;
00256 dum=vv[i]*fabs(sum);
00257 if ( dum >= big)
00258 {
00259 big=dum;
00260 imax=i;
00261 }
00262 }
00263 if (j != imax)
00264 {
00265 for (int k=lb;k<=ub;k++)
00266 {
00267 dum=bb(imax,k);
00268 bb(imax,k)=bb(j,k);
00269 bb(j,k)=dum;
00270 }
00271 d = -1.*d;
00272 vv[imax]=vv[j];
00273
00274
00275 {
00276 int itemp=indx(imax);
00277 indx(imax)=indx(j);
00278 indx(j)=itemp;
00279 }
00280
00281 }
00282
00283 if (bb(j,j) == 0.0)
00284 {
00285 bb(j,j)=TINY;
00286 }
00287
00288 if (j != n)
00289 {
00290 dum=1.0/bb(j,j);
00291 for (int i=j+1;i<=ub;i++)
00292 {
00293 bb(i,j) = bb(i,j) * dum;
00294 }
00295 }
00296 }
00297
00298
00299 sign=d;
00300 dvar_vector part_prod(lb,ub);
00301 part_prod(lb)=log(fabs(bb(lb,lb)));
00302 if (bb(lb,lb)<0) sign=-sign;
00303 for (int j=lb+1;j<=ub;j++)
00304 {
00305 if (bb(j,j)<0) sign=-sign;
00306 part_prod(j)=part_prod(j-1)+log(fabs(bb(j,j)));
00307 }
00308 ln_unsigned_det=part_prod(ub);
00309
00310 dvar_matrix z=trans(tz);
00311 int mmin=z.indexmin();
00312 int mmax=z.indexmax();
00313 dvar_matrix x(mmin,mmax,lb,ub);
00314
00315
00316 dvar_vector y(lb,ub);
00317
00318
00319 dvar_matrix& b=bb;
00320 ivector indxinv(lb,ub);
00321 for (int i=lb;i<=ub;i++)
00322 {
00323 indxinv(indx(i))=i;
00324 }
00325 for (int kk=mmin;kk<=mmax;kk++)
00326 {
00327 for (int i=lb;i<=ub;i++)
00328 {
00329 y(indxinv(i))=z(kk)(i);
00330 }
00331
00332 for (int i=lb;i<=ub;i++)
00333 {
00334 sum=y(i);
00335 for (int j=lb;j<=i-1;j++)
00336 {
00337 sum-=b(i,j)*y(j);
00338 }
00339 y(i)=sum;
00340 }
00341 for (int i=ub;i>=lb;i--)
00342 {
00343 sum=y(i);
00344 for (int j=i+1;j<=ub;j++)
00345 {
00346 sum-=b(i,j)*x(kk)(j);
00347 }
00348 x(kk)(i)=sum/b(i,i);
00349 }
00350 }
00351 RETURN_ARRAYS_DECREMENT();
00352 return trans(x);
00353 }
00354 #undef TINY