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 #include "robot.h"
00054 #include <iomanip>
00055
00056 #ifdef use_namespace
00057 namespace ROBOOP {
00058 using namespace NEWMAT;
00059 #endif
00060
00061
00062 static const char rcsid[] __UNUSED__ = "$Id: utils.cpp,v 1.6 2005/07/26 03:22:09 ejt Exp $";
00063
00064
00065 ReturnMatrix x_prod_matrix(const ColumnVector & x)
00066
00067 {
00068 Matrix S(3,3); S = 0.0;
00069 S(1,2) = -x(3); S(1,3) = x(2);
00070 S(2,1) = x(3); S(2,3) = -x(1);
00071 S(3,1) = -x(2); S(3,2) = x(1);
00072
00073 S.Release(); return (S);
00074 }
00075
00076
00077 ReturnMatrix Integ_Trap(const ColumnVector & present, ColumnVector & past,
00078 const Real dt)
00079
00080 {
00081 ColumnVector integration(present.Nrows());
00082 integration = (past+present)*0.5*dt;
00083 past = present;
00084
00085 integration.Release();
00086 return ( integration );
00087 }
00088
00089
00090 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin,
00091 bool & exit, bool & init),
00092 const Matrix & xo, Real to, Real tf, int nsteps)
00093
00094 {
00095 static Real h, h2, t;
00096 static Matrix k1, k2, k3, k4, x;
00097 static bool first_pass = true, init = false, exit = false;
00098
00099 if(first_pass || init)
00100 {
00101 h = (tf-to)/nsteps;
00102 h2 = h/2.0;
00103 t = to;
00104 x = xo;
00105 first_pass = false;
00106 }
00107 k1 = (*xdot)(t, x, exit, init) * h;
00108 if(exit) return;
00109 k2 = (*xdot)(t+h2, x+k1*0.5, exit, init)*h;
00110 if(exit) return;
00111 k3 = (*xdot)(t+h2, x+k2*0.5, exit, init)*h;
00112 if(exit) return;
00113 k4 = (*xdot)(t+h, x+k3, exit, init)*h;
00114 if(exit) return;
00115 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00116 t += h;
00117 }
00118
00119 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(const Real time, const Matrix & xin),
00120 const Matrix & xo, const Real to, const Real tf, const int nsteps)
00121 {
00122 static Real h, h2, t;
00123 static Matrix k1, k2, k3, k4, x;
00124 static bool first_pass = true;
00125
00126 if(first_pass)
00127 {
00128 h = (tf-to)/nsteps;
00129 h2 = h/2.0;
00130 t = to;
00131 x = xo;
00132 first_pass = false;
00133 }
00134 k1 = (*xdot)(t, x) * h;
00135 t += h2;
00136 k2 = (*xdot)(t, x+k1*0.5)*h;
00137 k3 = (*xdot)(t, x+k2*0.5)*h;
00138 t += h2;
00139 k4 = (*xdot)(t, x+k3)*h;
00140 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00141 }
00142
00143
00144 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00145 const Matrix & xo, Real to, Real tf, int nsteps,
00146 RowVector & tout, Matrix & xout)
00147
00148 {
00149 Real h, h2, t;
00150 Matrix k1, k2, k3, k4, x;
00151
00152 h = (tf-to)/nsteps;
00153 h2 = h/2.0;
00154 t = to;
00155 x = xo;
00156 xout = Matrix(xo.Nrows(),(nsteps+1)*xo.Ncols());
00157 xout.SubMatrix(1,xo.Nrows(),1,xo.Ncols()) = x;
00158 tout = RowVector(nsteps+1);
00159 tout(1) = to;
00160 for (int i = 1; i <= nsteps; i++) {
00161 k1 = (*xdot)(t, x) * h;
00162 k2 = (*xdot)(t+h2, x+k1*0.5)*h;
00163 k3 = (*xdot)(t+h2, x+k2*0.5)*h;
00164 k4 = (*xdot)(t+h, x+k3)*h;
00165 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00166 t += h;
00167 tout(i+1) = t;
00168 xout.SubMatrix(1,xo.Nrows(),i*xo.Ncols()+1,(i+1)*xo.Ncols()) = x;
00169 }
00170 }
00171
00172 ReturnMatrix rk4(const Matrix & x, const Matrix & dxdt, Real t, Real h,
00173 ReturnMatrix (*xdot)(Real time, const Matrix & xin))
00174
00175
00176
00177
00178
00179
00180
00181
00182 {
00183 Matrix xt, xout, dxm, dxt;
00184 Real th, hh, h6;
00185
00186 hh = h*0.5;
00187 h6 = h/6.0;
00188 th = t + hh;
00189 xt = x + hh*dxdt;
00190 dxt = (*xdot)(th,xt);
00191 xt = x + hh*dxt;
00192 dxm = (*xdot)(th,xt);
00193 xt = x + h*dxm;
00194 dxm += dxt;
00195 dxt = (*xdot)(t+h,xt);
00196 xout = x+h6*(dxdt+dxt+2.0*dxm);
00197
00198 xout.Release(); return xout;
00199 }
00200
00201 #define PGROW -0.20
00202 #define PSHRNK -0.25
00203 #define FCOR 0.06666666
00204 #define SAFETY 0.9
00205 #define ERRCON 6.0E-4
00206
00207 void rkqc(Matrix & x, Matrix & dxdt, Real & t, Real htry,
00208 Real eps, Matrix & xscal, Real & hdid, Real & hnext,
00209 ReturnMatrix (*xdot)(Real time, const Matrix & xin))
00210
00211
00212
00213
00214
00215
00216
00217
00218 {
00219 Real tsav, hh, h, temp, errmax;
00220 Matrix dxsav, xsav, xtemp;
00221
00222 tsav = t;
00223 xsav = x;
00224 dxsav = dxdt;
00225 h = htry;
00226 for(;;) {
00227 hh = 0.5*h;
00228 xtemp = rk4(xsav,dxsav,tsav,hh,xdot);
00229 t = tsav+hh;
00230 dxdt = (*xdot)(t,xtemp);
00231 x = rk4(xtemp,dxdt,t,hh,xdot);
00232 t = tsav+h;
00233 if(t == tsav) {
00234 cerr << "Step size too small in routine RKQC\n";
00235 exit(1);
00236 }
00237 xtemp = rk4(xsav,dxsav,tsav,h,xdot);
00238 errmax = 0.0;
00239 xtemp = x-xtemp;
00240 for(int i = 1; i <= x.Nrows(); i++) {
00241 temp = fabs(xtemp(i,1)/xscal(i,1));
00242 if(errmax < temp) errmax = temp;
00243 }
00244 errmax /= eps;
00245 if(errmax <= 1.0) {
00246 hdid = h;
00247 hnext = (errmax > ERRCON ?
00248 SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
00249 break;
00250 }
00251 h = SAFETY*h*exp(PSHRNK*log(errmax));
00252 }
00253 x += xtemp*FCOR;
00254 }
00255
00256 #define MAXSTP 10000
00257 #define TINY 1.0e-30
00258
00259 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00260 Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
00261 int & nok, int & nbad,
00262 RowVector & tout, Matrix & xout, Real dtsav)
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272 {
00273 Real tsav, t, hnext, hdid, h;
00274 RowVector tv(1);
00275
00276 Matrix xscal, x, dxdt;
00277
00278 tv = to;
00279 tout = tv;
00280 xout = xo;
00281 xscal = xo;
00282 t = to;
00283 h = (tf > to) ? fabs(h1) : -fabs(h1);
00284 nok = (nbad) = 0;
00285 x = xo;
00286 tsav = t;
00287 for(int nstp = 1; nstp <= MAXSTP; nstp++){
00288 dxdt = (*xdot)(t,x);
00289 for(int i = 1; i <= x.Nrows(); i++)
00290 xscal(i,1) = fabs(x(i,1))+fabs(dxdt(i,1)*h)+TINY;
00291 if((t+h-tf)*(t+h-to) > 0.0) h = tf-t;
00292 rkqc(x,dxdt,t,h,eps,xscal,hdid,hnext,xdot);
00293 if(hdid == h) ++(nok); else ++(nbad);
00294 if((t-tf)*(tf-to) >= 0.0) {
00295 xo = x;
00296 tv = t;
00297 tout = tout | tv;
00298 xout = xout | x;
00299 return;
00300 }
00301 if(fabs(t-tsav) > fabs(dtsav)) {
00302 tv = t;
00303 tout = tout | tv;
00304 xout = xout | x;
00305 tsav = t;
00306 }
00307 if(fabs(hnext) <= hmin) {
00308 cerr << "Step size too small in ODEINT\n";
00309 cerr << setw(7) << setprecision(3) << (tout & xout).t();
00310 exit(1);
00311 }
00312 h = hnext;
00313 }
00314 cerr << "Too many step in routine ODEINT\n";
00315 exit(1);
00316 }
00317
00318 ReturnMatrix sign(const Matrix & x)
00319
00320 {
00321 Matrix out = x;
00322
00323 for(int i = 1; i <= out.Ncols(); i++) {
00324 for(int j = 1; j <= out.Nrows(); j++) {
00325 if(out(j,i) > 0.0) {
00326 out(j,i) = 1.0;
00327 } else if(out(j,i) < 0.0) {
00328 out(j,i) = -1.0;
00329 }
00330 }
00331 }
00332
00333 out.Release(); return out;
00334 }
00335
00336
00337 short sign(const Real x)
00338
00339 {
00340 short i = 1;
00341 if(x > 0.0)
00342 i = 1;
00343 else
00344 i = -1;
00345
00346 return (i);
00347 }
00348
00349 #ifdef use_namespace
00350 }
00351 #endif
00352