00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00059
00060 static const char rcsid[] = "$Id: utils.cpp,v 1.26 2006/05/16 16:11:15 gourdeau Exp $";
00061
00062 #include "utils.h"
00063
00064 #ifdef _MSC_VER
00065 #if _MSC_VER < 1300 // Microsoft
00066 #ifndef GUARD_minmax_H
00067 #define GUARD_minmax_H
00068
00069
00070 template <class T> inline T max(const T& a, const T& b)
00071 {
00072 return (a > b) ? a : b;
00073 }
00074 template <class T> inline T min(const T& a, const T& b)
00075 {
00076 return (a < b) ? a : b;
00077 }
00078 #endif
00079 #endif
00080 #endif
00081
00082 #ifdef use_namespace
00083 namespace ROBOOP {
00084 using namespace NEWMAT;
00085 #endif
00086
00087
00088 ReturnMatrix x_prod_matrix(const ColumnVector & x)
00090 {
00091 Matrix S(3,3); S = 0.0;
00092 S(1,2) = -x(3); S(1,3) = x(2);
00093 S(2,1) = x(3); S(2,3) = -x(1);
00094 S(3,1) = -x(2); S(3,2) = x(1);
00095
00096 S.Release(); return (S);
00097 }
00098
00099 ReturnMatrix pinv(const Matrix & M)
00121 {
00122
00123 const Real eps = numeric_limits<Real>::epsilon();
00124
00125 int m = M.nrows();
00126 int n = M.ncols();
00127
00128 if(n > m)
00129 {
00130 Matrix X = pinv(M.t()).t();
00131 return X;
00132 }
00133
00134 Matrix U, V;
00135
00136 DiagonalMatrix Q;
00137
00138 Matrix X(n, m);
00139
00140 SVD(M, Q, U, V);
00141
00142 Real tol = max(m,n)*Q(1)*eps;
00143
00144
00145 int r = 0;
00146 for(int i = 1; i <= Q.size(); i++)
00147 if(Q(i) > tol)
00148 r++;
00149
00150 if(r != 0)
00151 {
00152 DiagonalMatrix D(r);
00153 for(int i = 1; i <= r; i++)
00154 D(i) = 1/Q(i);
00155
00156
00157
00158 X = V.SubMatrix(1,V.nrows(),1,r)*D*U.SubMatrix(1,U.nrows(),1,r).t();
00159
00160 }
00161 X.Release();
00162 return X;
00163 }
00164
00165 ReturnMatrix Integ_Trap(const ColumnVector & present, ColumnVector & past,
00166 const Real dt)
00168 {
00169 ColumnVector integration(present.Nrows());
00170 integration = (past+present)*0.5*dt;
00171 past = present;
00172
00173 integration.Release();
00174 return ( integration );
00175 }
00176
00177
00178 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin,
00179 bool & exit, bool & init),
00180 const Matrix & xo, Real to, Real tf, int nsteps)
00182 {
00183 static Real h, h2, t;
00184 static Matrix k1, k2, k3, k4, x;
00185 static bool first_pass = true, init = false, exit = false;
00186
00187 if(first_pass || init)
00188 {
00189 h = (tf-to)/nsteps;
00190 h2 = h/2.0;
00191 t = to;
00192 x = xo;
00193 first_pass = false;
00194 }
00195 k1 = (*xdot)(t, x, exit, init) * h;
00196 if(exit) return;
00197 k2 = (*xdot)(t+h2, x+k1*0.5, exit, init)*h;
00198 if(exit) return;
00199 k3 = (*xdot)(t+h2, x+k2*0.5, exit, init)*h;
00200 if(exit) return;
00201 k4 = (*xdot)(t+h, x+k3, exit, init)*h;
00202 if(exit) return;
00203 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00204 t += h;
00205 }
00206
00207 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(const Real time, const Matrix & xin),
00208 const Matrix & xo, const Real to, const Real tf, const int nsteps)
00209 {
00210 static Real h, h2, t;
00211 static Matrix k1, k2, k3, k4, x;
00212 static bool first_pass = true;
00213
00214 if(first_pass)
00215 {
00216 h = (tf-to)/nsteps;
00217 h2 = h/2.0;
00218 t = to;
00219 x = xo;
00220 first_pass = false;
00221 }
00222 k1 = (*xdot)(t, x) * h;
00223 t += h2;
00224 k2 = (*xdot)(t, x+k1*0.5)*h;
00225 k3 = (*xdot)(t, x+k2*0.5)*h;
00226 t += h2;
00227 k4 = (*xdot)(t, x+k3)*h;
00228 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00229 }
00230
00231
00232 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00233 const Matrix & xo, Real to, Real tf, int nsteps,
00234 RowVector & tout, Matrix & xout)
00236 {
00237 Real h, h2, t;
00238 Matrix k1, k2, k3, k4, x;
00239
00240 h = (tf-to)/nsteps;
00241 h2 = h/2.0;
00242 t = to;
00243 x = xo;
00244 xout = Matrix(xo.Nrows(),(nsteps+1)*xo.Ncols());
00245 xout.SubMatrix(1,xo.Nrows(),1,xo.Ncols()) = x;
00246 tout = RowVector(nsteps+1);
00247 tout(1) = to;
00248 for (int i = 1; i <= nsteps; i++) {
00249 k1 = (*xdot)(t, x) * h;
00250 k2 = (*xdot)(t+h2, x+k1*0.5)*h;
00251 k3 = (*xdot)(t+h2, x+k2*0.5)*h;
00252 k4 = (*xdot)(t+h, x+k3)*h;
00253 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00254 t += h;
00255 tout(i+1) = t;
00256 xout.SubMatrix(1,xo.Nrows(),i*xo.Ncols()+1,(i+1)*xo.Ncols()) = x;
00257 }
00258 }
00259
00260 ReturnMatrix rk4(const Matrix & x, const Matrix & dxdt, Real t, Real h,
00261 ReturnMatrix (*xdot)(Real time, const Matrix & xin))
00270 {
00271 Matrix xt, xout, dxm, dxt;
00272 Real th, hh, h6;
00273
00274 hh = h*0.5;
00275 h6 = h/6.0;
00276 th = t + hh;
00277 xt = x + hh*dxdt;
00278 dxt = (*xdot)(th,xt);
00279 xt = x + hh*dxt;
00280 dxm = (*xdot)(th,xt);
00281 xt = x + h*dxm;
00282 dxm += dxt;
00283 dxt = (*xdot)(t+h,xt);
00284 xout = x+h6*(dxdt+dxt+2.0*dxm);
00285
00286 xout.Release(); return xout;
00287 }
00288
00289 #define PGROW -0.20
00290 #define PSHRNK -0.25
00291 #define FCOR 0.06666666
00292 #define SAFETY 0.9
00293 #define ERRCON 6.0E-4
00294
00295 void rkqc(Matrix & x, Matrix & dxdt, Real & t, Real htry,
00296 Real eps, Matrix & xscal, Real & hdid, Real & hnext,
00297 ReturnMatrix (*xdot)(Real time, const Matrix & xin))
00306 {
00307 Real tsav, hh, h, temp, errmax;
00308 Matrix dxsav, xsav, xtemp;
00309
00310 tsav = t;
00311 xsav = x;
00312 dxsav = dxdt;
00313 h = htry;
00314 for(;;) {
00315 hh = 0.5*h;
00316 xtemp = rk4(xsav,dxsav,tsav,hh,xdot);
00317 t = tsav+hh;
00318 dxdt = (*xdot)(t,xtemp);
00319 x = rk4(xtemp,dxdt,t,hh,xdot);
00320 t = tsav+h;
00321 if(t == tsav) {
00322 cerr << "Step size too small in routine RKQC\n";
00323 exit(1);
00324 }
00325 xtemp = rk4(xsav,dxsav,tsav,h,xdot);
00326 errmax = 0.0;
00327 xtemp = x-xtemp;
00328 for(int i = 1; i <= x.Nrows(); i++) {
00329 temp = fabs(xtemp(i,1)/xscal(i,1));
00330 if(errmax < temp) errmax = temp;
00331 }
00332 errmax /= eps;
00333 if(errmax <= 1.0) {
00334 hdid = h;
00335 hnext = (errmax > ERRCON ?
00336 SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
00337 break;
00338 }
00339 h = SAFETY*h*exp(PSHRNK*log(errmax));
00340 }
00341 x += xtemp*FCOR;
00342 }
00343
00344 #define MAXSTP 10000
00345 #define TINY 1.0e-30
00346
00347 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00348 Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
00349 int & nok, int & nbad,
00350 RowVector & tout, Matrix & xout, Real dtsav)
00360 {
00361 Real tsav, t, hnext, hdid, h;
00362 RowVector tv(1);
00363
00364 Matrix xscal, x, dxdt;
00365
00366 tv = to;
00367 tout = tv;
00368 xout = xo;
00369 xscal = xo;
00370 t = to;
00371 h = (tf > to) ? fabs(h1) : -fabs(h1);
00372 nok = (nbad) = 0;
00373 x = xo;
00374 tsav = t;
00375 for(int nstp = 1; nstp <= MAXSTP; nstp++){
00376 dxdt = (*xdot)(t,x);
00377 for(int i = 1; i <= x.Nrows(); i++)
00378 xscal(i,1) = fabs(x(i,1))+fabs(dxdt(i,1)*h)+TINY;
00379 if((t+h-tf)*(t+h-to) > 0.0) h = tf-t;
00380 rkqc(x,dxdt,t,h,eps,xscal,hdid,hnext,xdot);
00381 if(hdid == h) ++(nok); else ++(nbad);
00382 if((t-tf)*(tf-to) >= 0.0) {
00383 xo = x;
00384 tv = t;
00385 tout = tout | tv;
00386 xout = xout | x;
00387 return;
00388 }
00389 if(fabs(t-tsav) > fabs(dtsav)) {
00390 tv = t;
00391 tout = tout | tv;
00392 xout = xout | x;
00393 tsav = t;
00394 }
00395 if(fabs(hnext) <= hmin) {
00396 cerr << "Step size too small in ODEINT\n";
00397 cerr << setw(7) << setprecision(3) << (tout & xout).t();
00398 exit(1);
00399 }
00400 h = hnext;
00401 }
00402 cerr << "Too many step in routine ODEINT\n";
00403 exit(1);
00404 }
00405
00406 ReturnMatrix sign(const Matrix & x)
00408 {
00409 Matrix out = x;
00410
00411 for(int i = 1; i <= out.Ncols(); i++) {
00412 for(int j = 1; j <= out.Nrows(); j++) {
00413 if(out(j,i) > 0.0) {
00414 out(j,i) = 1.0;
00415 } else if(out(j,i) < 0.0) {
00416 out(j,i) = -1.0;
00417 }
00418 }
00419 }
00420
00421 out.Release(); return out;
00422 }
00423
00424
00425 short sign(const Real x)
00427 {
00428 short i = 1;
00429 if(x > 0.0)
00430 i = 1;
00431 else
00432 i = -1;
00433
00434 return (i);
00435 }
00436
00437 #ifdef use_namespace
00438 }
00439 #endif
00440